[Simulation] Mapping graph visitors

Sofa/Component/LinearSystem/src/sofa/component/linearsystem/TypedMatrixLinearSystem.inl

@@ -71,7 +71,7 @@ void TypedMatrixLinearSystem<TMatrix, TVector>::preAssembleSystem(const core::Me
     {
         SCOPED_TIMER_VARNAME(mappingGraphTimer, "buildMappingGraph");
         // build the mapping graph: this is used to know the relationship between the mechanical states and their associated components
-        m_mappingGraph.build(mparams, getSolveContext());
+        m_mappingGraph.build(getSolveContext());
     }
 
     associateLocalMatrixToComponents(mparams);
@@ -191,7 +191,7 @@ void TypedMatrixLinearSystem<TMatrix, TVector>::setRHS(core::MultiVecDerivId v)
 {
     if (!m_mappingGraph.isBuilt()) //note: this check does not make sure the scene graph is different from when the mapping graph has been built
     {
-        m_mappingGraph.build(core::execparams::defaultInstance(), getSolveContext());
+        m_mappingGraph.build(getSolveContext());
     }
 
     copyLocalVectorToGlobalVector(v, getRHSVector());
@@ -202,7 +202,7 @@ void TypedMatrixLinearSystem<TMatrix, TVector>::setSystemSolution(core::MultiVec
 {
     if (!m_mappingGraph.isBuilt()) //note: this check does not guarantee the scene graph is not different from when the mapping graph has been built
     {
-        m_mappingGraph.build(core::execparams::defaultInstance(), getSolveContext());
+        m_mappingGraph.build(getSolveContext());
     }
 
     if (!v.isNull())

Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp

@@ -22,7 +22,7 @@
 #include <sofa/component/odesolver/backward/EulerImplicitSolver.h>
 
 #include <sofa/core/visual/VisualParams.h>
-#include <sofa/simulation/MechanicalOperations.h>
+#include <sofa/simulation/MappingGraphMechanicalOperations.h>
 #include <sofa/simulation/VectorOperations.h>
 #include <sofa/helper/AdvancedTimer.h>
 #include <sofa/core/ObjectFactory.h>
@@ -82,11 +82,13 @@ void EulerImplicitSolver::cleanup()
 
 void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult)
 {
+    m_mappingGraph.build(this->getContext());
+
 #ifdef SOFA_DUMP_VISITOR_INFO
     sofa::simulation::Visitor::printNode("SolverVectorAllocation");
 #endif
     sofa::simulation::common::VectorOperations vop( params, this->getContext() );
-    sofa::simulation::common::MechanicalOperations mop( params, this->getContext() );
+    sofa::simulation::common::MappingGraphMechanicalOperations mop( params, this->getContext() );
     MultiVecCoord pos(&vop, core::vec_id::write_access::position );
     MultiVecDeriv vel(&vop, core::vec_id::write_access::velocity );
     MultiVecDeriv f(&vop, core::vec_id::write_access::force );
@@ -126,7 +128,7 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa::
         SCOPED_TIMER("ComputeForce");
         mop->setImplicit(true); // this solver is implicit
         // compute the net forces at the beginning of the time step
-        mop.computeForce(f);                                                               //f = Kx + Bv
+        mop.computeForce(m_mappingGraph, f, true, true, nullptr);
 
         msg_info() << "initial f = " << f;
     }
@@ -147,7 +149,7 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa::
             msg_info() << "f = " << f;
 
             // add the change of force due to stiffness + Rayleigh damping
-            mop.addMBKv(b, core::MatricesFactors::M(-d_rayleighMass.getValue()),
+            mop.addMBKv(m_mappingGraph, b, core::MatricesFactors::M(-d_rayleighMass.getValue()),
                         core::MatricesFactors::B(0),
                         core::MatricesFactors::K(h * tr + d_rayleighStiffness.getValue())); // b =  f + ( rm M + (h+rs) K ) v
 
@@ -157,7 +159,7 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa::
 
         msg_info() << "b = " << b;
 
-        mop.projectResponse(b);                                   // b is projected to the constrained space
+        mop.projectResponse(m_mappingGraph, b);                                   // b is projected to the constrained space
 
         msg_info() << "projected b = " << b;
     }

Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.h

@@ -22,8 +22,8 @@
 #pragma once
 #include <sofa/component/odesolver/backward/config.h>
 #include <sofa/core/behavior/LinearSolverAccessor.h>
-
 #include <sofa/core/behavior/OdeSolver.h>
+#include <sofa/simulation/MappingGraph.h>
 
 namespace sofa::simulation::common
 {
@@ -178,6 +178,8 @@ class SOFA_COMPONENT_ODESOLVER_BACKWARD_API EulerImplicitSolver :
     void reallocSolutionVector(sofa::simulation::common::VectorOperations* vop);
     void reallocRightHandSideVector(sofa::simulation::common::VectorOperations* vop);
     void reallocResidualVector(sofa::simulation::common::VectorOperations* vop);
+
+    sofa::simulation::MappingGraph m_mappingGraph;
 };
 
 } // namespace sofa::component::odesolver::backward

Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.cpp

@@ -27,10 +27,8 @@
 #include <sofa/helper/AdvancedTimer.h>
 #include <sofa/helper/ScopedAdvancedTimer.h>
 #include <sofa/simulation/MappingGraph.h>
-#include <sofa/simulation/MechanicalOperations.h>
+#include <sofa/simulation/MappingGraphMechanicalOperations.h>
 #include <sofa/simulation/VectorOperations.h>
-#include <sofa/simulation/mechanicalvisitor/MechanicalGetNonDiagonalMassesCountVisitor.h>
-using sofa::simulation::mechanicalvisitor::MechanicalGetNonDiagonalMassesCountVisitor;
 
 //#define SOFA_NO_VMULTIOP
 
@@ -66,10 +64,12 @@ void EulerExplicitSolver::solve(const core::ExecParams* params,
 
     SCOPED_TIMER("EulerExplicitSolve");
 
+    m_mappingGraph.build(this->getContext());
+
     // Create the vector and mechanical operations tools. These are used to execute special operations (multiplication,
     // additions, etc.) on multi-vectors (a vector that is stored in different buffers inside the mechanical objects)
     sofa::simulation::common::VectorOperations vop( params, this->getContext() );
-    sofa::simulation::common::MechanicalOperations mop( params, this->getContext() );
+    sofa::simulation::common::MappingGraphMechanicalOperations mop( params, this->getContext() );
 
     // Let the mechanical operations know that the current solver is explicit. This will be propagated back to the
     // force fields during the addForce and addKToMatrix phase. Force fields use this information to avoid
@@ -121,7 +121,7 @@ void EulerExplicitSolver::solve(const core::ExecParams* params,
 }
 
 void EulerExplicitSolver::updateState(sofa::simulation::common::VectorOperations* vop,
-                                      sofa::simulation::common::MechanicalOperations* mop,
+                                      sofa::simulation::common::MappingGraphMechanicalOperations* mop,
                                       sofa::core::MultiVecCoordId xResult,
                                       sofa::core::MultiVecDerivId vResult,
                                       const sofa::core::behavior::MultiVecDeriv& acc,
@@ -267,7 +267,7 @@ void EulerExplicitSolver::init()
     d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid);
 }
 
-void EulerExplicitSolver::addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt, core::MultiVecDerivId v)
+void EulerExplicitSolver::addSeparateGravity(sofa::simulation::common::MappingGraphMechanicalOperations* mop, SReal dt, core::MultiVecDerivId v)
 {
     SCOPED_TIMER("addSeparateGravity");
 
@@ -277,17 +277,17 @@ void EulerExplicitSolver::addSeparateGravity(sofa::simulation::common::Mechanica
     mop->addSeparateGravity(dt, v);
 }
 
-void EulerExplicitSolver::computeForce(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId f)
+void EulerExplicitSolver::computeForce(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId f) const
 {
     SCOPED_TIMER("ComputeForce");
 
     // 1. Clear the force vector (F := 0)
     // 2. Go down in the current context tree calling addForce on every forcefields
     // 3. Go up from the current context tree leaves calling applyJT on every mechanical mappings
-    mop->computeForce(f);
+    mop->computeForce(m_mappingGraph, f, true, true, nullptr);
 }
 
-void EulerExplicitSolver::computeAcceleration(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc, core::ConstMultiVecDerivId f)
+void EulerExplicitSolver::computeAcceleration(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId acc, core::ConstMultiVecDerivId f)
 {
     SCOPED_TIMER("AccFromF");
 
@@ -299,25 +299,25 @@ void EulerExplicitSolver::computeAcceleration(sofa::simulation::common::Mechanic
     mop->accFromF(acc, f);
 }
 
-void EulerExplicitSolver::projectResponse(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId vecId)
+void EulerExplicitSolver::projectResponse(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId vecId) const
 {
     SCOPED_TIMER("projectResponse");
 
     // Calls the "projectResponse" method of every BaseProjectiveConstraintSet objects found in the
     // current context tree. An example of such constraint set is the FixedProjectiveConstraint. In this case,
     // it will set to 0 every row (i, _) of the input vector for the ith degree of freedom.
-    mop->projectResponse(vecId);
+    mop->projectResponse(m_mappingGraph, vecId);
 }
 
-void EulerExplicitSolver::solveConstraints(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc)
+void EulerExplicitSolver::solveConstraints(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId acc)
 {
     SCOPED_TIMER("solveConstraint");
 
     // Calls "solveConstraint" method of every ConstraintSolver objects found in the current context tree.
     mop->solveConstraint(acc, core::ConstraintOrder::ACC);
 }
 
-void EulerExplicitSolver::assembleSystemMatrix(sofa::simulation::common::MechanicalOperations* mop) const
+void EulerExplicitSolver::assembleSystemMatrix(sofa::simulation::common::MappingGraphMechanicalOperations* mop) const
 {
     SCOPED_TIMER("MBKBuild");
 
@@ -342,52 +342,9 @@ void EulerExplicitSolver::solveSystem(core::MultiVecDerivId solution, core::Mult
     l_linearSolver->getLinearSystem()->dispatchSystemSolution(solution);
 }
 
-class MappedMassVisitor final : public simulation::BaseMechanicalVisitor
-{
-public:
-    MappedMassVisitor(const sofa::core::ExecParams* params, sofa::simulation::MappingGraph* mappingGraph)
-        : BaseMechanicalVisitor(params), m_mappingGraph(mappingGraph)
-    {}
-
-    Result fwdMass(simulation::Node*, sofa::core::behavior::BaseMass* mass) override
-    {
-        if (mass && m_mappingGraph)
-        {
-            m_hasMappedMass |= m_mappingGraph->hasAnyMappingInput(mass);
-        }
-        return Result::RESULT_CONTINUE;
-    }
-
-    [[nodiscard]] bool hasMappedMass() const { return m_hasMappedMass; }
-
-private:
-    sofa::simulation::MappingGraph* m_mappingGraph { nullptr };
-    bool m_hasMappedMass { false };
-};
-
-class AllOfMassesAreDiagonalVisitor final : public simulation::BaseMechanicalVisitor
-{
-public:
-    using simulation::BaseMechanicalVisitor::BaseMechanicalVisitor;
-    Result fwdMass(simulation::Node*, sofa::core::behavior::BaseMass* mass) override
-    {
-        if (mass)
-        {
-            m_allMassesAreDiagonal &= mass->isDiagonal();
-        }
-        return Result::RESULT_CONTINUE;
-    }
-
-    [[nodiscard]] bool areAllMassesAreDiagonal() const { return m_allMassesAreDiagonal; }
-
-private:
-    bool m_allMassesAreDiagonal { true };
-};
-
-bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams* params)
+bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams* params) const
 {
-    sofa::simulation::MappingGraph mappingGraph;
-    mappingGraph.build(params, this->getContext());
+    SOFA_UNUSED(params);
 
     // To achieve a diagonal global mass matrix in this system:
     // 1) Each individual mass matrix must itself be diagonal.
@@ -397,15 +354,23 @@ bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams
     // we identify a mapped mass, we cannot guarantee the global mass matrix will remain diagonal.
     // Moreover, computing the inverse of a mapped mass would require a complex API. Therefore, this
     // case is not supported without assembling the global mass matrix.
-    MappedMassVisitor mappedMassVisitor(params, &mappingGraph);
-    mappedMassVisitor.execute(this->getContext());
-    if (mappedMassVisitor.hasMappedMass())
+    bool hasMappedMass = false;
+    m_mappingGraph.algorithms.traverseComponentGroups_([&hasMappedMass](const sofa::core::behavior::BaseMass&)
+    {
+        hasMappedMass = true;
+    }, simulation::VisitorApplication::ONLY_MAPPED_NODES);
+    if (hasMappedMass)
+    {
         return false;
+    }
 
     // At this stage, we know that we don't have any mapped mass. We can check if they are all diagonal.
-    AllOfMassesAreDiagonalVisitor allOfMassesAreDiagonalVisitor(params);
-    allOfMassesAreDiagonalVisitor.execute(this->getContext());
-    return allOfMassesAreDiagonalVisitor.areAllMassesAreDiagonal();
+    bool areAllMassesDiagonal = true;
+    m_mappingGraph.algorithms.traverseComponentGroups_([&areAllMassesDiagonal](const sofa::core::behavior::BaseMass& mass)
+    {
+        areAllMassesDiagonal &= mass.isDiagonal();
+    });
+    return areAllMassesDiagonal;
 }
 
 } // namespace sofa::component::odesolver::forward

Sofa/Component/ODESolver/Forward/src/sofa/component/odesolver/forward/EulerExplicitSolver.h

@@ -25,10 +25,11 @@
 #include <sofa/core/behavior/OdeSolver.h>
 #include <sofa/core/behavior/LinearSolver.h>
 #include <sofa/core/behavior/MultiVec.h>
+#include <sofa/simulation/MappingGraph.h>
 
 namespace sofa::simulation::common
 {
-class MechanicalOperations;
+class MappingGraphMechanicalOperations;
 class VectorOperations;
 }
 
@@ -96,35 +97,38 @@ class SOFA_COMPONENT_ODESOLVER_FORWARD_API EulerExplicitSolver : public sofa::co
     /// Update state variable (new position and velocity) based on the computed acceleration
     /// The update takes constraints into account
     void updateState(sofa::simulation::common::VectorOperations* vop,
-                     sofa::simulation::common::MechanicalOperations* mop,
+                     sofa::simulation::common::MappingGraphMechanicalOperations* mop,
                      sofa::core::MultiVecCoordId xResult,
                      sofa::core::MultiVecDerivId vResult,
                      const sofa::core::behavior::MultiVecDeriv& acc,
                      SReal dt) const;
 
     /// Gravity times time step size is added to the velocity for some masses
     /// v += g * dt
-    static void addSeparateGravity(sofa::simulation::common::MechanicalOperations* mop, SReal dt, core::MultiVecDerivId v);
+    static void addSeparateGravity(sofa::simulation::common::MappingGraphMechanicalOperations* mop, SReal dt, core::MultiVecDerivId v);
 
     /// Assemble the force vector (right-hand side of the equation)
-    static void computeForce(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId f);
+    void computeForce(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId f) const;
 
     /// Compute the acceleration from the force and the inverse of the mass
     /// acc = M^-1 * f
-    static void computeAcceleration(sofa::simulation::common::MechanicalOperations* mop,
+    static void computeAcceleration(sofa::simulation::common::MappingGraphMechanicalOperations* mop,
                                     core::MultiVecDerivId acc,
                                     core::ConstMultiVecDerivId f);
 
     /// Apply projective constraints, such as FixedProjectiveConstraint
-    static void projectResponse(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId vecId);
+    void projectResponse(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId vecId) const;
 
-    static void solveConstraints(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc);
+    static void solveConstraints(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId acc);
 
-    void assembleSystemMatrix(sofa::simulation::common::MechanicalOperations* mop) const;
+    void assembleSystemMatrix(sofa::simulation::common::MappingGraphMechanicalOperations* mop) const;
 
     void solveSystem(core::MultiVecDerivId solution, core::MultiVecDerivId rhs) const;
 
-    bool isMassMatrixTriviallyInvertible(const core::ExecParams* params);
+    bool isMassMatrixTriviallyInvertible(const core::ExecParams* params) const;
+
+
+    simulation::MappingGraph m_mappingGraph;
 };
 
 } // namespace sofa::component::odesolver::forward

