From 363d7064b18841a4ef8314580f5c620af8620ae1 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 15 Nov 2024 14:49:16 +0100 Subject: [PATCH 1/8] [Core] Modern visitor creation --- Sofa/framework/Core/CMakeLists.txt | 1 + .../src/sofa/core/objectmodel/BaseContext.h | 5 + .../sofa/core/objectmodel/VisitorDirection.h | 95 +++++++++++++++++++ .../Core/src/sofa/simulation/Node.cpp | 60 ++++++++++++ .../Core/src/sofa/simulation/Node.h | 6 ++ .../Simulation/Graph/test/Node_test.cpp | 94 ++++++++++++++++++ 6 files changed, 261 insertions(+) create mode 100644 Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h diff --git a/Sofa/framework/Core/CMakeLists.txt b/Sofa/framework/Core/CMakeLists.txt index e490b7af3bc..33d65d27c58 100644 --- a/Sofa/framework/Core/CMakeLists.txt +++ b/Sofa/framework/Core/CMakeLists.txt @@ -169,6 +169,7 @@ set(HEADER_FILES ${SRC_ROOT}/objectmodel/TagSet.h ${SRC_ROOT}/objectmodel/vectorData.h ${SRC_ROOT}/objectmodel/vectorLinks.h + ${SRC_ROOT}/objectmodel/VisitorDirection.h ${SRC_ROOT}/topology/BaseMeshTopology.h ${SRC_ROOT}/topology/BaseTopology.h ${SRC_ROOT}/topology/BaseTopologyData.h diff --git a/Sofa/framework/Core/src/sofa/core/objectmodel/BaseContext.h b/Sofa/framework/Core/src/sofa/core/objectmodel/BaseContext.h index 6163cb1a95e..a4788ee7151 100644 --- a/Sofa/framework/Core/src/sofa/core/objectmodel/BaseContext.h +++ b/Sofa/framework/Core/src/sofa/core/objectmodel/BaseContext.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace sofa::simulation { @@ -387,6 +388,10 @@ class SOFA_CORE_API BaseContext : public virtual Base /// Propagate an event virtual void propagateEvent( const core::ExecParams* params, Event* ); + virtual void accept(const TopDownVisitor& visitor) {} + virtual void accept(const BottomUpVisitor& visitor) {} + virtual void accept(const TopDownVisitor& topDownVisitor, const BottomUpVisitor& bottomUpVisitor) {} + /// @} diff --git a/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h b/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h new file mode 100644 index 00000000000..f2177c2a5a1 --- /dev/null +++ b/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h @@ -0,0 +1,95 @@ +/****************************************************************************** +* 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::core::objectmodel +{ + +struct DirectionalVisitor +{ + virtual ~DirectionalVisitor() = default; + DirectionalVisitor() = default; + virtual void operator()(sofa::core::behavior::OdeSolver*) const {} + virtual void operator()(sofa::core::behavior::ConstraintSolver*) const {} + virtual void operator()(sofa::core::BaseMapping*) const {} + virtual void operator()(sofa::core::behavior::BaseMechanicalState*) const {} + virtual void operator()(sofa::core::behavior::BaseMass*) const {} + virtual void operator()(sofa::core::behavior::BaseForceField*) const {} + virtual void operator()(sofa::core::behavior::BaseInteractionForceField*) const {} + virtual void operator()(sofa::core::behavior::BaseProjectiveConstraintSet*) const {} + virtual void operator()(sofa::core::behavior::BaseConstraintSet*) const {} + virtual void operator()(sofa::core::behavior::BaseInteractionProjectiveConstraintSet*) const {} + virtual void operator()(sofa::core::behavior::BaseInteractionConstraint*) const {} +}; + +struct SOFA_CORE_API TopDownVisitor : DirectionalVisitor +{ + TopDownVisitor() = default; +}; +struct SOFA_CORE_API BottomUpVisitor : DirectionalVisitor +{ + BottomUpVisitor() = default; +}; + +inline TopDownVisitor topDownVisitor {}; +inline BottomUpVisitor bottomUpVisitor {}; + + +template>> +struct SpecializedVisitor : public virtual VisitorDirection +{ + Func m_specializedFunction; + + SpecializedVisitor(const VisitorDirection& visitor, const Func& massFunction) + : VisitorDirection(visitor), m_specializedFunction(massFunction) {} + + void operator()(VisitedObject* object) const override + { + m_specializedFunction(object); + } +}; + +#define DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(ObjectType) \ +template>> \ +SpecializedVisitor operator|( \ + const VisitorDirection& input, \ + const Func& f) \ +{ \ + return {input, f}; \ +} + +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::behavior::OdeSolver) +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::behavior::ConstraintSolver) +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::BaseMapping) +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::behavior::BaseMechanicalState) +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::behavior::BaseMass) +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::behavior::BaseForceField) +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::behavior::BaseInteractionForceField) +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::behavior::BaseProjectiveConstraintSet) +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::behavior::BaseConstraintSet) +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::behavior::BaseInteractionProjectiveConstraintSet) +DEFINE_PIPE_OPERATOR_FOR_OBJECT_TYPE(sofa::core::behavior::BaseInteractionConstraint) + +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.cpp index f3c4f3d34ec..d1e63388d1c 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.cpp @@ -904,6 +904,66 @@ void Node::executeVisitor(Visitor* action, bool precomputedOrder) } } +void Node::visitLocalNode(const sofa::core::objectmodel::DirectionalVisitor& visitor) +{ + const auto visitContainer = [&visitor](auto& container) + { + for (auto& obj : container) + { + if (obj) + { + (visitor)(obj); + } + } + }; + + visitContainer(solver); + visitContainer(constraintSolver); + visitContainer(mapping); + + if (mechanicalState) + { + (visitor)(mechanicalState.get()); + } + if (mass) + { + (visitor)(mass.get()); + } + + visitContainer(forceField); + visitContainer(interactionForceField); + visitContainer(projectiveConstraintSet); + visitContainer(constraintSet); +} + +template +void visitChildren(Node& self, const Visitor& visitor) +{ + for (const auto& c : self.child) + { + c->accept(visitor); + } +} + +void Node::accept(const sofa::core::objectmodel::TopDownVisitor& visitor) +{ + visitLocalNode(visitor); + visitChildren(*this, visitor); +} + +void Node::accept(const sofa::core::objectmodel::BottomUpVisitor& visitor) +{ + visitChildren(*this, visitor); + visitLocalNode(visitor); +} + +void Node::accept(const sofa::core::objectmodel::TopDownVisitor& topDownVisitor, + const sofa::core::objectmodel::BottomUpVisitor& bottomUpVisitor) +{ + accept(topDownVisitor); + accept(bottomUpVisitor); +} + /// Propagate an event void Node::propagateEvent(const core::ExecParams* params, core::objectmodel::Event* event) { diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.h index 0f6d4aa7d54..990e9ffc910 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.h @@ -218,6 +218,10 @@ class SOFA_SIMULATION_CORE_API Node : public sofa::core::objectmodel::BaseNode, /// Possible optimization with traversal precomputation, not mandatory and does nothing by default virtual void precomputeTraversalOrder( const sofa::core::ExecParams* ) {} + void accept(const sofa::core::objectmodel::TopDownVisitor& visitor) override; + void accept(const sofa::core::objectmodel::BottomUpVisitor& visitor) override; + void accept(const sofa::core::objectmodel::TopDownVisitor& topDownVisitor, const sofa::core::objectmodel::BottomUpVisitor& bottomUpVisitor) override; + /// @} template @@ -554,6 +558,8 @@ class SOFA_SIMULATION_CORE_API Node : public sofa::core::objectmodel::BaseNode, virtual void notifyEndAddSlave(sofa::core::objectmodel::BaseObject* master, sofa::core::objectmodel::BaseObject* slave) const; virtual void notifyEndRemoveSlave(sofa::core::objectmodel::BaseObject* master, sofa::core::objectmodel::BaseObject* slave) const; + void visitLocalNode(const sofa::core::objectmodel::DirectionalVisitor& visitor); + protected: BaseContext* _context; diff --git a/Sofa/framework/Simulation/Graph/test/Node_test.cpp b/Sofa/framework/Simulation/Graph/test/Node_test.cpp index bb07c4221c1..4cdc6f09f97 100644 --- a/Sofa/framework/Simulation/Graph/test/Node_test.cpp +++ b/Sofa/framework/Simulation/Graph/test/Node_test.cpp @@ -28,6 +28,9 @@ using namespace sofa::simpleapi ; #include "Node_test.h" #include +#include +#include + namespace sofa { @@ -177,6 +180,97 @@ TEST(Node_test, getObjectsStdUnorderedSet) EXPECT_NE(objects.find(B.get()), objects.end()); } +TEST(NodeVisitor_test, EmptyNodeEmptyVisitor) +{ + const Node::SPtr root = sofa::simpleapi::createNode("A"); + root->accept(sofa::core::objectmodel::topDownVisitor); +} + +TEST(NodeVisitor_test, EmptyNodeCounterVisitor) +{ + const Node::SPtr root = sofa::simpleapi::createNode("A"); + + std::size_t counter {}; + + auto visitor = sofa::core::objectmodel::topDownVisitor + | [&counter](core::BaseMapping*){ counter++; }; + root->accept(visitor); + + EXPECT_EQ(0, counter); +} + +TEST(NodeVisitor_test, OneMassCounterVisitor) +{ + const Node::SPtr root = sofa::simpleapi::createNode("A"); + const auto mass = core::objectmodel::New>(); + root->addObject(mass); + + ASSERT_TRUE(root->mass.get() != nullptr); + + std::size_t counter {}; + root->accept(sofa::core::objectmodel::topDownVisitor + | [&counter](core::behavior::BaseMass*){ counter++; }); + + EXPECT_EQ(1, counter); +} + +TEST(NodeVisitor_test, OneMassInChildCounterVisitor) +{ + const Node::SPtr root = sofa::simpleapi::createNode("root"); + const Node::SPtr child = sofa::simpleapi::createChild(root, "child"); + const auto mass = core::objectmodel::New>(); + child->addObject(mass); + + ASSERT_TRUE(child->mass.get() != nullptr); + + std::size_t counter {}; + root->accept(sofa::core::objectmodel::topDownVisitor + | [&counter](core::behavior::BaseMass*){ counter++; }); + + EXPECT_EQ(1, counter); +} + +TEST(NodeVisitor_test, OneMassOneStateInChildCounterVisitor) +{ + const Node::SPtr root = sofa::simpleapi::createNode("root"); + const Node::SPtr child = sofa::simpleapi::createChild(root, "child"); + const auto mass = core::objectmodel::New>(); + const auto state = core::objectmodel::New>(); + child->addObject(mass); + child->addObject(state); + + ASSERT_TRUE(child->mass.get() != nullptr); + + std::size_t counterMass {}, counterState {}, counter {}; + root->accept(sofa::core::objectmodel::topDownVisitor + | [&counter, &counterMass](core::behavior::BaseMass*){ counter++; counterMass++; } + | [&counter, &counterState](core::behavior::BaseMechanicalState*){ counter++; counterState++; }); + + EXPECT_EQ(2, counter); + EXPECT_EQ(1, counterState); + EXPECT_EQ(1, counterMass); +} + +TEST(NodeVisitor_test, OneMassInChildCounterBothDirectionsVisitor) +{ + const Node::SPtr root = sofa::simpleapi::createNode("root"); + const Node::SPtr child = sofa::simpleapi::createChild(root, "child"); + const auto mass = core::objectmodel::New>(); + child->addObject(mass); + + ASSERT_TRUE(child->mass.get() != nullptr); + + std::size_t counter {}; + root->accept( + sofa::core::objectmodel::topDownVisitor + | [&counter](core::behavior::BaseMass*){ counter++; }, + sofa::core::objectmodel::bottomUpVisitor + | [&counter](core::behavior::BaseMass*){ counter -= 2; }); + + EXPECT_EQ(-1, counter); +} + + }// namespace sofa From 6c2ea489554b4f22736ec19028cca3d2d8a04161 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 15 Nov 2024 16:30:09 +0100 Subject: [PATCH 2/8] examples --- .../odesolver/forward/EulerExplicitSolver.cpp | 4 +- .../sofa/simulation/MechanicalOperations.cpp | 101 ++++++++++++++++-- 2 files changed, 95 insertions(+), 10 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 a1a0ecc83b2..ab952ad5dff 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 @@ -85,7 +85,9 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, computeForce(&mop, f); sofa::Size nbNonDiagonalMasses = 0; - MechanicalGetNonDiagonalMassesCountVisitor(&mop.mparams, &nbNonDiagonalMasses).execute(this->getContext()); + this->getContext()->accept( + sofa::core::objectmodel::topDownVisitor + | [&nbNonDiagonalMasses](sofa::core::behavior::BaseMass* mass){nbNonDiagonalMasses++;}); // Mass matrix is diagonal, solution can thus be found by computing acc = f/m if(nbNonDiagonalMasses == 0.) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp index 9167f002344..dd8882cda64 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp @@ -57,6 +57,8 @@ #include #include +#include +#include using namespace sofa::core; @@ -244,19 +246,44 @@ void MechanicalOperations::accFromF(core::MultiVecDerivId a, core::ConstMultiVec setDx(a); setF(f); - executeVisitor( MechanicalAccFromFVisitor(&mparams, a) ); + ctx->accept( + objectmodel::topDownVisitor + | [this, &a](core::behavior::BaseMass* mass){mass->accFromF(&mparams, a);}); } /// Compute the current force (given the latest propagated position and velocity) void MechanicalOperations::computeForce(core::MultiVecDerivId result, bool clear, bool accumulate) { setF(result); + if (clear) { - executeVisitor( MechanicalResetForceVisitor(&mparams, result, false) ); - //finish(); + ctx->accept(objectmodel::topDownVisitor + | [this, &result](core::behavior::BaseMechanicalState* state) + { + state->resetForce(&mparams, result.getId(state)); + }); } - executeVisitor( MechanicalComputeForceVisitor(&mparams, result, accumulate) ); + + ctx->accept(objectmodel::topDownVisitor + | [this, &result](core::behavior::BaseMechanicalState* state) + { + state->accumulateForce(&mparams, result.getId(state)); + } + | [this, &result](core::behavior::BaseForceField* force) + { + force->addForce(&mparams, result); + }, + + objectmodel::bottomUpVisitor + | [this, &result, accumulate](core::BaseMapping* mapping) + { + if (accumulate) + { + mapping->applyJT(&mparams, result, result); + } + } + ); } @@ -266,10 +293,31 @@ void MechanicalOperations::computeDf(core::MultiVecDerivId df, bool clear, bool setDf(df); if (clear) { - executeVisitor( MechanicalResetForceVisitor(&mparams, df, false) ); - // finish(); + ctx->accept(objectmodel::topDownVisitor + | [this, &df](core::behavior::BaseMechanicalState* state) + { + state->resetForce(&mparams, df.getId(state)); + }); } - executeVisitor( MechanicalComputeDfVisitor( &mparams, df, accumulate) ); + + ctx->accept(objectmodel::topDownVisitor + | [this, &df](core::behavior::BaseForceField* force) + { + force->addDForce(&mparams, df); + }, + + objectmodel::bottomUpVisitor + | [this, &df, accumulate](core::BaseMapping* mapping) + { + if (accumulate) + { + mapping->applyJT(&mparams, df, df); + if( mparams.kFactor() != 0_sreal ) + { + mapping->applyDJT(&mparams, df, df); + } + } + }); } /// Compute the current force delta (given the latest propagated velocity) @@ -299,7 +347,25 @@ void MechanicalOperations::addMBKdx(core::MultiVecDerivId df, SReal m, SReal b, mparams.setBFactor(b); mparams.setKFactor(k); mparams.setMFactor(m); - executeVisitor( MechanicalAddMBKdxVisitor(&mparams, df, accumulate) ); + + ctx->accept(objectmodel::topDownVisitor + | [this, &df](core::behavior::BaseForceField* force) + { + force->addMBKdx(&mparams, df); + }, + + objectmodel::bottomUpVisitor + | [this, &df, accumulate](core::BaseMapping* mapping) + { + if (accumulate) + { + mapping->applyJT(&mparams, df, df); + if( mparams.kFactor() != 0_sreal ) + { + mapping->applyDJT(&mparams, df, df); + } + } + }); } /// accumulate $ df += (m M + b B + k K) velocity $ @@ -317,7 +383,24 @@ void MechanicalOperations::addMBKv(core::MultiVecDerivId df, SReal m, SReal b, S mparams.setKFactor(k); mparams.setMFactor(m); /* useV = true */ - executeVisitor( MechanicalAddMBKdxVisitor(&mparams, df, accumulate) ); + ctx->accept(objectmodel::topDownVisitor + | [this, &df](core::behavior::BaseForceField* force) + { + force->addMBKdx(&mparams, df); + }, + + objectmodel::bottomUpVisitor + | [this, &df, accumulate](core::BaseMapping* mapping) + { + if (accumulate) + { + mapping->applyJT(&mparams, df, df); + if( mparams.kFactor() != 0_sreal ) + { + mapping->applyDJT(&mparams, df, df); + } + } + }); mparams.setDx(dx); } From c6c528e661aa053a8dd8ba896c369a8b87de752b Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 15 Nov 2024 20:53:17 +0100 Subject: [PATCH 3/8] consistency order with BaseMechanicalVisitor --- .../Core/src/sofa/simulation/Node.cpp | 43 ++++++++++++++++--- 1 file changed, 37 insertions(+), 6 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.cpp index d1e63388d1c..ffd9fb86498 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/Node.cpp @@ -912,24 +912,29 @@ void Node::visitLocalNode(const sofa::core::objectmodel::DirectionalVisitor& vis { if (obj) { - (visitor)(obj); + visitor(obj); } } }; visitContainer(solver); - visitContainer(constraintSolver); - visitContainer(mapping); + + if (mechanicalMapping) + { + visitor(mechanicalMapping.get()); + } if (mechanicalState) { - (visitor)(mechanicalState.get()); + visitor(mechanicalState.get()); } + if (mass) { - (visitor)(mass.get()); + visitor(mass.get()); } + visitContainer(constraintSolver); visitContainer(forceField); visitContainer(interactionForceField); visitContainer(projectiveConstraintSet); @@ -954,7 +959,33 @@ void Node::accept(const sofa::core::objectmodel::TopDownVisitor& visitor) void Node::accept(const sofa::core::objectmodel::BottomUpVisitor& visitor) { visitChildren(*this, visitor); - visitLocalNode(visitor); + + const auto visitContainer = [&visitor](auto& container) + { + for (auto& obj : container) + { + if (obj) + { + visitor(obj); + } + } + }; + + visitContainer(projectiveConstraintSet); + visitContainer(constraintSet); + visitContainer(constraintSolver); + + if (mechanicalState) + { + visitor(mechanicalState.get()); + } + + if (mechanicalMapping) + { + visitor(mechanicalMapping.get()); + } + + visitContainer(solver); } void Node::accept(const sofa::core::objectmodel::TopDownVisitor& topDownVisitor, From afee53f1b8a7b0f46d371f09ffb06a70f6f0f7eb Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Sat, 16 Nov 2024 14:57:19 +0100 Subject: [PATCH 4/8] fix test --- .../Core/src/sofa/core/objectmodel/VisitorDirection.h | 4 ++-- Sofa/framework/Simulation/Graph/test/Node_test.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h b/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h index f2177c2a5a1..c73e274edc2 100644 --- a/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h +++ b/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h @@ -61,8 +61,8 @@ struct SpecializedVisitor : public virtual VisitorDirection { Func m_specializedFunction; - SpecializedVisitor(const VisitorDirection& visitor, const Func& massFunction) - : VisitorDirection(visitor), m_specializedFunction(massFunction) {} + SpecializedVisitor(const VisitorDirection& visitor, const Func& specializedFunction) + : VisitorDirection(visitor), m_specializedFunction(specializedFunction) {} void operator()(VisitedObject* object) const override { diff --git a/Sofa/framework/Simulation/Graph/test/Node_test.cpp b/Sofa/framework/Simulation/Graph/test/Node_test.cpp index 4cdc6f09f97..2995f80e775 100644 --- a/Sofa/framework/Simulation/Graph/test/Node_test.cpp +++ b/Sofa/framework/Simulation/Graph/test/Node_test.cpp @@ -255,17 +255,17 @@ TEST(NodeVisitor_test, OneMassInChildCounterBothDirectionsVisitor) { const Node::SPtr root = sofa::simpleapi::createNode("root"); const Node::SPtr child = sofa::simpleapi::createChild(root, "child"); - const auto mass = core::objectmodel::New>(); - child->addObject(mass); + const auto state = core::objectmodel::New>(); + child->addObject(state); - ASSERT_TRUE(child->mass.get() != nullptr); + ASSERT_TRUE(child->mechanicalState.get() != nullptr); std::size_t counter {}; root->accept( sofa::core::objectmodel::topDownVisitor - | [&counter](core::behavior::BaseMass*){ counter++; }, + | [&counter](core::behavior::BaseMechanicalState*){ counter++; }, sofa::core::objectmodel::bottomUpVisitor - | [&counter](core::behavior::BaseMass*){ counter -= 2; }); + | [&counter](core::behavior::BaseMechanicalState*){ counter -= 2; }); EXPECT_EQ(-1, counter); } From 0fc0eca59b6abbb127901ea40fdac5593668f7b4 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Sat, 16 Nov 2024 22:08:07 +0100 Subject: [PATCH 5/8] default behavior for interaction components is to call the function on the non-interaction component --- .../src/sofa/core/objectmodel/VisitorDirection.h | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h b/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h index c73e274edc2..c06f5b77c15 100644 --- a/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h +++ b/Sofa/framework/Core/src/sofa/core/objectmodel/VisitorDirection.h @@ -35,11 +35,20 @@ struct DirectionalVisitor virtual void operator()(sofa::core::behavior::BaseMechanicalState*) const {} virtual void operator()(sofa::core::behavior::BaseMass*) const {} virtual void operator()(sofa::core::behavior::BaseForceField*) const {} - virtual void operator()(sofa::core::behavior::BaseInteractionForceField*) const {} + virtual void operator()(sofa::core::behavior::BaseInteractionForceField* force) const + { + this->operator()((behavior::BaseForceField*)force); + } virtual void operator()(sofa::core::behavior::BaseProjectiveConstraintSet*) const {} virtual void operator()(sofa::core::behavior::BaseConstraintSet*) const {} - virtual void operator()(sofa::core::behavior::BaseInteractionProjectiveConstraintSet*) const {} - virtual void operator()(sofa::core::behavior::BaseInteractionConstraint*) const {} + virtual void operator()(sofa::core::behavior::BaseInteractionProjectiveConstraintSet* constraint) const + { + this->operator()((behavior::BaseProjectiveConstraintSet*)constraint); + } + virtual void operator()(sofa::core::behavior::BaseInteractionConstraint* constraint) const + { + this->operator()((behavior::BaseConstraintSet*)constraint); + } }; struct SOFA_CORE_API TopDownVisitor : DirectionalVisitor From 8abbb70a6b416c93803c7cfe7fc12c15f20505eb Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Sat, 16 Nov 2024 22:21:02 +0100 Subject: [PATCH 6/8] remove unused variable warning --- Sofa/framework/Core/src/sofa/core/objectmodel/BaseContext.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Sofa/framework/Core/src/sofa/core/objectmodel/BaseContext.h b/Sofa/framework/Core/src/sofa/core/objectmodel/BaseContext.h index a4788ee7151..987d1ff175d 100644 --- a/Sofa/framework/Core/src/sofa/core/objectmodel/BaseContext.h +++ b/Sofa/framework/Core/src/sofa/core/objectmodel/BaseContext.h @@ -388,9 +388,9 @@ class SOFA_CORE_API BaseContext : public virtual Base /// Propagate an event virtual void propagateEvent( const core::ExecParams* params, Event* ); - virtual void accept(const TopDownVisitor& visitor) {} - virtual void accept(const BottomUpVisitor& visitor) {} - virtual void accept(const TopDownVisitor& topDownVisitor, const BottomUpVisitor& bottomUpVisitor) {} + virtual void accept(const TopDownVisitor&) {} + virtual void accept(const BottomUpVisitor&) {} + virtual void accept(const TopDownVisitor&, const BottomUpVisitor&) {} /// @} From cda3269ab04c4555be8e98e43b258d19ace70bd2 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Tue, 19 Nov 2024 09:33:45 +0100 Subject: [PATCH 7/8] forgot the condition on diagonal masses --- .../odesolver/forward/EulerExplicitSolver.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 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 ab952ad5dff..77d6fca17be 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,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -87,10 +88,16 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, sofa::Size nbNonDiagonalMasses = 0; this->getContext()->accept( sofa::core::objectmodel::topDownVisitor - | [&nbNonDiagonalMasses](sofa::core::behavior::BaseMass* mass){nbNonDiagonalMasses++;}); + | [&nbNonDiagonalMasses](sofa::core::behavior::BaseMass* mass) + { + if (mass && !mass->isDiagonal()) + { + nbNonDiagonalMasses++; + } + }); // Mass matrix is diagonal, solution can thus be found by computing acc = f/m - if(nbNonDiagonalMasses == 0.) + if(nbNonDiagonalMasses == 0) { // acc = M^-1 * f computeAcceleration(&mop, acc, f); From 7b8af416963035d28dbc3920117dbb3c7e18076f Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Tue, 19 Nov 2024 15:08:05 +0100 Subject: [PATCH 8/8] modern visitor creation (the old ones) --- Sofa/framework/Simulation/Core/CMakeLists.txt | 1 + .../sofa/simulation/MechanicalOperations.cpp | 105 ++++++++++++-- .../simulation/MechanicalVisitorCreator.h | 137 ++++++++++++++++++ 3 files changed, 231 insertions(+), 12 deletions(-) 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/MechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp index dd8882cda64..52146c52bb6 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp @@ -59,6 +59,8 @@ #include #include #include +#include +#include using namespace sofa::core; @@ -159,8 +161,15 @@ void MechanicalOperations::propagateDxAndResetDf(core::MultiVecDerivId dx, core: void MechanicalOperations::propagateX(core::MultiVecCoordId x) { setX(x); - const MechanicalPropagateOnlyPositionVisitor visitor(&mparams, 0.0, x); - executeVisitor( visitor ); + const auto v = sofa::simulation::makeMechanicalVisitor(&mparams, + TopDownMechanicalMappingCallable( + [this, &x](simulation::Node* /*node*/, core::BaseMapping* map) -> Visitor::Result + { + map->apply(&mparams, x, x); + return Visitor::RESULT_CONTINUE; + }) + ); + executeVisitor( v ); } /// Propagate the given velocity through all mappings @@ -201,10 +210,20 @@ void MechanicalOperations::computeEnergy(SReal &kineticEnergy, SReal &potentialE { kineticEnergy = 0; potentialEnergy = 0; - MechanicalComputeEnergyVisitor energyVisitor(&mparams); - executeVisitor(&energyVisitor); - kineticEnergy = energyVisitor.getKineticEnergy(); - potentialEnergy = energyVisitor.getPotentialEnergy(); + + const auto v = makeMechanicalVisitor(&mparams, + TopDownMassCallable([&kineticEnergy](simulation::Node*, const sofa::core::behavior::BaseMass* mass) + { + kineticEnergy += mass->getKineticEnergy(); + return Visitor::RESULT_CONTINUE; + }), + TopDownForceFieldCallable([&potentialEnergy](simulation::Node*, const sofa::core::behavior::BaseForceField* force) + { + potentialEnergy += force->getPotentialEnergy(); + return Visitor::RESULT_CONTINUE; + }) + ); + executeVisitor( v ); } /// Apply projective constraints to the given velocity vector void MechanicalOperations::projectVelocity(core::MultiVecDerivId v, SReal time) @@ -328,10 +347,41 @@ void MechanicalOperations::computeDfV(core::MultiVecDerivId df, bool clear, bool setDf(df); if (clear) { - executeVisitor( MechanicalResetForceVisitor(&mparams, df, false) ); - //finish(); + const auto reset = [this, &df](simulation::Node* /*node*/, core::behavior::BaseMechanicalState* mm) + { + mm->resetForce(&mparams, df.getId(mm)); + return Visitor::RESULT_CONTINUE; + }; + + executeVisitor(makeMechanicalVisitor(&mparams, + TopDownMechanicalStateCallable(reset), + TopDownMappedMechanicalStateCallable(reset) + )); + } + + const auto addDForce = TopDownForceFieldCallable([this, &df](simulation::Node* /*node*/, core::behavior::BaseForceField* ff) + { + ff->addDForce(&mparams, df); + return Visitor::RESULT_CONTINUE; + }); + + if (accumulate) + { + executeVisitor(makeMechanicalVisitor(&mparams, addDForce, + BottomUpMechanicalMappingCallable([this, &df](simulation::Node* /*node*/, core::BaseMapping* map) + { + map->applyJT(&mparams, df, df); // apply material stiffness: variation of force below the mapping + if( mparams.kFactor() ) + { + map->applyDJT(&mparams, df, df); // apply geometric stiffness: variation due to a change of mapping, with a constant force below the mapping + } + }))); } - executeVisitor( MechanicalComputeDfVisitor(&mparams, df, accumulate) ); + else + { + executeVisitor(makeMechanicalVisitor(&mparams, addDForce)); + } + mparams.setDx(dx); } @@ -411,7 +461,16 @@ void MechanicalOperations::addSeparateGravity(SReal dt, core::MultiVecDerivId re { mparams.setDt(dt); setV(result); - executeVisitor( MechanicalAddSeparateGravityVisitor(&mparams, result) ); + executeVisitor( makeMechanicalVisitor(&mparams, + TopDownMassCallable([this, &result](simulation::Node* /*node*/, core::behavior::BaseMass* mass) + { + if( mass->m_separateGravity.getValue() ) + { + mass->addGravityToV(&mparams, result); + } + return Visitor::RESULT_CONTINUE; + }) + ) ); } void MechanicalOperations::computeContactForce(core::MultiVecDerivId result) @@ -436,8 +495,30 @@ void MechanicalOperations::computeAcc(SReal t, core::MultiVecDerivId a, core::Mu setDx(a); setX(x); setV(v); - executeVisitor( MechanicalProjectPositionAndVelocityVisitor(&mparams, t,x,v) ); - executeVisitor( MechanicalPropagateOnlyPositionAndVelocityVisitor(&mparams, t,x,v) ); + + executeVisitor( makeMechanicalVisitor(&mparams, + TopDownMechanicalMappingCallable([](simulation::Node* /*node*/, core::BaseMapping* /*map*/) + { + return Visitor::RESULT_PRUNE; + }), + TopDownProjectiveConstraintSetCallable([this, &x, &v](simulation::Node* /*node*/, core::behavior::BaseProjectiveConstraintSet* c) + { + c->projectPosition(&mparams, x); + c->projectVelocity(&mparams, v); + return Visitor::RESULT_CONTINUE; + }) + ) ); + + executeVisitor( makeMechanicalVisitor(&mparams, + TopDownMechanicalMappingCallable([this, &x, &v](simulation::Node* /*node*/, core::BaseMapping* map) + { + map->apply(&mparams, x, x); + map->applyJ(&mparams, v, v); + + return Visitor::RESULT_CONTINUE; + }) + ) ); + computeForce(f); accFromF(a,f); 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..382f6541582 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalVisitorCreator.h @@ -0,0 +1,137 @@ +/****************************************************************************** +* 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 +#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) + + +}