[FEM] Refactor PolarDecomposition and add an affine motion fit for rotation extraction

Sofa/Component/SolidMechanics/FEM/Elastic/CMakeLists.txt

@@ -62,7 +62,6 @@ set(HEADER_FILES
     ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/impl/rotations/IdentityRotation.h
     ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/impl/rotations/PolarDecomposition.h
     ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/impl/rotations/RotationMethodsContainer.h
-    ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/impl/rotations/StablePolarDecomposition.h
     ${SOFACOMPONENTSOLIDMECHANICSFEMELASTIC_SOURCE_DIR}/impl/rotations/TriangleRotation.h
 )
 

Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/CorotationalFEMForceField.h

@@ -27,7 +27,6 @@
 #include <sofa/component/solidmechanics/fem/elastic/impl/rotations/IdentityRotation.h>
 #include <sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h>
 #include <sofa/component/solidmechanics/fem/elastic/impl/rotations/RotationMethodsContainer.h>
-#include <sofa/component/solidmechanics/fem/elastic/impl/rotations/StablePolarDecomposition.h>
 #include <sofa/component/solidmechanics/fem/elastic/impl/rotations/TriangleRotation.h>
 #include <sofa/core/behavior/ForceField.h>
 #include <sofa/component/solidmechanics/fem/elastic/CauchyStressEvaluator.h>
@@ -54,10 +53,13 @@ namespace sofa::component::solidmechanics::fem::elastic
  */
 template <class DataTypes, class ElementType>
 struct RotationMethods : RotationMethodsContainer<DataTypes, ElementType,
-    StablePolarDecomposition<DataTypes>, PolarDecomposition<DataTypes>, IdentityRotation
+    RigidFitStablePolarDecomposition<DataTypes, ElementType>, RigidFitPolarDecomposition<DataTypes, ElementType>, IdentityRotation,
+    AffineFitStablePolarDecomposition<DataTypes, ElementType>, AffineFitPolarDecomposition<DataTypes, ElementType>
 >
 {
-    using Inherit = RotationMethodsContainer<DataTypes, ElementType, StablePolarDecomposition<DataTypes>, PolarDecomposition<DataTypes>, IdentityRotation>;
+    using Inherit = RotationMethodsContainer<DataTypes, ElementType,
+        RigidFitStablePolarDecomposition<DataTypes, ElementType>, RigidFitPolarDecomposition<DataTypes, ElementType>, IdentityRotation,
+        AffineFitStablePolarDecomposition<DataTypes, ElementType>, AffineFitPolarDecomposition<DataTypes, ElementType>>;
     explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent)
     {}
 };
@@ -68,11 +70,13 @@ using Vec3Real = sofa::defaulttype::StdVectorTypes<sofa::type::Vec<3, Real>, sof
 //partial specialization for linear triangle in 3D
 template <class Real>
 struct RotationMethods<Vec3Real<Real>, sofa::geometry::Triangle> : RotationMethodsContainer<Vec3Real<Real>, sofa::geometry::Triangle,
