[FEM] Account for rotation matrix derivatives in tangent stiffness for CorotationalFEMForceField

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

@@ -171,6 +171,9 @@ class CorotationalFEMForceField :
 
     StressVoigtVector computeStress(const DeformationGradient& F, sofa::Size elementId) override;
 
+    sofa::Data<bool> d_consistentTangent;
+    sofa::Data<bool> d_symmetrizeTangent;
+
 protected:
 
     void beforeElementForce(const sofa::core::MechanicalParams* mparams,
@@ -192,6 +195,10 @@ class CorotationalFEMForceField :
     sofa::type::vector<RotationMatrix> m_rotations;
     sofa::type::vector<RotationMatrix> m_initialRotationsTransposed;
 
+    /// Element stiffness plus the stiffness due to the element rotation, in the element frame,
+    /// accumulated in computeElementsForces. Empty if consistentTangent is off.
+    sofa::type::vector<typename trait::ElementHessian> m_tangents;
+
     /// Local displacement of the element nodes in the element's own rotated frame. Flat vector.
     ElementDisplacement computeElementLocalDisplacement(
         const std::array<sofa::Coord_t<DataTypes>, trait::NumberOfNodesInElement>& nodes,

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

@@ -32,7 +32,9 @@ namespace sofa::component::solidmechanics::fem::elastic
 
 template <class DataTypes, class ElementType>
 CorotationalFEMForceField<DataTypes, ElementType>::CorotationalFEMForceField()
-    : m_rotationMethods(this)
+    : d_consistentTangent(initData(&d_consistentTangent, true, "consistentTangent", "Include the change of the element rotations in the force derivatives"))
+    , d_symmetrizeTangent(initData(&d_symmetrizeTangent, true, "symmetrizeTangent", "Use the symmetric part of the consistent tangent"))
+    , m_rotationMethods(this)
 {
     this->addUpdateCallback("selectRotationMethod", {&this->m_rotationMethods.d_rotationMethod},
         [this](const sofa::core::DataTracker&)
@@ -74,6 +76,7 @@ void CorotationalFEMForceField<DataTypes, ElementType>::beforeElementForce(
     SOFA_UNUSED(x);
     const auto& elements = trait::FiniteElement::getElementSequence(*this->l_topology);
     m_rotations.resize(elements.size(), RotationMatrix::Identity());
+    m_tangents.resize(d_consistentTangent.getValue() ? elements.size() : 0);
 }
 
 template <class DataTypes, class ElementType>
@@ -129,6 +132,48 @@ void CorotationalFEMForceField<DataTypes, ElementType>::computeElementsForces(
         auto& elementForce = elementForces[elementId];
         elementForce = stiffnessMatrix * displacement;
 
+        if (!m_tangents.empty())
+        {
+            typename RotationMethods<DataTypes, ElementType>::RotationJacobian jacobian;
+            if (!m_rotationMethods.computeRotationJacobian(jacobian, elementRotation, elementNodesCoordinates, restElementNodesCoordinates))
+            {
+                msg_warning_once() << "The rotation jacobian of an element could not be computed. Its tangent ignores the change of rotation.";
+            }
+
+            const auto t0 = translation(restElementNodesCoordinates);
+            std::array<decltype(sofa::type::crossProductMatrix(sofa::Coord_t<DataTypes>()).transposed()), trait::NumberOfNodesInElement> localPositions;
+            sofa::Coord_t<DataTypes> nodeValue { sofa::type::NOINIT };
+            for (sofa::Size c = 0; c < trait::NumberOfNodesInElement; ++c)
+            {
+                displacement.getsub(c * DIM, nodeValue);
+                localPositions[c] = sofa::type::crossProductMatrix(nodeValue + restElementNodesCoordinates[c] - t0).transposed();
+            }
+
+            auto& tangent = m_tangents[elementId];
+            tangent = stiffnessMatrix;
+            RotationMatrix block { sofa::type::NOINIT };
+            for (sofa::Size a = 0; a < trait::NumberOfNodesInElement; ++a)
+            {
+                elementForce.getsub(a * DIM, nodeValue);
+                auto W = sofa::type::crossProductMatrix(nodeValue).transposed();
+                for (sofa::Size c = 0; c < trait::NumberOfNodesInElement; ++c)
+                {
+                    stiffnessMatrix.getsub(a * DIM, c * DIM, block);
+                    W -= block * localPositions[c];
+                }
+                for (sofa::Size b = 0; b < trait::NumberOfNodesInElement; ++b)
+                {
+                    tangent.getsub(a * DIM, b * DIM, block);
+                    tangent.setsub(a * DIM, b * DIM, block + W * jacobian[b]);
+                }
+            }
+
+            if (d_symmetrizeTangent.getValue())
+            {
+                tangent = (tangent + tangent.transposed()) / 2;
+            }
+        }
+
         sofa::type::Vec<DIM, sofa::Real_t<DataTypes>> nodeForce { sofa::type::NOINIT };
         for (sofa::Size n = 0; n < trait::NumberOfNodesInElement; ++n)
         {
@@ -149,6 +194,7 @@ void CorotationalFEMForceField<DataTypes, ElementType>::computeElementsForcesDer
     SOFA_UNUSED(mparams);
     const auto& elements = trait::FiniteElement::getElementSequence(*this->l_topology);
     auto elementStiffness = sofa::helper::getReadAccessor(this->d_elementStiffness);
+    const auto& elementTangentStiffness = m_tangents.empty() ? elementStiffness.ref() : m_tangents;
 
     for (std::size_t elementId = range.start; elementId < range.end; ++elementId)
     {
@@ -163,7 +209,7 @@ void CorotationalFEMForceField<DataTypes, ElementType>::computeElementsForcesDer
             element_dx.setsub(n * trait::spatial_dimensions, elementRotation.multTranspose(nodeDx[element[n]]));
         }
 
-        const auto& stiffnessMatrix = elementStiffness[elementId];
+        const auto& stiffnessMatrix = elementTangentStiffness[elementId];
 
         auto& df = elementForcesDeriv[elementId];
         df = stiffnessMatrix * element_dx;
@@ -194,7 +240,8 @@ void CorotationalFEMForceField<DataTypes, ElementType>::buildStiffnessMatrix(
         return;
     }
 
-    auto elementStiffnessIt = elementStiffness.begin();
+    const auto& elementTangentStiffness = m_tangents.empty() ? elementStiffness.ref() : m_tangents;
+    auto elementStiffnessIt = elementTangentStiffness.begin();
     auto rotationMatrixIt = m_rotations.begin();
     for (const auto& element : elements)
     {

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

@@ -64,6 +64,45 @@ struct HexahedronRotation
         rotationMatrix = currentRotation.multTranspose(initialRotationMatrix);
     }
 
+    template<std::size_t NumberOfNodesInElement, class JacobianBlock>
+    bool computeRotationJacobian(std::array<JacobianBlock, NumberOfNodesInElement>& jacobian,
+        const RotationMatrix& rotationMatrix,
+        const std::array<sofa::Coord_t<DataTypes>, NumberOfNodesInElement>& nodesPosition,
+        const std::array<sofa::Coord_t<DataTypes>, NumberOfNodesInElement>& nodesRestPosition)
+    {
+        using Real = sofa::Real_t<DataTypes>;
+        SOFA_UNUSED(nodesRestPosition);
+
+        RotationMatrix currentFrame(sofa::type::NOINIT);
+        computeOrientationFromHexahedron(currentFrame, nodesPosition);
+        const RotationMatrix restFrame = currentFrame * rotationMatrix;
+
+        const auto a = (nodesPosition[1] - nodesPosition[0] +
+                        nodesPosition[2] - nodesPosition[3] +
+                        nodesPosition[5] - nodesPosition[4] +
+                        nodesPosition[6] - nodesPosition[7]) * 0.25;
+        const auto b = (nodesPosition[3] - nodesPosition[0] +
+                        nodesPosition[2] - nodesPosition[1] +
+                        nodesPosition[7] - nodesPosition[4] +
+                        nodesPosition[6] - nodesPosition[5]) * 0.25;
+        const auto lengthA = a.norm();
+        const auto b1 = b * currentFrame[0];
+        const auto b2 = b * currentFrame[1];
+
+        const RotationMatrix Ga {{0, 0, -b1 / (lengthA * b2)}, {0, 0, -1 / lengthA}, {0, 1 / lengthA, 0}};
+        const RotationMatrix Gb {{0, 0, 1 / b2}, {0, 0, 0}, {0, 0, 0}};
+
+        constexpr std::array<Real, 8> alpha {-0.25, 0.25, 0.25, -0.25, -0.25, 0.25, 0.25, -0.25};
+        constexpr std::array<Real, 8> beta {-0.25, -0.25, 0.25, 0.25, -0.25, -0.25, 0.25, 0.25};
+
+        jacobian = {};
+        for (std::size_t k = 0; k < 8; ++k)
+        {
+            jacobian[k] = restFrame.multTranspose((Ga * alpha[k] + Gb * beta[k]) * restFrame);
+        }
+        return true;
+    }
+
     static constexpr sofa::helper::Item getItem()
     {
         return {"hexahedron", "Compute the rotation based on two average edges in the hexahedron"};

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

@@ -41,6 +41,7 @@ template <class DataTypes, class ElementType, Fit FitType, Algorithm AlgorithmTy
 struct PolarDecomposition
 {
     using RotationMatrix = sofa::type::Mat<DataTypes::spatial_dimensions, DataTypes::spatial_dimensions, sofa::Real_t<DataTypes>>;
+    using StretchMatrix = RotationMatrix;
 
     /// The remainder of the dimensions of an embedded element within a higher-dimensional space.
     // TODO: embedded elements (Codimension > 0) are not handled.
@@ -108,6 +109,81 @@ struct PolarDecomposition
         }
     }
 
+    /// Computes the jacobian of the rotation matrix
+    template<std::size_t NumberOfNodesInElement, class JacobianBlock>
+    bool computeRotationJacobian(std::array<JacobianBlock, NumberOfNodesInElement>& jacobian,
+        const RotationMatrix& rotationMatrix,
+        const std::array<sofa::Coord_t<DataTypes>, NumberOfNodesInElement>& nodesPosition,
+        const std::array<sofa::Coord_t<DataTypes>, NumberOfNodesInElement>& nodesRestPosition)
+    {
+        if constexpr (DataTypes::spatial_dimensions == 1)
+        {
+            // In 1D, there is no rotation.
+            return true;
+        }
+        else if constexpr (Codimension > 0)
+        {
+            // TODO: embedded elements.
+            jacobian = {};
+            return false;
+        }
+        else
+        {
+            const auto t = computeCentroid(nodesPosition);
+            const auto t0 = computeCentroid(nodesRestPosition);
+
+            sofa::type::Mat<NumberOfNodesInElement, DataTypes::spatial_dimensions, sofa::Real_t<DataTypes>> P(sofa::type::NOINIT);
+            sofa::type::Mat<NumberOfNodesInElement, DataTypes::spatial_dimensions, sofa::Real_t<DataTypes>> Q(sofa::type::NOINIT);
+
+            for (sofa::Size j = 0; j < NumberOfNodesInElement; ++j)
+            {
+                P[j] = nodesPosition[j] - t;
+                Q[j] = nodesRestPosition[j] - t0;
+            }
+
+            const auto M = Q.multTranspose(Q);
+            const auto traceM = sofa::type::trace(M);
+
+            const auto w = [&Q, &M, traceM]()
+            {
+                if constexpr (FitType == Fit::Affine)
+                {
+                    return Q * (M / traceM).inverted() / traceM;
+                }
+                else
+                {
+                    return Q / traceM;
+                }
+            }();
+
+            const auto F = P.multTranspose(w);
+            const StretchMatrix S = rotationMatrix.multTranspose(F);
+
+            sofa::type::Mat<JacobianBlock::nbLines, JacobianBlock::nbLines, sofa::Real_t<DataTypes>> A;
+            if constexpr (DataTypes::spatial_dimensions == 3)
+            {
+                A = StretchMatrix::Identity() * sofa::type::trace(S) - S;
+            }
+            else
+            {
+                A(0, 0) = sofa::type::trace(S);
+            }
+
+            decltype(A) inverseA(sofa::type::NOINIT);
+            if (!inverseA.invert(A))
+            {
+                jacobian = {};
+                return false;
+            }
+
+            for (sofa::Size k = 0; k < NumberOfNodesInElement; ++k)
+            {
+                jacobian[k] = inverseA * sofa::type::crossProductMatrix(w[k]);
+            }
+            return true;
+        }
+    }
+
     static constexpr sofa::helper::Item getItem()
     {
         if constexpr (AlgorithmType == Algorithm::Standard)

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

@@ -71,6 +71,7 @@ struct RotationMethodsContainer
 
     using RotationMatrix = sofa::type::Mat<DataTypes::spatial_dimensions, DataTypes::spatial_dimensions, sofa::Real_t<DataTypes>>;
     static constexpr std::size_t NumberOfNodesInElement = ElementType::NumberOfNodes;
+    using RotationJacobian = std::array<decltype(sofa::type::crossProductMatrix(sofa::Coord_t<DataTypes>())), NumberOfNodesInElement>;
 
     /**
      * Computes the current rotation state relative to the initial configuration.
@@ -92,6 +93,35 @@ struct RotationMethodsContainer
             m_rotationComputer);
     }
 
+    /**
+     * Computes the jacobian of the rotation matrix. Zero for methods that do not provide one.
+     *
+     * @param jacobian Output: Derivative of the rotation with respect to each node position, in the element frame
+     * @param rotationMatrix Current rotation matrix, as computed by computeRotation
+     * @param nodesPosition Current positions of element nodes
+     * @param nodesRestPosition Rest positions of element nodes (initial configuration)
+     * @return False if the method failed to compute the jacobian
+     */
+    bool computeRotationJacobian(RotationJacobian& jacobian, const RotationMatrix& rotationMatrix,
+        const std::array<sofa::Coord_t<DataTypes>, NumberOfNodesInElement>& nodesPosition,
+        const std::array<sofa::Coord_t<DataTypes>, NumberOfNodesInElement>& nodesRestPosition)
+    {
+        return std::visit(
+            [&](auto& rotationComputer)
+            {
+                if constexpr (requires { rotationComputer.computeRotationJacobian(jacobian, rotationMatrix, nodesPosition, nodesRestPosition); })
+                {
+                    return rotationComputer.computeRotationJacobian(jacobian, rotationMatrix, nodesPosition, nodesRestPosition);
+                }
+                else
+                {
+                    jacobian = {};
+                    return true;
+                }
+            },
+            m_rotationComputer);
+    }
+
     struct RotationMethodsItems final : sofa::helper::SelectableItem<RotationMethodsItems>
     {
         using sofa::helper::SelectableItem<RotationMethodsItems>::SelectableItem;

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

@@ -45,6 +45,34 @@ struct TriangleRotation
         rotationMatrix = currentRotation.multTranspose(initialRotationMatrix);
     }
 
+    template<std::size_t NumberOfNodesInElement, class JacobianBlock>
+    bool computeRotationJacobian(std::array<JacobianBlock, NumberOfNodesInElement>& jacobian,
+        const RotationMatrix& rotationMatrix,
+        const std::array<sofa::Coord_t<DataTypes>, NumberOfNodesInElement>& nodesPosition,
+        const std::array<sofa::Coord_t<DataTypes>, NumberOfNodesInElement>& nodesRestPosition)
+    {
+        SOFA_UNUSED(nodesRestPosition);
+
+        RotationMatrix currentFrame(sofa::type::NOINIT);
+        computeRotationFrom3Points(currentFrame, {nodesPosition[0], nodesPosition[1], nodesPosition[2]});
+        const RotationMatrix restFrame = currentFrame * rotationMatrix;
+
+        const auto a = nodesPosition[1] - nodesPosition[0];
+        const auto b = nodesPosition[2] - nodesPosition[0];
+        const auto lengthA = a.norm();
+        const auto b1 = b * currentFrame[0];
+        const auto b2 = b * currentFrame[1];
+
+        const RotationMatrix Ga {{0, 0, -b1 / (lengthA * b2)}, {0, 0, -1 / lengthA}, {0, 1 / lengthA, 0}};
+        const RotationMatrix Gb {{0, 0, 1 / b2}, {0, 0, 0}, {0, 0, 0}};
+
+        jacobian = {};
+        jacobian[0] = restFrame.multTranspose((-Ga - Gb) * restFrame);
+        jacobian[1] = restFrame.multTranspose(Ga * restFrame);
+        jacobian[2] = restFrame.multTranspose(Gb * restFrame);
+        return true;
+    }
+
     static constexpr sofa::helper::Item getItem()
     {
         return {"triangle", "Compute the rotation based on the Gram-Schmidt frame alignment"};

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

@@ -5,6 +5,7 @@ project(Sofa.Component.SolidMechanics.FEM.Elastic_test)
 set(SOURCE_FILES
     BaseTetrahedronFEMForceField_test.h
     BeamFEMForceField_test.cpp
+    CorotationalFEMForceField_test.cpp
     FastTetrahedralCorotationalForceField_test.cpp
     FEMSourceTermIntegrator_test.cpp
     HexahedralFEMForceField_test.cpp