[HyperElastic] Clean up the ElasticityTensor

Author: th-skam

Sofa/Component/SolidMechanics/FEM/HyperElastic/src/sofa/component/solidmechanics/fem/hyperelastic/material/Ogden.h

@@ -48,6 +48,20 @@ the determinant of the deformation gradient J and the right Cauchy Green deforma
 template<class DataTypes>
 class Ogden: public HyperelasticMaterial<DataTypes>
 {
+private:
+    // In SOFA Voigt order for 3D symmetric tensors is non-standard: xx, xy, yy, xz, yz, zz
+    const std::array<sofa::Index, 3*3> toVoigt = {0, 1, 3, 1, 2, 4, 3, 4, 5};
+    inline sofa::Index vId(sofa::Index i, sofa::Index j) {return toVoigt[i * 3 + j];}
+    const std::array<std::tuple<sofa::Index, sofa::Index>, 6> fromVoigt =
+    {
+        std::make_tuple(0,0),
+        std::make_tuple(0,1),
+        std::make_tuple(1,1),
+        std::make_tuple(0,2),
+        std::make_tuple(1,2),
+        std::make_tuple(2,2)
+    };
+
 public:
     static constexpr std::string_view Name = "Ogden";
 
@@ -70,8 +84,8 @@ class Ogden: public HyperelasticMaterial<DataTypes>
 
         // Solve eigen problem for C
         Eigen::Matrix<Real, 3, 3> CEigen;
-        CEigen(0,0)=C[0]; CEigen(0,1)=C[1]; CEigen(1,0)=C[1]; CEigen(1,1)=C[2];
-        CEigen(1,2)=C[4]; CEigen(2,1)=C[4]; CEigen(2,0)=C[3]; CEigen(0,2)=C[3]; CEigen(2,2)=C[5];
+        for (sofa::Index i = 0; i < 3; ++i)
+            for (sofa::Index j = 0; j < 3; ++j) CEigen(i, j) = C[vId(i, j)];
 
         // Disable temporarilly until fixed /*Eigen::SelfAdjointEigenSolver<EigenMatrix>*/
         Eigen::EigenSolver<Eigen::Matrix<Real, 3, 3> > EigenProblemSolver(CEigen, true);
@@ -103,9 +117,9 @@ class Ogden: public HyperelasticMaterial<DataTypes>
         const Real fj = pow(sinfo->J, -alpha1/3.0_sreal);
 
         // Solve eigen problem for C
-        Eigen::Matrix<Real,3,3> CEigen;
-        CEigen(0,0)=C[0]; CEigen(0,1)=C[1]; CEigen(1,0)=C[1]; CEigen(1,1)=C[2]; CEigen(1,2)=C[4]; CEigen(2,1)=C[4];
-        CEigen(2,0)=C[3]; CEigen(0,2)=C[3]; CEigen(2,2)=C[5];
+        Eigen::Matrix<Real, 3, 3> CEigen;
+        for (sofa::Index i = 0; i < 3; ++i)
+            for (sofa::Index j = 0; j < 3; ++j) CEigen(i, j) = C[vId(i, j)];
 
         // Disable temporarilly until fixed /*Eigen::SelfAdjointEigenSolver<EigenMatrix>*/
         Eigen::EigenSolver<Eigen::Matrix<Real, 3, 3> > EigenProblemSolver(CEigen, true);
@@ -140,7 +154,6 @@ class Ogden: public HyperelasticMaterial<DataTypes>
         SPKTensorGeneral = fj * mu1 / alpha1 * (CaBy2Minus1 + -1_sreal/3_sreal * trCaBy2 * invC) + k0*log(sinfo->J)*invC;
     }
 