Sofa/framework/Simulation/Core/CMakeLists.txt

@@ -67,6 +67,7 @@ set(HEADER_FILES
     ${SRC_ROOT}/SceneCheckRegistry.h
     ${SRC_ROOT}/SceneCheckMainRegistry.h
     ${SRC_ROOT}/MappingGraph.h
+    ${SRC_ROOT}/MappingGraphMechanicalOperations.h
 
     ${SRC_ROOT}/events/BuildConstraintSystemEndEvent.h
     ${SRC_ROOT}/events/SimulationInitDoneEvent.h
@@ -76,6 +77,15 @@ set(HEADER_FILES
     ${SRC_ROOT}/events/SimulationStopEvent.h
     ${SRC_ROOT}/events/SolveConstraintSystemEndEvent.h
 
+    ${SRC_ROOT}/mappinggraph/BaseMappingGraphNode.h
+    ${SRC_ROOT}/mappinggraph/CallableVisitor.h
+    ${SRC_ROOT}/mappinggraph/ComponentGroupMappingGraphNode.h
+    ${SRC_ROOT}/mappinggraph/ExportDot.h
+    ${SRC_ROOT}/mappinggraph/MappingGraphAlgorithms.h
+    ${SRC_ROOT}/mappinggraph/MappingGraphNode.h
+    ${SRC_ROOT}/mappinggraph/MappingGraphVisitor.h
+    ${SRC_ROOT}/mappinggraph/VisitorApplication.h
+
     ${SRC_ROOT}/mechanicalvisitor/MechanicalAccFromFVisitor.h
     ${SRC_ROOT}/mechanicalvisitor/MechanicalAccumulateJacobian.h
     ${SRC_ROOT}/mechanicalvisitor/MechanicalAccumulateJacobian.h
@@ -170,6 +180,7 @@ set(SOURCE_FILES
     ${SRC_ROOT}/IntegrateBeginEvent.cpp
     ${SRC_ROOT}/IntegrateEndEvent.cpp
     ${SRC_ROOT}/MappingGraph.cpp
+    ${SRC_ROOT}/MappingGraphMechanicalOperations.cpp
     ${SRC_ROOT}/MechanicalOperations.cpp
     ${SRC_ROOT}/MechanicalVPrintVisitor.cpp
     ${SRC_ROOT}/MechanicalVisitor.cpp
@@ -214,6 +225,11 @@ set(SOURCE_FILES
     ${SRC_ROOT}/events/SimulationStopEvent.cpp
     ${SRC_ROOT}/events/SolveConstraintSystemEndEvent.cpp
 
+    ${SRC_ROOT}/mappinggraph/BaseMappingGraphNode.cpp
+    ${SRC_ROOT}/mappinggraph/ExportDot.cpp
+    ${SRC_ROOT}/mappinggraph/MappingGraphAlgorithms.cpp
+    ${SRC_ROOT}/mappinggraph/MappingGraphVisitor.cpp
+
     ${SRC_ROOT}/mechanicalvisitor/MechanicalAccFromFVisitor.cpp
     ${SRC_ROOT}/mechanicalvisitor/MechanicalAccumulateJacobian.cpp
     ${SRC_ROOT}/mechanicalvisitor/MechanicalAccumulateJacobian.cpp