From 6d62139bf6cecf9b9aae5c4205ec11d2a5c509bd Mon Sep 17 00:00:00 2001
From: Alex Bilger <alexbilger0@gmail.com>
Date: Thu, 16 Nov 2023 14:26:34 +0100
Subject: [PATCH] [SolidMechanics.Spring] Implement buildStiffnessMatrix for
 TriangularBendingSprings

---
 .../spring/TriangularBendingSprings.h         |  1 +
 .../spring/TriangularBendingSprings.inl       | 30 +++++++++++-
 .../Spring/TriangularBendingSprings.scn       | 48 ++++++++-----------
 3 files changed, 50 insertions(+), 29 deletions(-)

diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/TriangularBendingSprings.h b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/TriangularBendingSprings.h
index 1c4a57cea86..6b37f9cbfc9 100644
--- a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/TriangularBendingSprings.h
+++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/TriangularBendingSprings.h
@@ -139,6 +139,7 @@ class TriangularBendingSprings : public core::behavior::ForceField<DataTypes>
 
     void addForce(const core::MechanicalParams* mparams, DataVecDeriv& d_f, const DataVecCoord& d_x, const DataVecDeriv& d_v) override;
     void addDForce(const core::MechanicalParams* mparams, DataVecDeriv& d_df, const DataVecDeriv& d_dx) override;
+    void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override;
     void buildDampingMatrix(core::behavior::DampingMatrix* /*matrix*/) final;
 
     void draw(const core::visual::VisualParams* vparams) override;
diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/TriangularBendingSprings.inl b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/TriangularBendingSprings.inl
index 90d04039e56..fd9f38232c2 100644
--- a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/TriangularBendingSprings.inl
+++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/TriangularBendingSprings.inl
@@ -502,7 +502,7 @@ void TriangularBendingSprings<DataTypes>::addForce(const core::MechanicalParams*
 template<class DataTypes>
 void TriangularBendingSprings<DataTypes>::addDForce(const core::MechanicalParams* mparams, DataVecDeriv& d_df, const DataVecDeriv& d_dx)
 {
-    VecDeriv& df = *d_df.beginEdit();
+    auto df = sofa::helper::getWriteAccessor(d_df);
     const VecDeriv& dx = d_dx.getValue();
     Real kFactor = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue());
 
@@ -524,7 +524,33 @@ void TriangularBendingSprings<DataTypes>::addDForce(const core::MechanicalParams
         df[a]+= dforce * kFactor;
         df[b]-= dforce * kFactor;
     }
-    d_df.endEdit();
+}
+
+template <class DataTypes>
+void TriangularBendingSprings<DataTypes>::buildStiffnessMatrix(
+    core::behavior::StiffnessMatrix* matrix)
+{
+    auto dfdx = matrix->getForceDerivativeIn(this->mstate)
+                       .withRespectToPositionsIn(this->mstate);
+
+    const type::vector<EdgeInformation>& edgeInf = edgeInfo.getValue();
+
+    for (sofa::Index i = 0; i < m_topology->getNbEdges(); i++)
+    {
+        const EdgeInformation& einfo = edgeInf[i];
+        if (einfo.is_activated) // edge not in middle of 2 triangles
+        {
+            const sofa::Index a = Deriv::total_size * einfo.m1;
+            const sofa::Index b = Deriv::total_size * einfo.m2;
+
+            const Mat& dfdxLocal = einfo.DfDx;
+
+            dfdx(a, a) += -dfdxLocal;
+            dfdx(a, b) +=  dfdxLocal;
+            dfdx(b, a) +=  dfdxLocal;
+            dfdx(b, b) += -dfdxLocal;
+        }
+    }
 }
 
 template <class DataTypes>
diff --git a/examples/Component/SolidMechanics/Spring/TriangularBendingSprings.scn b/examples/Component/SolidMechanics/Spring/TriangularBendingSprings.scn
index 608bfc799ed..1b7c86c3000 100644
--- a/examples/Component/SolidMechanics/Spring/TriangularBendingSprings.scn
+++ b/examples/Component/SolidMechanics/Spring/TriangularBendingSprings.scn
@@ -1,43 +1,37 @@
 <!-- Mechanical MassSpring Group Basic Example -->
 <Node name="root" dt="0.005" showBoundingTree="0" gravity="0 -90 10">
