example

sofa-framework · Dec 9, 2024 · f6b577b · f6b577b
1 parent 0ae3e7e
commit f6b577b
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 4 deletions.
diff --git a/.../Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.cpp b/.../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
@@ -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