From 0ae3e7e51e218ecce1bdbba7c4e78f5c4e2410ca Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Mon, 9 Dec 2024 13:09:27 +0100 Subject: [PATCH 1/2] [Simulation] In-place creation of mechanical visitors --- Sofa/framework/Simulation/Core/CMakeLists.txt | 1 + .../simulation/MechanicalVisitorCreator.h | 136 ++++++++++++++++++ 2 files changed, 137 insertions(+) create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalVisitorCreator.h diff --git a/Sofa/framework/Simulation/Core/CMakeLists.txt b/Sofa/framework/Simulation/Core/CMakeLists.txt index bca202e9d0c..c37b1b564ed 100644 --- a/Sofa/framework/Simulation/Core/CMakeLists.txt +++ b/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 diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalVisitorCreator.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalVisitorCreator.h new file mode 100644 index 00000000000..d2dc23f886b --- /dev/null +++ b/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 . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include + + +namespace sofa::simulation +{ + +template +struct VisitorCallable +{ + Callable m_callable; + constexpr explicit VisitorCallable(const Callable& callable) : m_callable(callable) {} + constexpr explicit VisitorCallable(Callable&& callable) : m_callable(std::forward(callable)) {} +}; + +template +struct TopDownCallable : VisitorCallable +{ + using VisitorCallable::VisitorCallable; + using VisitedObject = TVisitedObject; +}; + +template +struct BottomUpCallable : VisitorCallable +{ + using VisitorCallable::VisitorCallable; + using VisitedObject = TVisitedObject; +}; + +#define MAKE_CALLABLE(visitorCallable, Base, visitedObject) \ + template struct visitorCallable : Base { \ + constexpr explicit visitorCallable(const Callable& callable) : Base(callable) {} \ + constexpr explicit visitorCallable(Callable&& callable) : Base(std::forward(callable)) {} \ + }; + +#define MAKE_CREATORS(className, visitorCallable)\ + template\ + className makeMechanicalVisitor(const sofa::core::MechanicalParams* mparams, const visitorCallable& callable)\ + {\ + return className(mparams, callable); \ + }\ + template\ + auto makeMechanicalVisitor(const sofa::core::MechanicalParams* mparams, const visitorCallable& callable, const Tail&... tail)\ + {\ + const className r(mparams, callable, tail...); \ + return r;\ + } + +#define MAKE_TOP_DOWN_MECHANICAL_VISITOR_TYPE(className, visitedObject, functionName, visitorCallable) \ + MAKE_CALLABLE(visitorCallable, TopDownCallable, visitedObject)\ + template\ + class className : public Base\ + {\ + public:\ + template\ + className(const sofa::core::MechanicalParams* mparams, const visitorCallable& 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 className : public Base\ + {\ + public:\ + template\ + className(const sofa::core::MechanicalParams* mparams, const visitorCallable& 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) + + +} From f6b577baf6c234f7c269724c129705ef09dd65d0 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Mon, 9 Dec 2024 13:09:34 +0100 Subject: [PATCH 2/2] 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 #include #include +#include #include #include #include +#include #include 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