/*------------------------------------------------------------------------------------------------------    File        : AnisotropicTensorDistance.h     (GCM Library)  Description : This class specifies the virtual methods of the class "GenericPradosSchemesForFastMarching_3D" for the special case of the 3D anisotropic Eikonal equation.  Authors      : Emmanuel Prados (UCLA/INRIA), Christophe Lenglet (INRIA), Jean-Philippe Pons (INRIA)    --------------  License     : This software is governed by the CeCILL-C license under French law and abiding by the   rules of distribution of free software.   Users can use, modify and/ or redistribute the software under the terms of the CeCILL-C. In particular,   the exercising of this right is conditional upon the obligation to make available to the community the   modifications made to the source code of the software so as to contribute to its evolution (e.g. by the   mean of the web; i.e. by publishing a web page!).    In this respect, the risks associated with loading, using, modifying and/or developing or reproducing   the software by the user are brought to the user's attention, given its Free Software status, which may   make it complicated to use, with the result that its use is reserved for developers and experienced   professionals having in-depth computer knowledge. Users are therefore encouraged to load and test the   suitability of the software as regards their requirements in conditions enabling the security of their  systems and/or data to be ensured and, more generally, to use and operate it in the same conditions of   security. This Agreement may be freely reproduced and published, provided it is not altered, and that   no provisions are either added or removed herefrom.     CeCILL-C FREE SOFTWARE LICENSE AGREEMENT is available in the file                           Licence_CeCILL-C_V1-en.txt   or at                            http://www.cecill.info/index.en.html.  This Agreement may apply to any or all software for which the holder of the economic rights decides to   submit the use thereof to its provisions.    --------------  Associated publications  : This C++ code corresponds to the implementation of the algorithm presented   in the following articles:    - E. Prados, C. Lenglet, J.P. Pons, N. Wotawa, R. Deriche, O. Faugeras, S. Soatto;     "Control Theory and Fast Marching Methods for Brain Connectivity Mapping";     INRIA Research Report 5845 -- UCLA Computer Science Department Technical Report 060004, February 2006.  - E. Prados, C. Lenglet, J.P. Pons, N. Wotawa, R. Deriche, O. Faugeras, S. Soatto;     "Control Theory and Fast Marching Methods for Brain Connectivity Mapping";    Proc. IEEE Computer Society Conference on Computer Vision and Pattern Recognition, New York, NY, I: 1076-1083, June 17-22, 2006.    - For more references, we refer to the official web page of the GCM Library and to authors' web pages.  Please, if you use the GCM library in you work, make sure you will include the reference to the work of   the authors in your publications.  --------------    Technical Comments and Detailled description:  - This class specifies the virtual methods of the class "GenericPradosSchemesForFastMarching_3D"   for the special case of the 3D anisotropic Eikonal equation.  The functions we overcharge are:   * virtual bool eqSolverOnPart_with_s1s2s3_nonNul(...)   * virtual bool eqSolverOnPart_withOne_si_Nul(...)   * virtual bool eqSolverOnPart_withTwo_si_Nul(...)  $(s_1,s_2,s_3)$ being fixed,  these methods allow to solve the equations     $$ sup_{a in B} g_s(a,t) = 0 $$  where $g_s(a,t) = - f(x; a).P_{x,s,U}(t)-l(x; a)$   and where, according to the different cases,   * $B$ is the interior set of the $As$   (eqSolverOnPart_with_s1s2s3_nonNu)  * $B$ is the union of the 2D facets of $As$  (eqSolverOnPart_withOne_si_Nul)  * $B$ is the union of the 1D edges of $As$  (eqSolverOnPart_withTwo_si_Nul)  See section 4.3.2 of INRIA RR-5845 (page 11).  Let us note that these methods compute the $t_s$ and the associated dynamics.  - The goal of the class being to specify the virtual methods of the class    "GenericPradosSchemesForFastMarching_3D" for the special case of the 3D anisotropic Eikonal equation,  we then need to describe the data defining the Hamiltonian.  In this class, we then need to declare and construct all the associated structures.  - Note about the optimal dynamics:  The optimal dymamics (so its preservation and its transmission in the paramters) ARE NOT USEFULL IF we  only want to compute the viscosity solution of the considered equation by the Fast Marching Method.   Nevertheless the computation of the optimal dymamics is necessary in order to be able to chose the good   simplex. The Preservation and the transmission of the optimal Dymamics can be usefull in many applications   as for example fibers tracking in DTI (see INRIA RR-5845).  ----------------------------------------------------------------------------------------------------*/#ifndef ANISOTROPICTENSORDISTANCE_H#define ANISOTROPICTENSORDISTANCE_H#include <iostream>#include <cmath>#include <cassert>#include "GenericPradosSchemesForFastMarching_3D.h"/**************************************************/////  This class (AnisotropicTensorDistance) inherits of//  the class: "PradosSchemesForFastMarching_3D"////  Here we have to overcharge the functions//   - virtual bool eqSolverOnPart_with_s1s2s3_nonNul(...)//   - virtual bool eqSolverOnPart_withOne_si_Nul(...)//   - virtual bool eqSolverOnPart_withTwo_si_Nul(...)///**************************************************/namespace FastLevelSet {/**************************************************  The class AnisotropicTensorDistance inherits of  the class: "PradosSchemesForFastMarching_3D"**************************************************/    template <typename T = float>    class AnisotropicTensorDistance : public PradosSchemesForFastMarching_3D<T> {        /***************************************************************           Let us declare all the structures describing the data           charaterizing the 3D Eikonal equation considered.           We also declare some variables required to describe the scheme.        ****************************************************************/    protected:    T dx,dy,dz;                               // voxel sizes        typedef T Vector3[3];        typedef T Matrix3x3[3][3];        typedef T Matrix2x2[2][2];        // we start with the declaration of the class "Hamiltonian".          /***************************************************************            The Hamiltonian that we have implemented here is                           $  |A_x p |^2 - 1  $.            The Hamiltonian is equivalent to the Hamiltonian                     $  H_{AEik}(x,p) = |A_x p | - 1 $            given at page 7 of the  INRIA report RR-5845.            The $ H_i(x,p) $  and $ H_{ij}(x,p) $ described at page 12 (section 4.3.3) of the INRIA report RR-5845            are, in fact, the ones associated to $|A_x p |^2 - 1$.            There are no differences, except that we need to renormalize the optimal dynamics.             We will explain better this point in our forthcomming journal paper...          ****************************************************************/        class Hamiltonian {        public:            Matrix3x3 B, invB;            Matrix2x2 invinvB1, invinvB2, invinvB3;            T invsqinvA1, invsqinvA2, invsqinvA3;            // Initialization from B            bool InitFromB(const Matrix3x3 &_B) {                // Storage of B                CopyMatrix3x3(_B,B);                // Computation of the inverse of B//                 if (!InverseSymmetricMatrix3x3(B,invB)) return false;                if (Matrix3x3IsZero(B)) return false;                else PseudoInverseSymmetricMatrix3x3(B,invB);                // Computation of invinvB_i: the inverse of invB after removing the i^th row and i^th column                for (int i=0;i<2;i++) for (int j=0;j<2;j++) {                    invinvB1[i][j] = invB[i+1][j+1];                    invinvB2[i][j] = invB[2*i][2*j];                    invinvB3[i][j] = invB[i][j];                }                if (Matrix2x2IsZero(invinvB1)) return false;                else PseudoInverseMatrix2x2(invinvB1);                if (Matrix2x2IsZero(invinvB2)) return false;                else PseudoInverseMatrix2x2(invinvB2);                if (Matrix2x2IsZero(invinvB3)) return false;                else PseudoInverseMatrix2x2(invinvB3);//                 if (!InverseMatrix2x2(invinvB1) || !InverseMatrix2x2(invinvB2) || !InverseMatrix2x2(invinvB3)) return false;                // Computation of invinvAi: the inverse of the square norm of the i^th row of invA                invsqinvA1 = 1/invB[0][0];                invsqinvA2 = 1/invB[1][1];                invsqinvA3 = 1/invB[2][2];                return true;            }            // Initialization from A            bool InitFromA(const Matrix3x3 &_A) {                // TODO                return false;            }        private:            // Matrix operations            void DisplayMatrix3x3(const Matrix3x3 &M) {                for (int i=0;i<3;i++)                    std::cout << M[i][0] << " " << M[i][1] << " " << M[i][2] << std::endl;                std::cout << std::endl;            }            void DisplayMatrix2x2(const Matrix2x2 &M) {                for (int i=0;i<2;i++)                    std::cout << M[i][0] << " " << M[i][1]  << std::endl;                std::cout << std::endl;            }            void MultiplyMatrix3x3(const Matrix3x3 &A, const Matrix3x3 &B, Matrix3x3 &R) {                for (int i=0; i<3; ++i) {                    for (int j=0; j<3; ++j) {                        R[i][j] = 0.0;                        for (int k=0; k<3; ++k) R[i][j] += A[i][k] * B[k][j];                    }                }            }            void MultiplyMatrix2x2(const Matrix2x2 &A, const Matrix2x2 &B, Matrix2x2 &R) {                for (int i=0; i<2; ++i) {                    for (int j=0; j<2; ++j) {                        R[i][j] = 0.0;                        for (int k=0; k<2; ++k) R[i][j] += A[i][k] * B[k][j];                    }                }            }            void CopyMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {                for (int i=0;i<3;i++) for (int j=0;j<3;j++) out[i][j] = in[i][j];            }            void CopyMatrix2x2(const Matrix2x2 &in, Matrix2x2 &out) {                for (int i=0;i<2;i++) for (int j=0;j<2;j++) out[i][j] = in[i][j];            }            void TransposeMatrix2x2(const Matrix2x2 &in, Matrix2x2 &out) {                for (int i=0;i<2;i++) for (int j=0;j<2;j++) out[i][j] = in[j][i];            }            void TransposeMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {                for (int i=0;i<3;i++) for (int j=0;j<3;j++) out[i][j] = in[j][i];            }            bool Matrix2x2IsZero(const Matrix2x2 &M) {                return (M[0][0]==0 && M[0][1]==0 && M[1][1]==0);            }            bool Matrix3x3IsZero(const Matrix3x3 &M) {                return (M[0][0]==0 && M[0][1]==0 && M[0][2]==0 && M[1][1]==0 && M[1][2]==0 && M[2][2]==0);            }            bool InverseUpperTriangularMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {                if (in[0][0] == 0 || in[1][1] == 0 || in[2][2] == 0) return false;                out[0][0] = 1/in[0][0];                out[0][1] = -in[0][1]/in[0][0]/in[1][1];                out[0][2] = (in[0][1]*in[1][2] - in[0][2]*in[1][1])/in[0][0]/in[1][1]/in[2][2];                out[1][0] = 0;                out[1][1] = 1/in[1][1];                out[1][2] = -in[1][2]/in[1][1]/in[2][2];                out[2][0] = 0;                out[2][1] = 0;                out[2][2] = 1/in[2][2];                return true;            }            bool InverseSymmetricMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {                // for computing the determinant,                // we develop with respect to the first row:                T firstCrossProduct  = in[1][1]*in[2][2]-in[1][2]*in[1][2];                T secondCrossProduct = in[0][1]*in[2][2]-in[0][2]*in[1][2];                T thirdCrossProduct  = in[0][1]*in[1][2]-in[0][2]*in[1][1];                T determinant =                        in[0][0]*firstCrossProduct                    -   in[0][1]*secondCrossProduct                    +   in[0][2]*thirdCrossProduct;                if (determinant == 0) return false;                out[0][0] =  (  firstCrossProduct   )/determinant;                out[0][1] =  ( - secondCrossProduct )/determinant;                out[0][2] =  (  thirdCrossProduct   )/determinant;                out[1][0] = out[0][1];                out[1][1] =  (-in[0][2]*in[0][2]+in[0][0]*in[2][2])/determinant;                out[1][2] =  (-in[0][0]*in[1][2]+in[0][2]*in[0][1])/determinant;                out[2][0] = out[0][2];                out[2][1] = out[1][2];                out[2][2] =  (-in[0][1]*in[0][1]+in[0][0]*in[1][1])/determinant;                return true;            }            void PseudoInverseSymmetricMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {                Matrix3x3 M,N;                MultiplyMatrix3x3(in,in,M);                InverseSymmetricMatrix3x3(M,N);                MultiplyMatrix3x3(N,in,out);            }            //bool CholeskyMatrix3x3(const Matrix3x3 &in, Matrix3x3 &out) {            //  if (in[0][0]<0) return false;            //  out[0][0] = (T)std::sqrt(in[0][0]);            //  out[0][1] = in[0][1] / out[0][0];            //  out[0][2] = in[0][2] / out[0][0];            //  out[1][0] = 0;            //  const T d2 = in[1][1] - out[0][1]*out[0][1];            //  if (d2<0) return false;            //  out[1][1] = (T)std::sqrt(d2);            //  out[1][2] = ( in[1][2] - out[0][1]*out[0][2] ) / out[1][1];            //  out[2][0] = 0;            //  out[2][1] = 0;            //  const T d3 = in[2][2] - out[0][2]*out[0][2] - out[1][2]*out[1][2];            //  if (d3<0) return false;            //  out[2][2] = (T)std::sqrt(d3);            //  return true;            //}            bool InverseMatrix2x2(Matrix2x2 &M) {                const T det = M[0][0]*M[1][1]-M[0][1]*M[1][0];                if (det==0) return false;                std::swap(M[0][0],M[1][1]);                M[0][0] /= det;                M[1][1] /= det;                M[0][1] /= -det;                M[1][0] /= -det;