[Simulation] In-place creation of mechanical visitors

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

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

Sofa/framework/Simulation/Core/CMakeLists.txt

@@ -34,6 +34,7 @@ set(HEADER_FILES
     ${SRC_ROOT}/MechanicalOperations.h
     ${SRC_ROOT}/MechanicalVPrintVisitor.h
     ${SRC_ROOT}/MechanicalVisitor.h
+    ${SRC_ROOT}/MechanicalVisitorCreator.h
     ${SRC_ROOT}/MutationListener.h
     ${SRC_ROOT}/Node.h
     ${SRC_ROOT}/Node.inl

Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalVisitorCreator.h

@@ -0,0 +1,136 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program is free software; you can redistribute it and/or modify it     *
+* under the terms of the GNU Lesser General Public License as published by    *
+* the Free Software Foundation; either version 2.1 of the License, or (at     *
+* your option) any later version.                                             *
+*                                                                             *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details.                                                           *
+*                                                                             *
+* You should have received a copy of the GNU Lesser General Public License    *
+* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: [email protected]                             *
+******************************************************************************/
+#pragma once
+
+#include <sofa/simulation/MechanicalVisitor.h>
+
+
+namespace sofa::simulation
+{
+
+template<class Callable>
+struct VisitorCallable
+{
+    Callable m_callable;
+    constexpr explicit VisitorCallable(const Callable& callable) : m_callable(callable) {}
+    constexpr explicit VisitorCallable(Callable&& callable) : m_callable(std::forward<Callable>(callable)) {}
+};
+
+template<class Callable, class TVisitedObject>
+struct TopDownCallable : VisitorCallable<Callable>
+{
+    using VisitorCallable<Callable>::VisitorCallable;
+    using VisitedObject = TVisitedObject;
+};
+
+template<class Callable, class TVisitedObject>
+struct BottomUpCallable : VisitorCallable<Callable>
+{
+    using VisitorCallable<Callable>::VisitorCallable;
+    using VisitedObject = TVisitedObject;
+};
+
+#define MAKE_CALLABLE(visitorCallable, Base, visitedObject) \
+    template<class Callable> struct visitorCallable : Base<Callable, visitedObject> { \
+    constexpr explicit visitorCallable(const Callable& callable) : Base<Callable, visitedObject>(callable) {} \
+    constexpr explicit visitorCallable(Callable&& callable) : Base<Callable, visitedObject>(std::forward<Callable>(callable)) {} \
+    };
+
+#define MAKE_CREATORS(className, visitorCallable)\
+    template<class Callable>\
+    className<MechanicalVisitor, Callable> makeMechanicalVisitor(const sofa::core::MechanicalParams* mparams, const visitorCallable<Callable>& callable)\
+    {\
+        return className<MechanicalVisitor, Callable>(mparams, callable); \
+    }\
+    template<class Callable, class... Tail>\
+    auto makeMechanicalVisitor(const sofa::core::MechanicalParams* mparams, const visitorCallable<Callable>& callable, const Tail&... tail)\
+    {\
+        const className<decltype(makeMechanicalVisitor(mparams, tail...)), Callable> r(mparams, callable, tail...); \
+        return r;\
+    }
+
+#define MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(className, visitedObject, functionName, visitorCallable) \
+    MAKE_CALLABLE(visitorCallable, TopDownCallable, visitedObject)\
+    template<class Base, class Callable>\
+    class className : public Base\
+    {\
+    public:\
+        template<class... OtherCallables>\
+        className(const sofa::core::MechanicalParams* mparams, const visitorCallable<Callable>& callable, const OtherCallables&... others)\
+            : Base(mparams, others...), m_callable(callable.m_callable)\
+        {}\
+        \
+        Visitor::Result functionName(Node* node, visitedObject* solver) override\
+        {\
+            return m_callable(node, solver);\
+        }\
+\
+    protected:\
+        Callable m_callable;\
+    };\
+    MAKE_CREATORS(className, visitorCallable)
+
+#define MAKE_BOTTOM_UP_MECHANICAL_VISITOR_TYPE(className, visitedObject, functionName, visitorCallable) \
+    MAKE_CALLABLE(visitorCallable, TopDownCallable, visitedObject)\
+    template<class Base, class Callable>\
+    class className : public Base\
+    {\
+    public:\
+        template<class... OtherCallables>\
+        className(const sofa::core::MechanicalParams* mparams, const visitorCallable<Callable>& callable, const OtherCallables&... others)\
+            : Base(mparams, others...), m_callable(callable.m_callable)\
+        {}\
+        \
+        void functionName(Node* node, visitedObject* solver) override\
+        {\
+            m_callable(node, solver);\
+        }\
+    \
+    protected:\
+        Callable m_callable;\
+    };\
+    MAKE_CREATORS(className, visitorCallable)
+
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorOdeSolver, core::behavior::OdeSolver, fwdOdeSolver, TopDownOdeSolverCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorConstraintSolver, core::behavior::ConstraintSolver, fwdConstraintSolver, TopDownConstraintSolverCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorMechanicalMapping, core::BaseMapping, fwdMechanicalMapping, TopDownMechanicalMappingCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorMappedMechanicalState, sofa::core::behavior::BaseMechanicalState, fwdMappedMechanicalState, TopDownMappedMechanicalStateCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorMechanicalState, sofa::core::behavior::BaseMechanicalState, fwdMechanicalState, TopDownMechanicalStateCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorMass, sofa::core::behavior::BaseMass, fwdMass, TopDownMassCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorForceField, sofa::core::behavior::BaseForceField, fwdForceField, TopDownForceFieldCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorInteractionForceField, sofa::core::behavior::BaseInteractionForceField, fwdInteractionForceField, TopDownInteractionForceFieldCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorProjectiveConstraintSet, sofa::core::behavior::BaseProjectiveConstraintSet, fwdProjectiveConstraintSet, TopDownProjectiveConstraintSetCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorConstraintSet, sofa::core::behavior::BaseConstraintSet, fwdConstraintSet, TopDownConstraintSetCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorInteractionProjectiveConstraintSet, sofa::core::behavior::BaseInteractionProjectiveConstraintSet, fwdInteractionProjectiveConstraintSet, TopDownInteractionProjectiveConstraintSetCallable)
+MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(TopDownMechanicalVisitorInteractionConstraint, sofa::core::behavior::BaseInteractionConstraint, fwdInteractionConstraint, TopDownInteractionInteractionConstraintCallable)
+
+
+MAKE_BOTTOM_UP_MECHANICAL_VISITOR_TYPE(BottomUpMechanicalVisitorMechanicalState, core::behavior::BaseMechanicalState, bwdMechanicalState, BottomUpMechanicalStateCallable)
+MAKE_BOTTOM_UP_MECHANICAL_VISITOR_TYPE(BottomUpMechanicalVisitorMappedMechanicalState, core::behavior::BaseMechanicalState, bwdMappedMechanicalState, BottomUpMappedMechanicalStateCallable)
+MAKE_BOTTOM_UP_MECHANICAL_VISITOR_TYPE(BottomUpMechanicalVisitorMechanicalMapping, core::BaseMapping, bwdMechanicalMapping, BottomUpMechanicalMappingCallable)
+MAKE_BOTTOM_UP_MECHANICAL_VISITOR_TYPE(BottomUpMechanicalVisitorOdeSolver, core::behavior::OdeSolver, bwdOdeSolver, BottomUpOdeSolverCallable)
+MAKE_BOTTOM_UP_MECHANICAL_VISITOR_TYPE(BottomUpMechanicalVisitorConstraintSolver, core::behavior::ConstraintSolver, bwdConstraintSolver, BottomUpConstraintSolverCallable)
+MAKE_BOTTOM_UP_MECHANICAL_VISITOR_TYPE(BottomUpMechanicalVisitorProjectiveConstraintSet, core::behavior::BaseProjectiveConstraintSet, bwdProjectiveConstraintSet, BottomUpProjectiveConstraintSetCallable)
+MAKE_BOTTOM_UP_MECHANICAL_VISITOR_TYPE(BottomUpMechanicalVisitorConstraintSet, core::behavior::BaseConstraintSet, bwdConstraintSet, BottomUpConstraintSetCallable)
+
+
+}