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