[SpringForceField] Add stiffness matrix computation to JointSpringForceField

Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/JointSpringForceField.h

@@ -39,10 +39,31 @@ template<typename DataTypes>
 class JointSpring;
 
 
-/** JointSpringForceField simulates 6D springs between Rigid DOFS
-  Use kst vector to specify the directionnal stiffnesses (on each local axe)
-  Use ksr vector to specify the rotational stiffnesses (on each local axe)
-*/
+/**
+ * @class JointSpringForceField
+ * @brief A force field modeling complex joint interactions between two mechanical states.
+ *
+ * This component implements a force field that models elastic and dissipative interactions
+ * (springs and dampers) between pairs of points in two different MechanicalStates.
+ * It is primarily designed for use with Rigid3Types to handle both translation and rotation.
+ *
+ * Each joint (JointSpring) can have:
+ * - Different stiffness values for translation and rotation (soft and hard stiffness).
+ * - Damping factor for both translational and rotational velocities.
+ * - Angular limits for each rotation axis (X, Y, Z).
+ * - A 'blocking' stiffness applied when rotation limits are exceeded.
+ * - Initial translation and rotation offsets.
+ *
+ * The force computation accounts for:
+ * - Constant external forces/torques read from an input file.
+ * - Relative position and orientation between the joint points.
+ * - Relative velocities for damping.
+ * - Joint limits and blocking constraints.
+ *
+ * It also supports logging joint data to an output file at specified intervals.
+ *
+ * @tparam DataTypes The type of data used for the mechanical states (usually Rigid3Types).
+ */
 template<class DataTypes>
 class JointSpringForceField : public core::behavior::PairInteractionForceField<DataTypes>
 {
@@ -123,6 +144,8 @@ class JointSpringForceField : public core::behavior::PairInteractionForceField<D
                            const DataVecDeriv& data_dx1,
                            const DataVecDeriv& data_dx2) override;
 
+    void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override;
+
     void buildDampingMatrix(core::behavior::DampingMatrix* /*matrix*/) final;
 
     SReal getPotentialEnergy(const core::MechanicalParams*,

Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/JointSpringForceField.inl

@@ -25,6 +25,7 @@
 #include <sofa/core/visual/VisualParams.h>
 #include <sofa/core/MechanicalParams.h>
 #include <sofa/component/solidmechanics/spring/JointSpring.h>
+#include <sofa/core/behavior/BaseLocalForceFieldMatrix.h>
 #include <fstream>
 
 namespace sofa::component::solidmechanics::spring
@@ -358,6 +359,58 @@ void JointSpringForceField<DataTypes>::addDForce(const core::MechanicalParams *m
     data_df2.endEdit();
 }
 
+template<class DataTypes>
+void JointSpringForceField<DataTypes>::buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix)
+{
+    auto state1 = this->mstate1.get();
+    auto state2 = this->mstate2.get();
+    if (!state1 || !state2) return;
+
+    auto df1_dx1 = matrix->getForceDerivativeIn(state1).withRespectToPositionsIn(state1);
+    auto df1_dx2 = matrix->getForceDerivativeIn(state1).withRespectToPositionsIn(state2);
+    auto df2_dx1 = matrix->getForceDerivativeIn(state2).withRespectToPositionsIn(state1);
+    auto df2_dx2 = matrix->getForceDerivativeIn(state2).withRespectToPositionsIn(state2);
+
+    df1_dx1.checkValidity(this);
+    df1_dx2.checkValidity(this);
+    df2_dx1.checkValidity(this);
+    df2_dx2.checkValidity(this);
+
+    const auto springsAccessor = sofa::helper::getReadAccessor(d_springs);
+    for (const auto& spring : springsAccessor)
+    {
+        Mat mT;
+        for (sofa::Index j = 0; j < 3; ++j)
+        {
+            mT(j, j) = spring.KT[j];
+        }
+
+        Mat mR;
+        for (sofa::Index j = 0; j < 3; ++j)
+        {
+            mR(j, j) = spring.KR[j];
+        }
+
+        Mat rot(sofa::type::NOINIT);
+        spring.ref.toMatrix(rot);
+        const Mat rotInv = rot.transposed();
+
+        //translation
+        const Mat worldKT = rot * mT * rotInv;
+        df1_dx1(spring.m1, spring.m1) +=  worldKT;
+        df1_dx2(spring.m1, spring.m2) += -worldKT;
+        df2_dx1(spring.m2, spring.m1) += -worldKT;
+        df2_dx2(spring.m2, spring.m2) +=  worldKT;
+
+        //rotation
+        const Mat worldKR = rot * mR * rotInv;
+        df1_dx1(spring.m1 + N, spring.m1 + N) +=  worldKR;
+        df1_dx2(spring.m1 + N, spring.m2 + N) += -worldKR;
+        df2_dx1(spring.m2 + N, spring.m1 + N) += -worldKR;
+        df2_dx2(spring.m2 + N, spring.m2 + N) +=  worldKR;
+    }
+}
+
 template <class DataTypes>
 void JointSpringForceField<DataTypes>::buildDampingMatrix(core::behavior::DampingMatrix*)
 {