anisotropictensordistance.h.svn-base

fast marching method

                return true;
            }

            void PseudoInverseMatrix2x2(Matrix2x2 &in) {
                Matrix2x2 M,inT;
                TransposeMatrix2x2(in,inT);
                MultiplyMatrix2x2(inT,in,M);
                InverseMatrix2x2(M);
                MultiplyMatrix2x2(M,inT,in);
            }
        };   // end of the declaration of the class "Hamiltonian". =====================

        // Hamiltonian
        Hamiltonian *hamiltonian;

    public:
        // Constructor
        AnisotropicTensorDistance(T *_data, int _width, int _height, int _depth, T* mask, T *tensor, double *_voro = NULL, T _dx = T(1), T _dy = T(1), T _dz = T(1)) :
            PradosSchemesForFastMarching_3D<T>(_data,_width,_height,_depth,_voro), dx(_dx), dy(_dy), dz(_dz) {

            // Allocation and initialization of the Hamiltonian from the tensor field
            hamiltonian = new Hamiltonian[this->size];
            for (int x=0;x<this->width;x++) for (int y=0;y<this->height;y++) for(int z=0;z<this->depth;z++) {
                const int n = this->_offset(x,y,z);
                if (mask[n]) {
                    Matrix3x3 B = { { tensor[n], tensor[n+this->size], tensor[n+2*this->size] },
                                    { tensor[n+this->size], tensor[n+3*this->size], tensor[n+4*this->size] },
                                    { tensor[n+2*this->size], tensor[n+4*this->size], tensor[n+5*this->size] }};
                                    if (!hamiltonian[n].InitFromB(B)) {
                                        std::cerr << "Null tensor was found at ("<< x << ", " << y << ", " << z << "), discarding from mask..." << std::endl;
                                        mask[n] = 0;
                                        this->AddForbiddenPoint(x,y,z);
                                    }
                } else this->AddForbiddenPoint(x,y,z);
            }
        }


        // Destructor
        virtual ~AnisotropicTensorDistance() {
            delete [] hamiltonian;
        }

        virtual void setOptDynamics(int x, int y, int z, T component1, T component2, T component3) const {
            const int n = this->_offset(x,y,z);
            const Matrix3x3 &iB = hamiltonian[n].invB;
            const T &norm = std::sqrt(iB[0][0]*component1*component1 + iB[1][1]*component2*component2 + iB[2][2]*component3*component3 + 2*iB[0][1]*component1*component2 + 2*iB[0][2]*component1*component3 + 2*iB[1][2]*component2*component3);
            this->OptDynamics[n][0] = component1/norm;
            this->OptDynamics[n][1] = component2/norm;
            this->OptDynamics[n][2] = component3/norm;
        }

    protected:
        virtual bool eqSolverOnPart_with_s1s2s3_nonNull(
            const T U1, const T U2, const T U3,             // Values of the solution at the considered neigborhood voxels,
            const int s1, const int s2, const int s3,       // signs associated to the considered sector,
            const int x, const int y, const int z,          // coordinates of the considered voxel,
            T &Root,                                        // solution.
            T &optDymamics1,    T &optDymamics2,    T &optDymamics3 // optimal dynamic associated to the solution.
            ) const {

            if (U1==this->big || U2==this->big || U3==this->big) return false; // of course there is no such solution :-)

            // Solving of the equation
            // p_t^T B p_t - 1 = 0

            T t1=this->big, t2=this->big;

            const Matrix3x3 &B = hamiltonian[this->_offset(x,y,z)].B;

            if (basicAnisoEikonalEq_3D(
                B[0][0], B[0][1], B[0][2],
                         B[1][1], B[1][2],
                                  B[2][2],
                U1,U2,U3,
                s1*dx,s2*dy,s3*dz,
                t1, t2)) {

                // test of p_t1:            ($[p_t]_i = [ t-u(x+sihiei)] / (-sihi)$.)
                T  // description of p_t1
                    p_t1_1 =  ( t1-U1 ) / (-s1*dx),
                    p_t1_2 =  ( t1-U2 ) / (-s2*dy),
                    p_t1_3 =  ( t1-U3 ) / (-s3*dz);

                // with this equation, the optimal dymamics $f(x,a_p)$ associated to a vector $p$  is $B*p$.
                // So the optimal dynamics associated to $t1$ is $B*p_t1$:
                optDymamics1 =
                    // Bp_t1_1 =
                                B[0][0]*p_t1_1 + B[0][1]*p_t1_2 + B[0][2]*p_t1_3;
                optDymamics2 =
                    // Bp_t1_2 =
                                B[0][1]*p_t1_1 + B[1][1]*p_t1_2 + B[1][2]*p_t1_3;
                optDymamics3 =
                    // Bp_t1_3 =
                                B[0][2]*p_t1_1 + B[1][2]*p_t1_2 + B[2][2]*p_t1_3;

                // if for all i, [B*p_t1]_i < 0 then t1 is the solution
                if ( (optDymamics1*s1 < 0) && (optDymamics2*s2 < 0) && (optDymamics3*s3 < 0) ) {
                    Root = t1;
                    return true;
                }

                // test of p_t2:
                T
                    p_t2_1 =  ( t2-U1 ) / (-s1*dx),
                    p_t2_2 =  ( t2-U2 ) / (-s2*dy),
                    p_t2_3 =  ( t2-U3 ) / (-s3*dz);

                // The optimal dynamics associated to $t2$ is $B*p_t2$:
                optDymamics1 =
                    //  Bp_t2_1 =
                                B[0][0]*p_t2_1 + B[0][1]*p_t2_2 + B[0][2]*p_t2_3;
                optDymamics2 =
                    //  Bp_t2_2 =
                                B[0][1]*p_t2_1 + B[1][1]*p_t2_2 + B[1][2]*p_t2_3;
                optDymamics3 =
                    //  Bp_t2_3 =
                                B[0][2]*p_t2_1 + B[1][2]*p_t2_2 + B[2][2]*p_t2_3;

                // if for all i, [B*p_t2]_i < 0 then t2 is the solution
                if ( (optDymamics1*s1 < 0) && (optDymamics2*s2 < 0) && (optDymamics3*s3 < 0) ) {
                    Root = t2;
                    return true;
                }
            }

            // There is no solution
            return  false;
        }


        virtual bool eqSolverOnPart_withOne_si_Null(
            const T U1, const T U2, const T U3,
            const int s1, const int s2, const int s3,
            const int x, const int y, const int z,
            const int indice_si_EqualZero,
            T &Root,
            T &optDymamics1,    T &optDymamics2,    T &optDymamics3
            ) const {

            const int n = this->_offset(x,y,z);

            if (indice_si_EqualZero==1) {
                if (U2==this->big || U3==this->big) return false; // of course there is no such solution :-)
                const Matrix2x2 &invinvB = hamiltonian[n].invinvB1;
                return solveInDim2(invinvB[0][0], invinvB[0][1], invinvB[1][1], U2, U3, s2, s3, dy, dz, Root,optDymamics2,optDymamics3);
            }

            if (indice_si_EqualZero==2) {
                if (U1==this->big || U3==this->big) return false; // of course there is no such solution :-)
                const Matrix2x2 &invinvB = hamiltonian[n].invinvB2;
                return solveInDim2(invinvB[0][0], invinvB[0][1], invinvB[1][1], U1, U3, s1, s3, dx, dz, Root,optDymamics1,optDymamics3);
            }

            if (indice_si_EqualZero==3) {
                if (U1==this->big || U2==this->big) return false; // of course there is no such solution :-)
                const Matrix2x2 &invinvB = hamiltonian[n].invinvB3;
                return solveInDim2( invinvB[0][0], invinvB[0][1], invinvB[1][1], U1, U2, s1, s2, dx, dy, Root,optDymamics1,optDymamics2);
            }

            // In any case, indice_si_EqualZero must be equal to 1, 2 or 3 !!!!!
            assert(false);
            return  false;
        }


        virtual bool eqSolverOnPart_withTwo_si_Null(
            const T U1, const T U2, const T U3,
            const int s1, const int s2, const int s3,
            const int x, const int y, const int z,
            const int indice_si_DiffZero,
            T &Root,
            T &optDymamics1,    T &optDymamics2,    T &optDymamics3
            ) const {

            const int n = this->_offset(x,y,z);

            if (indice_si_DiffZero==1) {
                if (U1==this->big) return false; // of course there is no such solution :-)
                return solveInDim1(hamiltonian[n].invsqinvA1,U1,s1,dx,Root,optDymamics1);
            }

            if (indice_si_DiffZero==2) {
                if (U2==this->big) return false; // of course there is no such solution :-)
                return solveInDim1(hamiltonian[n].invsqinvA2,U2,s2,dy,Root,optDymamics2);
            }

            if (indice_si_DiffZero==3) {
                if (U3==this->big) return false; // of course there is no such solution :-)
                return solveInDim1(hamiltonian[n].invsqinvA3,U3,s3,dz,Root,optDymamics3);
            }

            // In any case, indice_si_DiffZero must be equal to 1, 2 or 3 !!!!!
            assert(false);
            return  false;
        }


        bool solveInDim2(
            const T C00, const T C01,
                         const T C11,
            const T U1, const T U2,
            const int s1, const int s2,
            const T dx1, const T dx2,
            T &root,
            T &optDymamics1,    T &optDymamics2) const {

            // Solving of the equation
            // p_t^T C p_t - 1 = 0

            T t1=this->big, t2=this->big;

            if (basicAnisoEikonalEq_2D(C00, C01, C11, U1, U2, s1*dx1, s2*dx2, t1, t2)) {

                // test of p_t1:            ($[p_t]_i = [ t-u(x+sihiei)] / (-sihi)$.)
                T  // description of p_t1
                    p_t1_1 =  ( t1-U1 ) / (-s1*dx1),
                    p_t1_2 =  ( t1-U2 ) / (-s2*dx2);

                // The optimal dynamics associated to $t1$ is $C*p_t1$:
                optDymamics1 = // Cp_t1_1 =
                                            C00*p_t1_1 + C01*p_t1_2;
                optDymamics2 = // Cp_t1_2 =
                                            C01*p_t1_1 + C11*p_t1_2;

                // if for all i, [C*p_t1]_i < 0 then t1 is the solution
                if ( (optDymamics1*s1 < 0) && (optDymamics2*s2 < 0) ) {
                    root = t1;
                    return true;
                }

                // test of p_t2:
                T   p_t2_1 =  ( t2-U1 ) / (-s1*dx1),
                    p_t2_2 =  ( t2-U2 ) / (-s2*dx2);

                // The optimal dynamics associated to $t2$ is $C*p_t2$:
                optDymamics1 = // Cp_t2_1 =
                                            C00*p_t2_1 + C01*p_t2_2;
                optDymamics2 = // Cp_t2_2 =
                                            C01*p_t2_1 + C11*p_t2_2;

                // if for all i, [B*p_t2]_i < 0 then t2 is the solution
                if ( (optDymamics1*s1 < 0) && (optDymamics2*s2 < 0) ) {
                    root = t2;
                    return true;
                }
            }

            // There is no solution
            return  false;
        }


        bool solveInDim1(
            const T C00,
            const T U1,
            const int s1,
            const T dx1,
            T &root,
            T &optDymamics1) const {

            // The 1D case is particularly simple
            if (C00>0) {
                T sqrtC00 = (T)std::sqrt(C00);
                root = U1 + dx1 / sqrtC00;
                optDymamics1 = -s1 * sqrtC00;
                return true;
            }

            return false;
        }


        //////////////////////////////////////////////////////////////////////////
        // Inversion of the eikonal equation in a given simplex

        bool basicAnisoEikonalEq_2D(
            T c11, T c12,               // Matrix C
                T c22,
            T u1, T u2,                 // Some values of U
            T dx1, T dx2,               // Signed mesh size
            T &sol_max,  T &sol_min) const {

            const T dx1_2 = dx1*dx1;
            const T dx2_2 = dx2*dx2;

            const T d1 = dx2_2*c11;
            const T d2 = dx1_2*c22;
            const T d12 = 2*dx1*dx2*c12;

            const T a = d1 + d2 + d12;
            const T b = - T(2)*d1*u1 - T(2)*d2*u2 - d12*(u1+u2);
            const T c = d1*u1*u1 + d2*u2*u2 + d12*u1*u2 - dx1_2*dx2_2;

            return this->_SolveTrinome(a,b,c,sol_max,sol_min);
        }


        bool basicAnisoEikonalEq_3D(
            T c11, T c12, T c13,        // Matrix C
                T c22, T c23,
                        T c33,
            T u1, T u2, T u3,           // Some values of U
            T dx1, T dx2, T dx3,        // Signed mesh size
            T &sol_max,  T &sol_min) const {

            const T dx1_2 = dx1*dx1;
            const T dx2_2 = dx2*dx2;
            const T dx3_2 = dx3*dx3;

            const T d1 = dx2_2*dx3_2*c11;
            const T d2 = dx1_2*dx3_2*c22;
            const T d3 = dx1_2*dx2_2*c33;

            const T d12 = 2*dx1*dx2*dx3_2*c12;
            const T d13 = 2*dx1*dx2_2*dx3*c13;
            const T d23 = 2*dx1_2*dx2*dx3*c23;

            const T a = d1 + d2 + d3 + d12 + d13 + d23;
            const T b = - 2*d1*u1 - 2*d2*u2 - 2*d3*u3 - d12*(u1+u2) - d13*(u1+u3) - d23*(u2+u3);
            const T c = d1*u1*u1 + d2*u2*u2 + d3*u3*u3 + d12*u1*u2 + d13*u1*u3 + d23*u2*u3 - dx1_2*dx2_2*dx3_2;

            return this->_SolveTrinome(a,b,c,sol_max,sol_min);
        }

    }; // End of the class AnisotropicTensorDistance.

} // End of the namespace "FastLevelSet".

#endif