-
     void applyElasticityTensor_old(StrainInformation<DataTypes> *sinfo, const MaterialParameters<DataTypes> &param,
                                const MatrixSym& inputTensor, MatrixSym& outputTensor)
     {
@@ -272,139 +285,130 @@ class Ogden: public HyperelasticMaterial<DataTypes>
 
     void ElasticityTensor(StrainInformation<DataTypes> *sinfo, const MaterialParameters<DataTypes> &param, Matrix6& outputTensor) override
     {
-        Real k0=param.parameterArray[0];
-        Real mu1=param.parameterArray[1];
-        Real alpha1=param.parameterArray[2];
-        MatrixSym C=sinfo->deformationTensor;
-        Eigen::Matrix<Real,3,3> CEigen;
-        CEigen(0,0)=C[0]; CEigen(0,1)=C[1]; CEigen(1,0)=C[1]; CEigen(1,1)=C[2]; CEigen(1,2)=C[4]; CEigen(2,1)=C[4];
-        CEigen(2,0)=C[3]; CEigen(0,2)=C[3]; CEigen(2,2)=C[5];
+        const MatrixSym& C = sinfo->deformationTensor;
+        const Real k0 = param.parameterArray[0];
+        const Real mu1 = param.parameterArray[1];
+        const Real alpha1 = param.parameterArray[2];
+        const Real fj = pow(sinfo->J, -alpha1/3.0_sreal);
 
-        /*Eigen::SelfAdjointEigenSolver<EigenMatrix>*/Eigen::EigenSolver<Eigen::Matrix<Real, 3, 3> > EigenProblemSolver(CEigen,true);
+        // Solve eigen problem for C
+        Eigen::Matrix<Real, 3, 3> CEigen;
+        for (sofa::Index i = 0; i < 3; ++i)
+            for (sofa::Index j = 0; j < 3; ++j) CEigen(i, j) = C[vId(i, j)];
+
+        // Disable temporarilly until fixed /*Eigen::SelfAdjointEigenSolver<EigenMatrix>*/
+        Eigen::EigenSolver<Eigen::Matrix<Real, 3, 3> > EigenProblemSolver(CEigen, true);
         if (EigenProblemSolver.info() != Eigen::Success)
         {
             dmsg_warning("Ogden") << "SelfAdjointEigenSolver iterations failed to converge";
             return;
         }
-        EigenMatrix Evect=EigenProblemSolver.eigenvectors().real();
-        CoordEigen Evalue=EigenProblemSolver.eigenvalues().real();
+        const EigenMatrix Evect = EigenProblemSolver.eigenvectors().real();
+        const CoordEigen Evalue = EigenProblemSolver.eigenvalues().real();
 
-        Real trCalpha=pow(Evalue[0],alpha1/(Real)2)+pow(Evalue[1],alpha1/(Real)2)+pow(Evalue[2],alpha1/(Real)2);
-        Matrix3 Pinverse;
-        Pinverse(0,0)=Evect(0,0); Pinverse(1,1)=Evect(1,1); Pinverse(2,2)=Evect(2,2); Pinverse(0,1)=Evect(1,0); Pinverse(1,0)=Evect(0,1); Pinverse(2,0)=Evect(0,2);
-        Pinverse(0,2)=Evect(2,0); Pinverse(2,1)=Evect(1,2); Pinverse(1,2)=Evect(2,1);
+        // trace of C^(alpha1/2)
+        const Real aBy2 = alpha1*0.5_sreal;
+        const Real trCaBy2 = pow(Evalue[0], aBy2) + pow(Evalue[1], aBy2) + pow(Evalue[2], aBy2);
 
-        MatrixSym Dalpha_1(
-            pow(Evalue[0], alpha1/2.0 - 1.0), 0,
-            pow(Evalue[1], alpha1/2.0 - 1.0), 0, 0,
-            pow(Evalue[2], alpha1/2.0 - 1.0)
-        );
-        MatrixSym Calpha_1;
-        Calpha_1.Mat2Sym(Pinverse.transposed()*Dalpha_1.SymMatMultiply(Pinverse),Calpha_1);
-        
-        MatrixSym Dalpha_2(
-            pow(Evalue[0], alpha1/2.0 - 2.0), 0,
-            pow(Evalue[1], alpha1/2.0 - 2.0), 0, 0,
-            pow(Evalue[2], alpha1/2.0 - 2.0)
-        );
-        MatrixSym Calpha_2; 
-        Calpha_2.Mat2Sym(Pinverse.transposed() * Dalpha_2.SymMatMultiply(Pinverse), Calpha_2);
+        // Transpose (also inverse) of the eigenvector matrix
+        Matrix3 EigenBasis;
+        for (auto m = 0; m < Evect.rows(); ++m)
+            for (auto n = 0; n < Evect.cols(); ++n) EigenBasis(m, n) = Evect(m, n);
 
-        MatrixSym inversematrix;
-        invertMatrix(inversematrix,sinfo->deformationTensor);
+        // Construct C^(alpha1/2 - 1) from eigenbasis: V * D * V^T; D_i = lambda_i^(alpha1/2 - 1)
+        const Real aBy2Minus1 = aBy2 - 1_sreal;
+        MatrixSym D(pow(Evalue[0], aBy2Minus1), 0, pow(Evalue[1], aBy2Minus1), 0, 0, pow(Evalue[2], aBy2Minus1));
+        MatrixSym CaBy2Minus1;
+        sofa::type::MatSym<3>::Mat2Sym(EigenBasis*D.SymMatMultiply(EigenBasis.transposed()), CaBy2Minus1);
 
-        // build 4th-order tensor contribution in Voigt notation
+        // Invert deformation tensor
+        MatrixSym invC;
+        invertMatrix(invC, C);
+
+        // Build the 4th-order tensor contribution in Voigt notation
         Matrix6 elasticityTensor;
 
-        // diagonal entries
-        const Real coef = 0.5 * alpha1 - 1.;
-        Real fj= (Real)(pow(sinfo->J,(Real)(-alpha1/3.0)));
-        for (int n = 0; n < 6; n++)
+        const Real aBy2Minus2 = aBy2 - 2_sreal;
+        for (sofa::Index m = 0; m < 6; m++)
         {
-            int i, j;
-            switch(n)
-            {
-                case 0: i=0; j=0; break;
-                case 1: i=0; j=1; break;
-                case 2: i=1; j=1; break;
-                case 3: i=0; j=2; break;
-                case 4: i=1; j=2; break;
-                case 5: i=2; j=2; break;
-            }
-            for (int m = 0; m < 6; m++) 
+            sofa::Index i, j;
+            std::tie(i, j) = fromVoigt[m];
+
+            for (sofa::Index n = 0; n < 6; n++) 
             {
-                int k, l;
-                switch(m)
-                {
-                    case 0: k=0; l=0; break;
-                    case 1: k=0; l=1; break;
-                    case 2: k=1; l=1; break;
-                    case 3: k=0; l=2; break;
-                    case 4: k=1; l=2; break;
-                    case 5: k=2; l=2; break;
-                }
-                // Simple Ogden summation
-                for (int ii = 0 ; ii < 3; ii++)
-                {
-                    Real eigenTerm = pow(Evalue[ii], alpha1/2. - 2.);
-                    elasticityTensor(n, m) += coef * eigenTerm 
-                        * Evect(i, ii) * Evect(j, ii) * Evect(k, ii) * Evect(l, ii);
+                sofa::Index k, l;
+                std::tie(k, l) = fromVoigt[n];
 
-                    const Real eigenTerm_ii = pow(Evalue[ii], alpha1/2. - 1.);
-                    for (int jj = 0 ; jj < 3; jj++)
+                // SPK derivative contribution from spectral part
+                for (sofa::Index eI = 0 ; eI < 3; eI++)
+                {
+                    // Distortion term from differenting of eigenvalues
+                    const Real evalPowI2 = pow(Evalue[eI], aBy2Minus2);
+                    elasticityTensor(m, n) += aBy2Minus1 * evalPowI2 
+                        * Evect(i, eI) * Evect(j, eI) * Evect(k, eI) * Evect(l, eI);
+
+                    // Rotational part from differenting of eigenvectors
+                    const Real evalPowI = pow(Evalue[eI], aBy2Minus1);
+                    for (sofa::Index eJ = 0 ; eJ < 3; eJ++)
                     {
-                        if (jj == ii) continue;
+                        if (eJ == eI) continue;
 
                         Real coefRot{0};
-                        if (std::fabs(Evalue[ii] - Evalue[jj]) < std::numeric_limits<Real>::epsilon()) 
-                            coefRot = (alpha1 / 2. - 1.) * pow(Evalue[ii], alpha1 / 2. - 2.);
+
+                        if (std::fabs(Evalue[eI] - Evalue[eJ]) < std::numeric_limits<Real>::epsilon()) 
+                            coefRot = aBy2Minus1 * evalPowI2;
                         else
                         {
-                            const Real eigenTerm_jj = pow(Evalue[jj], alpha1/2. - 1.);
-                            coefRot = (eigenTerm_ii - eigenTerm_jj)/(Evalue[ii] - Evalue[jj]);
+                            const Real evalPowJ = pow(Evalue[eJ], aBy2Minus1);
+                            coefRot = (evalPowI - evalPowJ)/(Evalue[eI] - Evalue[eJ]);
                         }
-                        elasticityTensor(n, m) += coefRot * 0.5 *
+
+                        elasticityTensor(m, n) += coefRot * 0.5_sreal *
                         (
-                            Evect(i, ii) * Evect(j, jj) * Evect(k, jj) * Evect(l, ii) +
-                            Evect(i, ii) * Evect(j, jj) * Evect(k, ii) * Evect(l, jj)
+                            Evect(i, eI) * Evect(j, eJ) * Evect(k, eJ) * Evect(l, eI) +
+                            Evect(i, eI) * Evect(j, eJ) * Evect(k, eI) * Evect(l, eJ)
                         );
                     }
                 }
-                // F(J) term
-                elasticityTensor(n, m) -= alpha1/6. * (inversematrix(i,j) * Calpha_1(k,l)
-                    - trCalpha / 3. * inversematrix(i,j) * inversematrix(k,l));
 
-                // T22 term 1
-                elasticityTensor(n, m) -= alpha1/6. * Calpha_1(i,j) * inversematrix(k,l);
+                // SPK derivative contributions from isochoric part; product rule applies
+                // Factor 1 - Directly differentiate F(J)
+                elasticityTensor(m, n) -= alpha1/6_sreal * (invC(i,j) * CaBy2Minus1(k,l)
+                    - trCaBy2 / 3_sreal * invC(i,j) * invC(k,l));
+
+                // Factor 2 - 1st term - trace(C^(alpha1/2)) contribution
+                elasticityTensor(m, n) -= alpha1/6_sreal * CaBy2Minus1(i,j) * invC(k,l);
 
-                // T22 term 2
-                elasticityTensor(n, m) += alpha1/3. * trCalpha * 0.5 * (
-                    inversematrix(i,k) * inversematrix(j,l) 
-                    + inversematrix(i,l) * inversematrix(j,k));
+                // Factor 2 - 2nd term - C inverse contribution
+                elasticityTensor(m, n) += alpha1/3_sreal * trCaBy2 * 0.5_sreal * (
+                    invC(i,k) * invC(j,l) 
+                    + invC(i,l) * invC(j,k));
 
-                elasticityTensor(n, m) += 0.5 * alpha1 / mu1 / fj * 
+                // SPK derivative contribution from the volumetric part
+                elasticityTensor(m, n) += 0.5_sreal * alpha1 / mu1 / fj * 
                     (
-                        k0 * inversematrix(i,j) * inversematrix(k,l)
-                        - k0*log(sinfo->J) *(inversematrix(i,k) * inversematrix(j,l) 
-                            + inversematrix(i,l) * inversematrix(j,k))
+                        k0 * invC(i,j) * invC(k,l)
+                        - k0*log(sinfo->J) *(invC(i,k) * invC(j,l) 
+                            + invC(i,l) * invC(j,k))
                     ) ;
             }
         }
 
-        // multiply by scalar factor
-        outputTensor = 2.0 * fj * mu1 / alpha1 * elasticityTensor ;
-        for (int n = 0; n < 6; n++)
+        outputTensor = 2_sreal * fj * mu1 / alpha1 * elasticityTensor ;
+
+        // Adjust for Voigt notation using 2x factor on the off-diagonal
+        for (sofa::Index m = 0; m < 6; m++)
         {
-            outputTensor(n,1) *= 2.;
-            outputTensor(n,3) *= 2.;
-            outputTensor(n,4) *= 2.;
+            outputTensor(m,1) *= 2_sreal;
+            outputTensor(m,3) *= 2_sreal;
+            outputTensor(m,4) *= 2_sreal;
         }
     }
 
     void applyElasticityTensor(StrainInformation<DataTypes> *sinfo, const MaterialParameters<DataTypes> &param,
                                const MatrixSym& inputTensor, MatrixSym& outputTensor) override
     {
-        // For now, let's just multiply matrices using the ElasticityTensor explicitely
+        // For now, let's just multiply matrices using the ElasticityTensor explicitly
         Matrix6 elasticityTensor;
         this->ElasticityTensor(sinfo, param, elasticityTensor);
         auto temp = elasticityTensor * inputTensor;