-    StablePolarDecomposition<Vec3Real<Real>>, PolarDecomposition<Vec3Real<Real>>, IdentityRotation, TriangleRotation<Vec3Real<Real>>
+    RigidFitStablePolarDecomposition<Vec3Real<Real>, sofa::geometry::Triangle>, RigidFitPolarDecomposition<Vec3Real<Real>, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation<Vec3Real<Real>>,
+    AffineFitStablePolarDecomposition<Vec3Real<Real>, sofa::geometry::Triangle>, AffineFitPolarDecomposition<Vec3Real<Real>, sofa::geometry::Triangle>
 >
 {
     using Inherit = RotationMethodsContainer<Vec3Real<Real>, sofa::geometry::Triangle,
-        StablePolarDecomposition<Vec3Real<Real>>, PolarDecomposition<Vec3Real<Real>>, IdentityRotation, TriangleRotation<Vec3Real<Real>> >;
+        RigidFitStablePolarDecomposition<Vec3Real<Real>, sofa::geometry::Triangle>, RigidFitPolarDecomposition<Vec3Real<Real>, sofa::geometry::Triangle>, IdentityRotation, TriangleRotation<Vec3Real<Real>>,
+        AffineFitStablePolarDecomposition<Vec3Real<Real>, sofa::geometry::Triangle>, AffineFitPolarDecomposition<Vec3Real<Real>, sofa::geometry::Triangle>>;
 
     explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent)
     {
@@ -83,11 +87,13 @@ struct RotationMethods<Vec3Real<Real>, sofa::geometry::Triangle> : RotationMetho
 //partial specialization for linear tetrahedron
 template <class DataTypes>
 struct RotationMethods<DataTypes, sofa::geometry::Tetrahedron> : RotationMethodsContainer<DataTypes, sofa::geometry::Tetrahedron,
-    StablePolarDecomposition<DataTypes>, PolarDecomposition<DataTypes>, IdentityRotation, TriangleRotation<DataTypes>
+    RigidFitStablePolarDecomposition<DataTypes, sofa::geometry::Tetrahedron>, RigidFitPolarDecomposition<DataTypes, sofa::geometry::Tetrahedron>, IdentityRotation, TriangleRotation<DataTypes>,
+    AffineFitStablePolarDecomposition<DataTypes, sofa::geometry::Tetrahedron>, AffineFitPolarDecomposition<DataTypes, sofa::geometry::Tetrahedron>
 >
 {
     using Inherit = RotationMethodsContainer<DataTypes, sofa::geometry::Tetrahedron,
-        StablePolarDecomposition<DataTypes>, PolarDecomposition<DataTypes>, IdentityRotation, TriangleRotation<DataTypes> >;
+        RigidFitStablePolarDecomposition<DataTypes, sofa::geometry::Tetrahedron>, RigidFitPolarDecomposition<DataTypes, sofa::geometry::Tetrahedron>, IdentityRotation, TriangleRotation<DataTypes>,
+        AffineFitStablePolarDecomposition<DataTypes, sofa::geometry::Tetrahedron>, AffineFitPolarDecomposition<DataTypes, sofa::geometry::Tetrahedron>>;
 
     explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent)
     {
@@ -98,11 +104,13 @@ struct RotationMethods<DataTypes, sofa::geometry::Tetrahedron> : RotationMethods
 //partial specialization for linear hexahedron
 template <class DataTypes>
 struct RotationMethods<DataTypes, sofa::geometry::Hexahedron> : RotationMethodsContainer<DataTypes, sofa::geometry::Hexahedron,
-    StablePolarDecomposition<DataTypes>, PolarDecomposition<DataTypes>, IdentityRotation, HexahedronRotation<DataTypes>
+    RigidFitStablePolarDecomposition<DataTypes, sofa::geometry::Hexahedron>, RigidFitPolarDecomposition<DataTypes, sofa::geometry::Hexahedron>, IdentityRotation, HexahedronRotation<DataTypes>,
+    AffineFitStablePolarDecomposition<DataTypes, sofa::geometry::Hexahedron>, AffineFitPolarDecomposition<DataTypes, sofa::geometry::Hexahedron>
 >
 {
     using Inherit = RotationMethodsContainer<DataTypes, sofa::geometry::Hexahedron,
-        StablePolarDecomposition<DataTypes>, PolarDecomposition<DataTypes>, IdentityRotation, HexahedronRotation<DataTypes> >;
+        RigidFitStablePolarDecomposition<DataTypes, sofa::geometry::Hexahedron>, RigidFitPolarDecomposition<DataTypes, sofa::geometry::Hexahedron>, IdentityRotation, HexahedronRotation<DataTypes>,
+        AffineFitStablePolarDecomposition<DataTypes, sofa::geometry::Hexahedron>, AffineFitPolarDecomposition<DataTypes, sofa::geometry::Hexahedron>>;
 
     explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent)
     {
@@ -113,15 +121,17 @@ struct RotationMethods<DataTypes, sofa::geometry::Hexahedron> : RotationMethodsC
 //partial specialization for quadratic tetrahedron
 template <class DataTypes>
 struct RotationMethods<DataTypes, sofa::geometry::QuadraticTetrahedron> : RotationMethodsContainer<DataTypes, sofa::geometry::QuadraticTetrahedron,
-    PolarDecomposition<DataTypes>, IdentityRotation
+    RigidFitPolarDecomposition<DataTypes, sofa::geometry::QuadraticTetrahedron>, IdentityRotation,
+    AffineFitPolarDecomposition<DataTypes, sofa::geometry::QuadraticTetrahedron>
 >
 {
     using Inherit = RotationMethodsContainer<DataTypes, sofa::geometry::QuadraticTetrahedron,
-        PolarDecomposition<DataTypes>, IdentityRotation>;
+        RigidFitPolarDecomposition<DataTypes, sofa::geometry::QuadraticTetrahedron>, IdentityRotation,
+        AffineFitPolarDecomposition<DataTypes, sofa::geometry::QuadraticTetrahedron>>;
 
     explicit RotationMethods(sofa::core::objectmodel::BaseObject* parent) : Inherit(parent)
     {
-        this->d_rotationMethod.setValue(PolarDecomposition<DataTypes>::getItem().key);
+        this->d_rotationMethod.setValue(RigidFitPolarDecomposition<DataTypes, sofa::geometry::QuadraticTetrahedron>::getItem().key);
     }
 };
 

Sofa/Component/SolidMechanics/FEM/Elastic/src/sofa/component/solidmechanics/fem/elastic/impl/rotations/PolarDecomposition.h

@@ -22,17 +22,31 @@
 #pragma once
 
 #include <sofa/core/trait/DataTypes.h>
+#include <sofa/fem/FiniteElement.h>
 #include <sofa/helper/SelectableItem.h>
 #include <sofa/helper/decompose.h>
 
 namespace sofa::component::solidmechanics::fem::elastic
 {
 
-template <class DataTypes>
+/// How the motion of the nodes is fitted before the polar decomposition.
+enum class Fit { Rigid, Affine };
+
+/// How the polar decomposition is computed: 
+/// Standard calls Decompose::polarDecomposition
+/// Stable calls Decompose::polarDecomposition_stable
+enum class Algorithm { Standard, Stable };
+
+template <class DataTypes, class ElementType, Fit FitType, Algorithm AlgorithmType>
 struct PolarDecomposition
 {
     using RotationMatrix = sofa::type::Mat<DataTypes::spatial_dimensions, DataTypes::spatial_dimensions, sofa::Real_t<DataTypes>>;
 
+    /// The remainder of the dimensions of an embedded element within a higher-dimensional space.
+    // TODO: embedded elements (Codimension > 0) are not handled.
+    static constexpr sofa::Size Codimension =
+        DataTypes::spatial_dimensions - sofa::fem::FiniteElement<ElementType, DataTypes>::TopologicalDimension;
+
     template<std::size_t NumberOfNodesInElement>
     sofa::Coord_t<DataTypes> computeCentroid(const std::array<sofa::Coord_t<DataTypes>, NumberOfNodesInElement>& nodes)
     {
@@ -64,15 +78,73 @@ struct PolarDecomposition
             Q[j] = nodesRestPosition[j] - t0;
         }
 
-        const auto H = P.multTranspose(Q);
+        const auto F = [&P, &Q]() -> RotationMatrix
+        {
+            const auto H = P.multTranspose(Q);
+
+            if constexpr (Codimension > 0)
+            {
+                // Not handled: H is decomposed as it is.
+                return H;
+            }
+            else if constexpr (FitType == Fit::Affine)
+            {
+                const auto M = Q.multTranspose(Q);
+                return H * (M / sofa::type::trace(M)).inverted();
+            }
+            else
+            {
+                return H;
+            }
+        }();
 
-        sofa::helper::Decompose<sofa::Real_t<DataTypes>>::polarDecomposition(H, rotationMatrix);
+        if constexpr (AlgorithmType == Algorithm::Standard)
+        {
+            sofa::helper::Decompose<sofa::Real_t<DataTypes>>::polarDecomposition(F, rotationMatrix);
+        }
+        else
+        {
+            sofa::helper::Decompose<sofa::Real_t<DataTypes>>::polarDecomposition_stable(F, rotationMatrix);
+        }
     }
 
     static constexpr sofa::helper::Item getItem()
     {
-        return {"polar", "Polar decomposition"};
+        if constexpr (AlgorithmType == Algorithm::Standard)
+        {
+            if constexpr (FitType == Fit::Rigid)
+            {
+                return {"polar", "Polar decomposition of the rigid fit"};
+            }
+            else
+            {
+                return {"polar_affine", "Polar decomposition of the affine fit"};
+            }
+        }
+        else
+        {
+            if constexpr (FitType == Fit::Rigid)
+            {
+                return {"stable_polar", "Stable polar decomposition of the rigid fit"};
+            }
+            else
+            {
+                return {"stable_polar_affine", "Stable polar decomposition of the affine fit"};
+            }
+        }
     }
 };
 
+template <class DataTypes, class ElementType>
+using RigidFitPolarDecomposition = PolarDecomposition<DataTypes, ElementType, Fit::Rigid, Algorithm::Standard>;
+
+template <class DataTypes, class ElementType>
+using AffineFitPolarDecomposition = PolarDecomposition<DataTypes, ElementType, Fit::Affine, Algorithm::Standard>;
+
+template <class DataTypes, class ElementType>
+using RigidFitStablePolarDecomposition = PolarDecomposition<DataTypes, ElementType, Fit::Rigid, Algorithm::Stable>;
+
+template <class DataTypes, class ElementType>
+using AffineFitStablePolarDecomposition = PolarDecomposition<DataTypes, ElementType, Fit::Affine, Algorithm::Stable>;
+
 }

Sofa/Component/SolidMechanics/FEM/Elastic/tests/CMakeLists.txt

@@ -9,6 +9,7 @@ set(SOURCE_FILES
     HexahedralFEMForceField_test.cpp
     HexahedronFEMForceField_test.cpp
     LinearSmallStrainFEMForceField_test.cpp
+    RotationMethods_test.cpp
     StrainDisplacement_test.cpp
     TetrahedralCorotationalFEMForceField_test.cpp
     TetrahedronFEMForceField_stepTest.cpp