From e3304b68789bc694a1ff461c95d3a3ac726cfac8 Mon Sep 17 00:00:00 2001
From: hugtalbot <hugo.talbot@sofa-framework.org>
Date: Tue, 17 Dec 2024 17:20:38 +0100
Subject: [PATCH] [Backward] Fix trapezoidal rule in EulerImplicitSolver

---
 .../odesolver/backward/EulerImplicitSolver.cpp | 18 ++++++++----------
 1 file changed, 8 insertions(+), 10 deletions(-)

diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp
index e89ed41245e..7d5bed30fc3 100644
--- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp
+++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp
@@ -130,32 +130,30 @@ 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, -d_rayleighMass.getValue(), 0,
-                        h + d_rayleighStiffness.getValue()); // b =  f + ( rm M + (h+rs) K ) v
+                        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;
     }
@@ -173,7 +171,7 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa::
         {
             mFact = 1 + tr * h * d_rayleighMass.getValue();
             bFact = -tr * h;
-            kFact = -tr * h * (h + d_rayleighStiffness.getValue());
+            kFact = -tr * h * (tr * h + d_rayleighStiffness.getValue());
         }
         mop.setSystemMBKMatrix(mFact, bFact, kFact, l_linearSolver.get());
 
@@ -234,7 +232,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)
@@ -244,7 +242,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)