-    <RequiredPlugin name="Sofa.Component.Collision.Detection.Algorithm"/> <!-- Needed to use components [BVHNarrowPhase BruteForceBroadPhase CollisionPipeline] -->
-    <RequiredPlugin name="Sofa.Component.Collision.Detection.Intersection"/> <!-- Needed to use components [MinProximityIntersection] -->
-    <RequiredPlugin name="Sofa.Component.Collision.Geometry"/> <!-- Needed to use components [TriangleCollisionModel] -->
-    <RequiredPlugin name="Sofa.Component.Collision.Response.Contact"/> <!-- Needed to use components [CollisionResponse] -->
-    <RequiredPlugin name="Sofa.Component.Constraint.Projective"/> <!-- Needed to use components [FixedConstraint] -->
-    <RequiredPlugin name="Sofa.Component.IO.Mesh"/> <!-- Needed to use components [MeshGmshLoader] -->
-    <RequiredPlugin name="Sofa.Component.LinearSolver.Iterative"/> <!-- Needed to use components [CGLinearSolver] -->
-    <RequiredPlugin name="Sofa.Component.Mapping.Linear"/> <!-- Needed to use components [IdentityMapping] -->
-    <RequiredPlugin name="Sofa.Component.Mass"/> <!-- Needed to use components [DiagonalMass] -->
-    <RequiredPlugin name="Sofa.Component.ODESolver.Backward"/> <!-- Needed to use components [EulerImplicitSolver] -->
-    <RequiredPlugin name="Sofa.Component.SolidMechanics.FEM.Elastic"/> <!-- Needed to use components [TriangularFEMForceField] -->
-    <RequiredPlugin name="Sofa.Component.SolidMechanics.Spring"/> <!-- Needed to use components [TriangularBendingSprings] -->
-    <RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
-    <RequiredPlugin name="Sofa.Component.Topology.Container.Dynamic"/> <!-- Needed to use components [TriangleSetGeometryAlgorithms TriangleSetTopologyContainer TriangleSetTopologyModifier] -->
-    <RequiredPlugin name="Sofa.Component.Visual"/> <!-- Needed to use components [VisualStyle] -->
-    <RequiredPlugin name="Sofa.GL.Component.Rendering3D"/> <!-- Needed to use components [OglModel] -->
+    <Node name="plugins">
+        <RequiredPlugin name="Sofa.Component.Constraint.Projective"/> <!-- Needed to use components [FixedConstraint] -->
+        <RequiredPlugin name="Sofa.Component.IO.Mesh"/> <!-- Needed to use components [MeshGmshLoader] -->
+        <RequiredPlugin name="Sofa.Component.LinearSolver.Direct"/> <!-- Needed to use components [AsyncSparseLDLSolver] -->
+        <RequiredPlugin name="Sofa.Component.LinearSolver.Iterative"/> <!-- Needed to use components [ShewchukPCGLinearSolver] -->
+        <RequiredPlugin name="Sofa.Component.Mapping.Linear"/> <!-- Needed to use components [IdentityMapping] -->
+        <RequiredPlugin name="Sofa.Component.Mass"/> <!-- Needed to use components [DiagonalMass] -->
+        <RequiredPlugin name="Sofa.Component.ODESolver.Backward"/> <!-- Needed to use components [EulerImplicitSolver] -->
+        <RequiredPlugin name="Sofa.Component.SolidMechanics.FEM.Elastic"/> <!-- Needed to use components [TriangularFEMForceField] -->
+        <RequiredPlugin name="Sofa.Component.SolidMechanics.Spring"/> <!-- Needed to use components [TriangularBendingSprings] -->
+        <RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
+        <RequiredPlugin name="Sofa.Component.Topology.Container.Dynamic"/> <!-- Needed to use components [TriangleSetTopologyContainer] -->
+        <RequiredPlugin name="Sofa.Component.Visual"/> <!-- Needed to use components [VisualStyle] -->
+        <RequiredPlugin name="Sofa.GL.Component.Rendering3D"/> <!-- Needed to use components [OglModel] -->
+    </Node>
     <VisualStyle displayFlags="showBehaviorModels" />
-    <CollisionPipeline verbose="0" />
-    <BruteForceBroadPhase/>
-    <BVHNarrowPhase/>
-    <CollisionResponse response="PenalityContactForceField" />
-    <MinProximityIntersection name="Proximity" alarmDistance="0.8" contactDistance="0.5" />
     <DefaultAnimationLoop/>
-    
+
     <Node name="SquareGravity">
         <EulerImplicitSolver name="cg_odesolver" printLog="false"  rayleighStiffness="0.1" rayleighMass="0.1" />
-        <CGLinearSolver iterations="25" name="linear solver" tolerance="1.0e-9" threshold="1.0e-9" />
+        <ShewchukPCGLinearSolver preconditioner="@preconditioner"/>
+        <AsyncSparseLDLSolver name="preconditioner" template="CompressedRowSparseMatrixMat3x3"/>
         <MeshGmshLoader name="loader" filename="mesh/square3.msh" createSubelements="true"/>
         <MechanicalObject src="@loader" scale="10" />
-        <include href="Objects/TriangleSetTopology.xml" src="@loader" />
+        <TriangleSetTopologyContainer name="Container" triangles="@loader.triangles"/>
         <DiagonalMass massDensity="0.015" />
         <FixedConstraint indices="0 1" />
         <TriangularFEMForceField name="FEM" youngModulus="60" poissonRatio="0.3" method="large" />
         <TriangularBendingSprings name="BS" stiffness="300" damping="1.0" />
-        <TriangleCollisionModel />
         <Node >
-          <OglModel name="Visual" color="yellow" />
-          <IdentityMapping input="@.." output="@Visual" />
+            <OglModel name="Visual" color="yellow" />
+            <IdentityMapping input="@.." output="@Visual" />
         </Node>
     </Node>
 </Node>