From f6b577baf6c234f7c269724c129705ef09dd65d0 Mon Sep 17 00:00:00 2001
From: Alex Bilger <alexbilger0@gmail.com>
Date: Mon, 9 Dec 2024 13:09:34 +0100
Subject: [PATCH] example

---
 .../odesolver/forward/EulerExplicitSolver.cpp | 24 +++++++++++++++----
 .../odesolver/forward/EulerExplicitSolver.h   |  2 ++
 2 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.cpp b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.cpp
index ebf86e5ed79..e1125646203 100644
--- a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.cpp
+++ b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.cpp
@@ -24,9 +24,11 @@
 #include <sofa/simulation/MechanicalOperations.h>
 #include <sofa/simulation/VectorOperations.h>
 #include <sofa/core/ObjectFactory.h>
+#include <sofa/core/behavior/BaseMass.h>
 #include <sofa/core/behavior/LinearSolver.h>
 #include <sofa/helper/AdvancedTimer.h>
 #include <sofa/helper/ScopedAdvancedTimer.h>
+#include <sofa/simulation/MechanicalVisitorCreator.h>
 
 #include <sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h>
 using sofa::simulation::mechanicalvisitor::MechanicalGetNonDiagonalMassesCountVisitor;
@@ -84,11 +86,8 @@ void EulerExplicitSolver::solve(const core::ExecParams* params,
     addSeparateGravity(&mop, dt, vResult);
     computeForce(&mop, f);
 
-    sofa::Size nbNonDiagonalMasses = 0;
-    MechanicalGetNonDiagonalMassesCountVisitor(&mop.mparams, &nbNonDiagonalMasses).execute(this->getContext());
-
     // Mass matrix is diagonal, solution can thus be found by computing acc = f/m
-    if(nbNonDiagonalMasses == 0.)
+    if(isMassMatrixDiagonal(mop) == 0)
     {
         // acc = M^-1 * f
         computeAcceleration(&mop, acc, f);
@@ -342,4 +341,21 @@ void EulerExplicitSolver::solveSystem(core::MultiVecDerivId solution, core::Mult
     l_linearSolver->solveSystem();
 }
 
+bool EulerExplicitSolver::isMassMatrixDiagonal(
+    const sofa::simulation::common::MechanicalOperations& mop)
+{
+    sofa::Size nbNonDiagonalMasses = 0;
+    auto visitor = simulation::makeMechanicalVisitor(&mop.mparams, simulation::TopDownMassCallable(
+        [&nbNonDiagonalMasses](simulation::Node*, const sofa::core::behavior::BaseMass* mass)
+        {
+            if (mass && !mass->isDiagonal())
+            {
+                nbNonDiagonalMasses++;
+            }
+            return simulation::Visitor::RESULT_CONTINUE;
+        }));
+    visitor.execute(this->getContext());
+    return nbNonDiagonalMasses > 0;
+}
+
 } // namespace sofa::component::odesolver::forward
diff --git a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.h b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.h
index d5c9d1b83f2..024868465e5 100644
--- a/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.h
+++ b/Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.h
@@ -123,6 +123,8 @@ class SOFA_COMPONENT_ODESOLVER_FORWARD_API EulerExplicitSolver : public sofa::co
     void assembleSystemMatrix(sofa::simulation::common::MechanicalOperations* mop) const;
 
     void solveSystem(core::MultiVecDerivId solution, core::MultiVecDerivId rhs) const;
+
+    bool isMassMatrixDiagonal(const sofa::simulation::common::MechanicalOperations& mop);
 };
 
 } // namespace sofa::component::odesolver::forward