[Backward] Fix trapezoidal rule in EulerImplicitSolver (#5169)

* [Backward] Fix trapezoidal rule in EulerImplicitSolver * add new benchmark and associated regression * Apply suggestions from code review --------- Co-authored-by: Paul Baksic <[email protected]> Co-authored-by: erik pernod <[email protected]>
sofa-framework · Jan 22, 2025 · 797e005 · 797e005
1 parent 53f78ec
commit 797e005
Show file tree

Hide file tree

Showing 3 changed files with 49 additions and 10 deletions.
diff --git a/...omponent/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp b/...omponent/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp
@@ -130,33 +130,31 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa::
 
     {
         SCOPED_TIMER("ComputeRHTerm");
-
         if (firstOrder)
         {
-            b.eq(f);
+            b.eq(f);  // b = f
         }
         else
         {
             // new more powerful visitors
 
             // force in the current configuration
-            b.eq(f, 1.0 / tr);                                                                    // b = f
+            b.eq(f);  // b = f
 
             msg_info() << "EulerImplicitSolver, f = " << f;
 
             // add the change of force due to stiffness + Rayleigh damping
             mop.addMBKv(b, core::MatricesFactors::M(-d_rayleighMass.getValue()),
                         core::MatricesFactors::B(0),
-                        core::MatricesFactors::K(h + d_rayleighStiffness.getValue())); // b =  f + ( rm M + (h+rs) K ) v
+                        core::MatricesFactors::K(h * tr + d_rayleighStiffness.getValue())); // b =  f + ( rm M + (h+rs) K ) v
 
             // integration over a time step
-            b.teq(h *
-                  tr);                                                                       // b = h(f + ( rm M + (h+rs) K ) v )
+            b.teq(h);                                             // b = h(f + ( rm M + (h+rs) K ) v )
         }
 
         msg_info() << "EulerImplicitSolver, b = " << b;
 
-        mop.projectResponse(b);          // b is projected to the constrained space
+        mop.projectResponse(b);                                   // b is projected to the constrained space
 
         msg_info() << "EulerImplicitSolver, projected b = " << b;
     }
@@ -165,7 +163,7 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa::
         SCOPED_TIMER("setSystemMBKMatrix");
         const core::MatricesFactors::M mFact (firstOrder ? 1 : 1 + tr * h * d_rayleighMass.getValue());
         const core::MatricesFactors::B bFact (firstOrder ? 0 : -tr * h);
-        const core::MatricesFactors::K kFact (firstOrder ? -h * tr : -tr * h * (h + d_rayleighStiffness.getValue()));
+        const core::MatricesFactors::K kFact (firstOrder ? -h * tr : -tr * h * (tr * h + d_rayleighStiffness.getValue()));
 
         mop.setSystemMBKMatrix(mFact, bFact, kFact, l_linearSolver.get());
 
@@ -226,7 +224,7 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa::
         sofa::helper::AdvancedTimer::stepBegin(prevStep);
 #define SOFATIMER_NEXTSTEP(s) { sofa::helper::AdvancedTimer::stepNext(prevStep,s); prevStep=s; }
 
-        // vel = vel + x
+        // v(t+dt) = Δv + v(t)
         newVel.eq(vel, x);
 
         if (solveConstraint)
@@ -236,7 +234,7 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa::
         }
         SOFATIMER_NEXTSTEP("UpdateX");
 
-        // pos = pos + h vel
+        // x(t+dt) = x(t) + dt * v(t+dt)
         newPos.eq(pos, newVel, h);
 
         if (solveConstraint)

diff --git a/examples/Benchmark/Analysis/Pendulum.scn b/examples/Benchmark/Analysis/Pendulum.scn
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<Node name="root" gravity="0 0 -9.81" dt="0.01">
+
+    <RequiredPlugin name="Sofa.Component.Constraint.Projective"/> <!-- Needed to use components [FixedProjectiveConstraint] -->
+    <RequiredPlugin name="Sofa.Component.Mass"/> <!-- Needed to use components [UniformMass] -->
+    <RequiredPlugin name="Sofa.Component.ODESolver.Forward"/> <!-- Needed to use components [EulerExplicitSolver] -->
+    <RequiredPlugin name="Sofa.Component.SolidMechanics.Spring"/> <!-- Needed to use components [SpringForceField] -->
+    <RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
+    <RequiredPlugin name="Sofa.Component.Visual"/> <!-- Needed to use components [VisualGrid VisualStyle] -->
+    <RequiredPlugin name="Sofa.GL.Component.Rendering3D"/> <!-- Needed to use components [OglSceneFrame] -->
+    <RequiredPlugin name="Sofa.Component.LinearSolver.Direct"/> <!-- Needed to use components [SparseLDLSolver] -->  
+    <RequiredPlugin name="Sofa.Component.ODESolver.Backward"/> <!-- Needed to use components [EulerImplicitSolver] -->  
+    <RequiredPlugin name="Sofa.GL.Component.Rendering2D"/> <!-- Needed to use components [OglLabel] -->  
+
+    <VisualGrid/>
+    <OglSceneFrame/>
+    <DefaultAnimationLoop/>
+
+    <VisualStyle displayFlags="showBehavior" />
+
+    <Node name="ClassicEulerImplicit" gravity="0 -9.81 0">
+    	<EulerImplicitSolver name="EulerImplicit-NoTrapez" trapezoidalScheme="0"/>
+    	<SparseLDLSolver template="CompressedRowSparseMatrixMat3x3d"/>
+        <MechanicalObject template="Vec3" name="Particles" restScale="1" position="0 0 0 1 0 0" showObject="1" showObjectScale="10"/>
+        <UniformMass template="Vec3" name="Mass" totalMass="1.0"/>
+        <FixedProjectiveConstraint template="Vec3" name="Fix Particle" indices="0" />
+        <SpringForceField template="Vec3" name="Internal Spring" spring="0 1 100 0.1 1" />
+        <OglLabel label="EulerImplicit" x="300" y="300"/>
+    </Node>
+
+    <Node name="TrapezoidalRule" gravity="0 -9.81 0">
+    	<EulerImplicitSolver name="EulerImplicit-WithTrapez" trapezoidalScheme="1"/>
+    	<SparseLDLSolver template="CompressedRowSparseMatrixMat3x3d"/>
+        <MechanicalObject template="Vec3" name="Particles" restScale="1" position="0 2 0 1 2 0" showObject="1" showObjectScale="10"/>
+        <UniformMass template="Vec3" name="Mass" totalMass="1.0"/>
+        <FixedProjectiveConstraint template="Vec3" name="Fix Particle" indices="0" />
+        <SpringForceField template="Vec3" name="Internal Spring" spring="0 1 100 0.1 1" />
+        <OglLabel label="TrapezoidalRule" x="400" y="100"/>
+    </Node>
+</Node>
diff --git a/examples/RegressionStateScenes.regression-tests b/examples/RegressionStateScenes.regression-tests
@@ -83,3 +83,4 @@ Benchmark/Performance/MatrixAssembly/MatrixAssembly_assembledCG_blocs.scn 300 1e
 Benchmark/Performance/MatrixAssembly/MatrixAssembly_direct.scn 300 1e-4 0 1
 Benchmark/Performance/MatrixAssembly/MatrixAssembly_direct_blocs.scn 300 1e-4 0 1
 Benchmark/Performance/MatrixAssembly/MatrixAssembly_matrixfreeCG.scn 300 1e-4 0 1
+Benchmark/Analysis/Pendulum.scn 300 1e-4 0 1