From 5c816e7cc927ac39a0ad921ede6977609f4cced8 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 17 Apr 2026 15:03:06 +0200 Subject: [PATCH 01/41] start implementation of mapping graph visitor --- Sofa/framework/Simulation/Core/CMakeLists.txt | 2 + .../src/sofa/simulation/MappingGraph2.cpp | 204 +++++++++++++++ .../Core/src/sofa/simulation/MappingGraph2.h | 199 +++++++++++++++ .../Simulation/Core/test/CMakeLists.txt | 1 + .../Core/test/MappingGraph2_test.cpp | 239 ++++++++++++++++++ 5 files changed, 645 insertions(+) create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h create mode 100644 Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp diff --git a/Sofa/framework/Simulation/Core/CMakeLists.txt b/Sofa/framework/Simulation/Core/CMakeLists.txt index e16319d1d4e..398979e070a 100644 --- a/Sofa/framework/Simulation/Core/CMakeLists.txt +++ b/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}/MappingGraph2.h ${SRC_ROOT}/events/BuildConstraintSystemEndEvent.h ${SRC_ROOT}/events/SimulationInitDoneEvent.h @@ -170,6 +171,7 @@ set(SOURCE_FILES ${SRC_ROOT}/IntegrateBeginEvent.cpp ${SRC_ROOT}/IntegrateEndEvent.cpp ${SRC_ROOT}/MappingGraph.cpp + ${SRC_ROOT}/MappingGraph2.cpp ${SRC_ROOT}/MechanicalOperations.cpp ${SRC_ROOT}/MechanicalVPrintVisitor.cpp ${SRC_ROOT}/MechanicalVisitor.cpp diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp new file mode 100644 index 00000000000..acb9cc0819f --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp @@ -0,0 +1,204 @@ +#include + +namespace sofa::simulation +{ + +MappingGraph2::InputLists MappingGraph2::InputLists::makeFromNode(sofa::simulation::Node::SPtr node) +{ + InputLists inputLists; + node->getTreeObjects(inputLists.mechanicalStates); + node->getTreeObjects(inputLists.mappings); + node->getTreeObjects(inputLists.forceFields); + node->getTreeObjects(inputLists.masses); + return inputLists; +} + +void MappingGraph2::traverseTopDown(MappingGraphVisitor& visitor) const +{ + // pending count = number of parents not yet visited. + std::queue ready; + for (auto& node : m_allNodes) + { + node->m_pendingCount = static_cast(node->m_parents.size()); + if (node->m_pendingCount == 0) + { + ready.push(node.get()); + } + } + + processQueue(ready, visitor, /*topDown=*/true); +} + +void MappingGraph2::traverseBottomUp(MappingGraphVisitor& visitor) const +{ + // pending count = number of children not yet visited. + std::queue ready; + for (auto& node : m_allNodes) + { + node->m_pendingCount = static_cast(node->m_children.size()); + if (node->m_pendingCount == 0) + { + ready.push(node.get()); + } + } + + processQueue(ready, visitor, /*topDown=*/false); +} + +bool MappingGraph2::isBuilt() const { return m_isBuilt; } + +void MappingGraph2::build(const InputLists& input) +{ + // 1. Create one wrapper node per object; index state nodes by raw ptr. + std::vector*> mechanicalStateNodes; + for (auto& s : input.mechanicalStates) + { + auto node = makeMappingGraphNode(s); + mechanicalStateNodes.push_back(node.get()); + m_stateIndex[s] = node.get(); + m_allNodes.push_back(std::move(node)); + } + + // Collect (leafNode*, connected states) for every leaf component type. + std::vector>> + leafConnections; + + const auto processComponents = [&leafConnections, this](const sofa::type::vector& components) + { + for (const auto& component : components) + { + auto node = makeMappingGraphNode(component); + + const auto& states = component->getMechanicalStates(); + std::vector statesVector{states.begin(), + states.end()}; + leafConnections.emplace_back(node.get(), statesVector); + + m_allNodes.push_back(std::move(node)); + } + }; + + processComponents(input.forceFields); + processComponents(input.masses); + + // 2. Wire leaf component edges: connectedState → leafComponent + for (auto& [leafNode, states] : leafConnections) + { + auto sn = findGroupNode(states); + if (sn) + { + addEdge(sn.get(), leafNode); + } + } + + std::vector mappingNodePtrs; + for (auto& m : input.mappings) + { + auto node = makeMappingGraphNode(m); + mappingNodePtrs.push_back(node.get()); + m_allNodes.push_back(std::move(node)); + } + + // 3. Wire mapping edges: inputState → mapping → outputState + for (size_t i = 0; i < input.mappings.size(); ++i) + { + BaseMappingGraphNode* mappingNode = mappingNodePtrs[i]; + for (auto& s : input.mappings[i]->getMechFrom()) + { + if (auto groupNode = findInGroupNodes(s)) + { + addEdge(groupNode.get(), mappingNode); + } + else if (auto* sn = findStateNode(s)) + { + addEdge(sn, mappingNode); + } + } + + for (auto& s : input.mappings[i]->getMechTo()) + { + if (auto* sn = findStateNode(s)) + { + addEdge(mappingNode, sn); + } + } + } + + // 4. Roots: MechanicalState nodes with no parents. + std::copy_if(mechanicalStateNodes.begin(), mechanicalStateNodes.end(), std::back_inserter(m_roots), + [](auto* node) { return node->m_parents.empty(); }); + + m_isBuilt = true; +} + +ComponentGroupMappingGraphNode::SPtr MappingGraph2::findGroupNode( + const std::vector& states) +{ + auto it = std::find_if(m_groupIndex.begin(), m_groupIndex.end(), + [&states](auto& group){ return group.first == states; }); + if (it != m_groupIndex.end()) + return it->second; + + std::cout << "Creating group node for " << sofa::helper::join(states.begin(), states.end(), [](auto state){ return state->getName(); }, ", ") << std::endl; + auto group = ComponentGroupMappingGraphNode::SPtr{new ComponentGroupMappingGraphNode}; + for (const auto& state : states) + { + if (auto* sn = findStateNode(state.get())) + { + addEdge(sn, group.get()); + } + } + m_groupIndex.emplace_back(states, group); + m_allNodes.push_back(group); + return group; +} + +ComponentGroupMappingGraphNode::SPtr MappingGraph2::findInGroupNodes( + const core::behavior::BaseMechanicalState::SPtr state) +{ + auto it = std::find_if(m_groupIndex.begin(), m_groupIndex.end(), + [&state](auto& group) + { + return std::find(group.first.begin(), group.first.end(), state) != group.first.end(); + }); + if (it != m_groupIndex.end()) + return it->second; + return nullptr; +} + +void MappingGraph2::processQueue(std::queue& ready, MappingGraphVisitor& visitor, + bool topDown) +{ + while (!ready.empty()) + { + BaseMappingGraphNode* current = ready.front(); + std::cout << "Processing node: " << current->getName() << std::endl; + ready.pop(); + + current->accept(visitor); + + const auto& neighbours = topDown ? current->m_children : current->m_parents; + for (auto& neighbour : neighbours) + { + --(neighbour->m_pendingCount); + if (neighbour->m_pendingCount == 0) + { + ready.push(neighbour.get()); + } + } + } +} + +BaseMappingGraphNode* MappingGraph2::findStateNode(core::behavior::BaseMechanicalState* raw) const +{ + auto it = m_stateIndex.find(raw); + return (it != m_stateIndex.end()) ? it->second : nullptr; +} + +void MappingGraph2::addEdge(BaseMappingGraphNode* from, BaseMappingGraphNode* to) +{ + from->m_children.push_back(to->shared_from_this()); + to->m_parents.push_back(from->shared_from_this()); +} + +} // namespace sofa::simulation diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h new file mode 100644 index 00000000000..1630c8f8533 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h @@ -0,0 +1,199 @@ +/****************************************************************************** +* 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 +#include +#include +#include "Node.h" + +#include +#include +#include +#include + +namespace sofa::simulation +{ + +// --------------------------------------------------------------------------- +// Visitor interface +// --------------------------------------------------------------------------- +class MappingGraphVisitor +{ +public: + virtual ~MappingGraphVisitor() = default; + + virtual void visit(core::behavior::BaseMechanicalState&) {} + virtual void visit(core::BaseMapping&) {} + virtual void visit(core::behavior::BaseForceField&) {} + virtual void visit(core::behavior::BaseMass&) {} +}; + +// --------------------------------------------------------------------------- +// Graph node +// --------------------------------------------------------------------------- +class BaseMappingGraphNode : public std::enable_shared_from_this +{ +public: + using SPtr = std::shared_ptr; + friend class MappingGraph2; + virtual ~BaseMappingGraphNode() = default; + + virtual void accept(MappingGraphVisitor& visitor) const = 0; + + virtual std::string getName() const { return {}; } + +private: + std::vector m_parents; // prerequisite nodes + std::vector m_children; // dependent nodes + + // Mutable counter used during traversal (reset before each traversal). + mutable int m_pendingCount = 0; +}; + +template +class MappingGraphNode : public BaseMappingGraphNode +{ +public: + using SPtr = std::shared_ptr; + explicit MappingGraphNode(typename TComponent::SPtr s) + : m_component(std::move(s)) + {} + + void accept(MappingGraphVisitor& visitor) const override + { + if (m_component) + { + visitor.visit(*m_component); + } + } + + std::string getName() const override + { + return m_component->getName(); + } + +private: + typename TComponent::SPtr m_component; +}; + +template +MappingGraphNode::SPtr makeMappingGraphNode(typename TComponent::SPtr s) +{ + return MappingGraphNode::SPtr( new MappingGraphNode(s) ); +} + +class ComponentGroupMappingGraphNode : public BaseMappingGraphNode +{ +public: + using SPtr = std::shared_ptr; + void accept(MappingGraphVisitor& visitor) const override { SOFA_UNUSED(visitor); } + + std::string getName() const override + { + return "group"; + } +}; + +// --------------------------------------------------------------------------- +// Mapping graph +// --------------------------------------------------------------------------- +class SOFA_SIMULATION_CORE_API MappingGraph2 +{ +public: + // ------------------------------------------------------------------ + // Input: flat lists collected from the scene DAG (or any other source). + // Add further leaf component lists as needed. + // ------------------------------------------------------------------ + struct SOFA_SIMULATION_CORE_API InputLists + { + sofa::type::vector mechanicalStates; + sofa::type::vector mappings; + sofa::type::vector forceFields; + sofa::type::vector masses; + + static InputLists makeFromNode(sofa::simulation::Node::SPtr node); + }; + + MappingGraph2() = default; + explicit MappingGraph2(const InputLists& input) { build(input); } + + // ------------------------------------------------------------------ + // Top-down traversal: roots (unmapped states) → leaves (components). + // + // Invariants: + // • A BaseMechanicalState is visited only after every BaseMapping + // that produces it as output has been visited. + // • A BaseMapping is visited only after every input + // BaseMechanicalState has been visited. + // • A leaf component is visited only after every BaseMechanicalState + // connected to it has been visited. + // • Each node is visited exactly once. + // ------------------------------------------------------------------ + void traverseTopDown(MappingGraphVisitor& visitor) const; + + // ------------------------------------------------------------------ + // Bottom-up traversal: leaves → roots. + // + // Mirror invariants of top-down, reversed. + // ------------------------------------------------------------------ + void traverseBottomUp(MappingGraphVisitor& visitor) const; + + // Accessors for inspection / testing. + const std::vector& allNodes() const { return m_allNodes; } + const std::vector*>& roots() const { return m_roots; } + + [[nodiscard]] bool isBuilt() const; + + // ------------------------------------------------------------------ + // Graph construction + // ------------------------------------------------------------------ + void build(const InputLists& input); + +private: + bool m_isBuilt = false; + std::vector m_allNodes; + std::vector*> m_roots; + std::unordered_map m_stateIndex; + std::vector, + ComponentGroupMappingGraphNode::SPtr>> m_groupIndex; + + // ------------------------------------------------------------------ + // Kahn-style BFS used by both traversal directions. + // topDown == true → follow children, decrement their pendingCount. + // topDown == false → follow parents, decrement their pendingCount. + // ------------------------------------------------------------------ + static void processQueue(std::queue& ready, + MappingGraphVisitor& visitor, + bool topDown); + + ComponentGroupMappingGraphNode::SPtr findGroupNode(const std::vector& states); + ComponentGroupMappingGraphNode::SPtr findInGroupNodes(const core::behavior::BaseMechanicalState::SPtr state); + BaseMappingGraphNode* findStateNode(core::behavior::BaseMechanicalState* raw) const; + + // Directed edge from → to. + // Both parent and child lists hold SPtr so the graph owns all nodes. + static void addEdge(BaseMappingGraphNode* from, BaseMappingGraphNode* to); +}; +} diff --git a/Sofa/framework/Simulation/Core/test/CMakeLists.txt b/Sofa/framework/Simulation/Core/test/CMakeLists.txt index 3f58b89edc9..3f3bc5cd26f 100644 --- a/Sofa/framework/Simulation/Core/test/CMakeLists.txt +++ b/Sofa/framework/Simulation/Core/test/CMakeLists.txt @@ -5,6 +5,7 @@ project(Sofa.Simulation.Core_test) set(SOURCE_FILES Colors_test.cpp MappingGraph_test.cpp + MappingGraph2_test.cpp ParallelForEach_test.cpp RequiredPlugin_test.cpp SceneCheckRegistry_test.cpp diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp new file mode 100644 index 00000000000..d9028030709 --- /dev/null +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -0,0 +1,239 @@ +/****************************************************************************** +* 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 * +******************************************************************************/ +#include +#include +#include +#include + +namespace sofa +{ + +TEST(MappingGraph, DefaultConstructor) +{ + sofa::simulation::MappingGraph2 mappingGraph; + EXPECT_FALSE(mappingGraph.isBuilt()); +} + +TEST(MappingGraph, Build) +{ + sofa::simulation::MappingGraph2 mappingGraph; + sofa::simulation::MappingGraph2::InputLists input; + mappingGraph.build(input); + EXPECT_TRUE(mappingGraph.isBuilt()); +} + +struct CollectNamesVisitor : public sofa::simulation::MappingGraphVisitor +{ + void visit(core::BaseMapping& mapping) override + { + names.push_back(mapping.getName()); + } + + void visit(core::behavior::BaseMechanicalState& state) override + { + names.push_back(state.getName()); + } + + void visit(core::behavior::BaseForceField& ff) override + { + names.push_back(ff.getName()); + } + + void visit(core::behavior::BaseMass& mass) override + { + names.push_back(mass.getName()); + } + + std::vector names; +}; + +TEST(MappingGraph, SingleState) +{ + const sofa::simulation::Node::SPtr root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + + sofa::simpleapi::importPlugin(Sofa.Component.StateContainer); + sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state"}}); + + auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root); + ASSERT_EQ(inputs.mechanicalStates.size(), 1); + sofa::simulation::MappingGraph2 mappingGraph(inputs); + ASSERT_TRUE(mappingGraph.isBuilt()); + + CollectNamesVisitor visitor; + + mappingGraph.traverseTopDown(visitor); + ASSERT_EQ(visitor.names.size(), 1); + EXPECT_EQ(visitor.names[0], "state"); +} + +TEST(MappingGraph, SingleMappingInSingleNode) +{ + const sofa::simulation::Node::SPtr root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + + sofa::simpleapi::importPlugin(Sofa.Component.Mapping.Linear); + sofa::simpleapi::importPlugin(Sofa.Component.StateContainer); + + sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state1"}}); + sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state2"}}); + sofa::simpleapi::createObject(root, "IdentityMapping", {{"name", "mapping"}, {"input", "@state1"}, {"output", "@state2"}}); + + auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root); + ASSERT_EQ(inputs.mappings.size(), 1); + ASSERT_EQ(inputs.mechanicalStates.size(), 2); + sofa::simulation::MappingGraph2 mappingGraph(inputs); + ASSERT_TRUE(mappingGraph.isBuilt()); + + CollectNamesVisitor visitor; + + mappingGraph.traverseTopDown(visitor); + ASSERT_EQ(visitor.names.size(), 3); + EXPECT_EQ(visitor.names[0], "state1"); + EXPECT_EQ(visitor.names[1], "mapping"); + EXPECT_EQ(visitor.names[2], "state2"); + + visitor.names.clear(); + mappingGraph.traverseBottomUp(visitor); + ASSERT_EQ(visitor.names.size(), 3); + EXPECT_EQ(visitor.names[0], "state2"); + EXPECT_EQ(visitor.names[1], "mapping"); + EXPECT_EQ(visitor.names[2], "state1"); +} + +TEST(MappingGraph, SingleMappingWithIntermediateNode) +{ + const sofa::simulation::Node::SPtr root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + + sofa::simpleapi::importPlugin(Sofa.Component.Mapping.Linear); + sofa::simpleapi::importPlugin(Sofa.Component.StateContainer); + + sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state1"}}); + const auto node1 = root->createChild("node1"); + sofa::simpleapi::createObject(node1, "MechanicalObject", {{"name", "state2"}}); + sofa::simpleapi::createObject(node1, "IdentityMapping", {{"name", "mapping"}, {"input", "@state1"}, {"output", "@state2"}}); + + auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root); + ASSERT_EQ(inputs.mappings.size(), 1); + ASSERT_EQ(inputs.mechanicalStates.size(), 2); + sofa::simulation::MappingGraph2 mappingGraph(inputs); + ASSERT_TRUE(mappingGraph.isBuilt()); + + CollectNamesVisitor visitor; + + mappingGraph.traverseTopDown(visitor); + ASSERT_EQ(visitor.names.size(), 3); + EXPECT_EQ(visitor.names[0], "state1"); + EXPECT_EQ(visitor.names[1], "mapping"); + EXPECT_EQ(visitor.names[2], "state2"); + + visitor.names.clear(); + mappingGraph.traverseBottomUp(visitor); + ASSERT_EQ(visitor.names.size(), 3); + EXPECT_EQ(visitor.names[0], "state2"); + EXPECT_EQ(visitor.names[1], "mapping"); + EXPECT_EQ(visitor.names[2], "state1"); +} + +TEST(MappingGraph, SingleMappingWithIntermediateNodeInverseInputOutput) +{ + const sofa::simulation::Node::SPtr root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + + sofa::simpleapi::importPlugin(Sofa.Component.Mapping.Linear); + sofa::simpleapi::importPlugin(Sofa.Component.StateContainer); + + sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state1"}}); + const auto node1 = root->createChild("node1"); + sofa::simpleapi::createObject(node1, "MechanicalObject", {{"name", "state2"}}); + sofa::simpleapi::createObject(node1, "IdentityMapping", {{"name", "mapping"}, {"input", "@state2"}, {"output", "@state1"}}); + + auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root); + ASSERT_EQ(inputs.mappings.size(), 1); + ASSERT_EQ(inputs.mechanicalStates.size(), 2); + sofa::simulation::MappingGraph2 mappingGraph(inputs); + ASSERT_TRUE(mappingGraph.isBuilt()); + + CollectNamesVisitor visitor; + + mappingGraph.traverseTopDown(visitor); + ASSERT_EQ(visitor.names.size(), 3); + EXPECT_EQ(visitor.names[0], "state2"); + EXPECT_EQ(visitor.names[1], "mapping"); + EXPECT_EQ(visitor.names[2], "state1"); + + visitor.names.clear(); + mappingGraph.traverseBottomUp(visitor); + ASSERT_EQ(visitor.names.size(), 3); + EXPECT_EQ(visitor.names[0], "state1"); + EXPECT_EQ(visitor.names[1], "mapping"); + EXPECT_EQ(visitor.names[2], "state2"); +} + +TEST(MappingGraph, ComplexGraph) +{ + const sofa::simulation::Node::SPtr root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + + sofa::simpleapi::importPlugin(Sofa.Component.Mapping.Linear); + sofa::simpleapi::importPlugin(Sofa.Component.StateContainer); + sofa::simpleapi::importPlugin(Sofa.Component.MechanicalLoad); + sofa::simpleapi::importPlugin(Sofa.Component.Mass); + + sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state1"}}); + sofa::simpleapi::createObject(root, "ConstantForceField", {{"name", "ff1"}, {"state", "@state1"}, {"forces", "1 0 0"}}); + sofa::simpleapi::createObject(root, "UniformMass", {{"name", "mass1"}}); + const auto node1 = root->createChild("node1"); + sofa::simpleapi::createObject(node1, "MechanicalObject", {{"name", "state2"}}); + sofa::simpleapi::createObject(node1, "ConstantForceField", {{"name", "ff2"}, {"state", "@state2"}, {"forces", "1 0 0"}}); + sofa::simpleapi::createObject(node1, "UniformMass", {{"name", "mass2"}}); + sofa::simpleapi::createObject(node1, "IdentityMapping", {{"name", "mapping"}, {"input", "@state1"}, {"output", "@state2"}}); + + sofa::simulation::node::initRoot(root.get()); + + auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root); + ASSERT_EQ(inputs.mappings.size(), 1); + ASSERT_EQ(inputs.mechanicalStates.size(), 2); + ASSERT_EQ(inputs.forceFields.size(), 4); + sofa::simulation::MappingGraph2 mappingGraph(inputs); + ASSERT_TRUE(mappingGraph.isBuilt()); + + CollectNamesVisitor visitor; + + mappingGraph.traverseTopDown(visitor); + ASSERT_EQ(visitor.names.size(), 9); // 9 and not 7 because a UniformMass is a BaseMass and also a BaseForceField + EXPECT_EQ(visitor.names[0], "state1"); + EXPECT_EQ(visitor.names[1], "ff1"); + EXPECT_EQ(visitor.names[2], "mass1"); + EXPECT_EQ(visitor.names[3], "mass1"); + + EXPECT_EQ(visitor.names[4], "mapping"); + EXPECT_EQ(visitor.names[5], "state2"); + EXPECT_EQ(visitor.names[6], "ff2"); + EXPECT_EQ(visitor.names[7], "mass2"); + EXPECT_EQ(visitor.names[8], "mass2"); + + visitor.names.clear(); + // mappingGraph.traverseBottomUp(visitor); + // ASSERT_EQ(visitor.names.size(), 9); + // EXPECT_EQ(visitor.names[0], "state2"); + // EXPECT_EQ(visitor.names[1], "mapping"); + // EXPECT_EQ(visitor.names[2], "state1"); +} + +} From 347a0058b4ecf32b61312b88210bf9de7c76b581 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 17 Apr 2026 15:28:14 +0200 Subject: [PATCH 02/41] continue --- .../src/sofa/simulation/MappingGraph2.cpp | 110 +++++++++++++----- .../Core/src/sofa/simulation/MappingGraph2.h | 12 +- .../Core/test/MappingGraph2_test.cpp | 19 ++- 3 files changed, 103 insertions(+), 38 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp index acb9cc0819f..67b493afbc7 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp @@ -1,5 +1,7 @@ #include +#include + namespace sofa::simulation { @@ -31,22 +33,101 @@ void MappingGraph2::traverseTopDown(MappingGraphVisitor& visitor) const void MappingGraph2::traverseBottomUp(MappingGraphVisitor& visitor) const { - // pending count = number of children not yet visited. std::queue ready; for (auto& node : m_allNodes) { - node->m_pendingCount = static_cast(node->m_children.size()); + node->m_pendingCount = static_cast(node->m_parents.size()); if (node->m_pendingCount == 0) { ready.push(node.get()); } } - processQueue(ready, visitor, /*topDown=*/false); + sofa::type::vector nodes; + while (!ready.empty()) + { + BaseMappingGraphNode* current = ready.front(); + nodes.push_back(current); + ready.pop(); + + for (auto& child : current->m_children) + { + --(child->m_pendingCount); + if (child->m_pendingCount == 0) + { + ready.push(child.get()); + } + } + } + + for (auto it = nodes.crbegin(); it != nodes.crend(); ++it) + { + (*it)->accept(visitor); + } +} + +void MappingGraph2::traverseComponentGroups(MappingGraphVisitor& visitor) const +{ + for (auto& [states, node] : m_groupIndex) + { + for (auto& child : node->m_children) + { + child->accept(visitor); + } + } +} +void MappingGraph2::traverseComponentGroups(MappingGraphVisitor& visitor, + TaskScheduler* taskScheduler) const +{ + if (taskScheduler) + { + sofa::simulation::parallelForEach(*taskScheduler, m_groupIndex.begin(), m_groupIndex.end(), + [&visitor](const auto& pair) + { + for (auto& child : pair.second->m_children) + { + child->accept(visitor); + } + }); + } + else + { + traverseComponentGroups(visitor); + } } +void MappingGraph2::processQueue(std::queue& ready, MappingGraphVisitor& visitor, + bool topDown) +{ + while (!ready.empty()) + { + BaseMappingGraphNode* current = ready.front(); + std::cout << "Processing node: " << current->getName() << std::endl; + ready.pop(); + + current->accept(visitor); + + const auto& neighbours = topDown ? current->m_children : current->m_parents; + for (auto& neighbour : neighbours) + { + --(neighbour->m_pendingCount); + if (neighbour->m_pendingCount == 0) + { + ready.push(neighbour.get()); + } + } + } +} + + bool MappingGraph2::isBuilt() const { return m_isBuilt; } +template +MappingGraphNode::SPtr makeMappingGraphNode(typename TComponent::SPtr s) +{ + return MappingGraphNode::SPtr( new MappingGraphNode(s) ); +} + void MappingGraph2::build(const InputLists& input) { // 1. Create one wrapper node per object; index state nodes by raw ptr. @@ -166,29 +247,6 @@ ComponentGroupMappingGraphNode::SPtr MappingGraph2::findInGroupNodes( return nullptr; } -void MappingGraph2::processQueue(std::queue& ready, MappingGraphVisitor& visitor, - bool topDown) -{ - while (!ready.empty()) - { - BaseMappingGraphNode* current = ready.front(); - std::cout << "Processing node: " << current->getName() << std::endl; - ready.pop(); - - current->accept(visitor); - - const auto& neighbours = topDown ? current->m_children : current->m_parents; - for (auto& neighbour : neighbours) - { - --(neighbour->m_pendingCount); - if (neighbour->m_pendingCount == 0) - { - ready.push(neighbour.get()); - } - } - } -} - BaseMappingGraphNode* MappingGraph2::findStateNode(core::behavior::BaseMechanicalState* raw) const { auto it = m_stateIndex.find(raw); diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h index 1630c8f8533..b1f4b47fea2 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h @@ -25,7 +25,7 @@ #include #include #include -#include "Node.h" +#include #include #include @@ -34,6 +34,7 @@ namespace sofa::simulation { +class TaskScheduler; // --------------------------------------------------------------------------- // Visitor interface @@ -97,12 +98,6 @@ class MappingGraphNode : public BaseMappingGraphNode typename TComponent::SPtr m_component; }; -template -MappingGraphNode::SPtr makeMappingGraphNode(typename TComponent::SPtr s) -{ - return MappingGraphNode::SPtr( new MappingGraphNode(s) ); -} - class ComponentGroupMappingGraphNode : public BaseMappingGraphNode { public: @@ -159,6 +154,9 @@ class SOFA_SIMULATION_CORE_API MappingGraph2 // ------------------------------------------------------------------ void traverseBottomUp(MappingGraphVisitor& visitor) const; + void traverseComponentGroups(MappingGraphVisitor& visitor) const; + void traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const; + // Accessors for inspection / testing. const std::vector& allNodes() const { return m_allNodes; } const std::vector*>& roots() const { return m_roots; } diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index d9028030709..e363e1f4b6c 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -217,6 +217,7 @@ TEST(MappingGraph, ComplexGraph) mappingGraph.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 9); // 9 and not 7 because a UniformMass is a BaseMass and also a BaseForceField + EXPECT_EQ(visitor.names[0], "state1"); EXPECT_EQ(visitor.names[1], "ff1"); EXPECT_EQ(visitor.names[2], "mass1"); @@ -229,11 +230,19 @@ TEST(MappingGraph, ComplexGraph) EXPECT_EQ(visitor.names[8], "mass2"); visitor.names.clear(); - // mappingGraph.traverseBottomUp(visitor); - // ASSERT_EQ(visitor.names.size(), 9); - // EXPECT_EQ(visitor.names[0], "state2"); - // EXPECT_EQ(visitor.names[1], "mapping"); - // EXPECT_EQ(visitor.names[2], "state1"); + mappingGraph.traverseBottomUp(visitor); + ASSERT_EQ(visitor.names.size(), 9); + + EXPECT_EQ(visitor.names[0], "mass2"); + EXPECT_EQ(visitor.names[1], "mass2"); + EXPECT_EQ(visitor.names[2], "ff2"); + EXPECT_EQ(visitor.names[3], "state2"); + + EXPECT_EQ(visitor.names[4], "mapping"); + EXPECT_EQ(visitor.names[5], "mass1"); + EXPECT_EQ(visitor.names[6], "mass1"); + EXPECT_EQ(visitor.names[7], "ff1"); + EXPECT_EQ(visitor.names[8], "state1"); } } From c98d27ecca93431713dbc3c95857f215228085f3 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 17 Apr 2026 16:38:54 +0200 Subject: [PATCH 03/41] implement interface of MappingGraph --- .../src/sofa/simulation/MappingGraph2.cpp | 147 +++++++++++++++++- .../Core/src/sofa/simulation/MappingGraph2.h | 80 ++++++++-- .../Core/test/MappingGraph2_test.cpp | 4 +- .../Core/test/MappingGraph_test.cpp | 34 ++-- 4 files changed, 227 insertions(+), 38 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp index 67b493afbc7..46ba97df931 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp @@ -5,16 +5,130 @@ namespace sofa::simulation { -MappingGraph2::InputLists MappingGraph2::InputLists::makeFromNode(sofa::simulation::Node::SPtr node) +MappingGraph2::InputLists MappingGraph2::InputLists::makeFromNode(core::objectmodel::BaseContext* node) { InputLists inputLists; - node->getTreeObjects(inputLists.mechanicalStates); - node->getTreeObjects(inputLists.mappings); - node->getTreeObjects(inputLists.forceFields); - node->getTreeObjects(inputLists.masses); + if (node) + { + node->getObjects(inputLists.mechanicalStates, core::objectmodel::BaseContext::SearchDirection::SearchDown); + node->getObjects(inputLists.mappings, core::objectmodel::BaseContext::SearchDirection::SearchDown); + node->getObjects(inputLists.forceFields, core::objectmodel::BaseContext::SearchDirection::SearchDown); + node->getObjects(inputLists.masses, core::objectmodel::BaseContext::SearchDirection::SearchDown); + } return inputLists; } +MappingGraph2::MappingGraph2(const InputLists& input) +{ + build(input); +} + +MappingGraph2::MappingGraph2(core::objectmodel::BaseContext* node) +{ + build(node); +} + +core::objectmodel::BaseContext* MappingGraph2::getRootNode() const +{ + return m_rootNode; +} + +const sofa::type::vector& +MappingGraph2::getMainMechanicalStates() const +{ + return m_roots; +} +MappingGraph2::MappingInputs MappingGraph2::getTopMostMechanicalStates( + core::behavior::BaseMechanicalState* state) const +{ + auto* sn = findStateNode(state); + if (sn) + { + struct CollectInput : public MappingGraphVisitor + { + void visit(core::behavior::BaseMechanicalState& state) override + { + inputs.push_back(&state); + } + + MappingInputs inputs; + } visitor; + + for (auto& node : m_allNodes) + { + node->m_pendingCount = 0; + } + + std::queue nodes; + nodes.push(sn); + + while (!nodes.empty()) + { + BaseMappingGraphNode* current = nodes.front(); + nodes.pop(); + + ++(current->m_pendingCount); + + if (current->m_parents.empty()) + { + current->accept(visitor); + } + + for (auto& parent : current->m_parents) + { + if (parent->m_pendingCount == 0) + { + nodes.push(parent.get()); + } + } + } + + return visitor.inputs; + } + + return {}; +} + +bool MappingGraph2::hasAnyMapping() const +{ + return m_hasAnyMapping; +} + +sofa::Size MappingGraph2::getTotalNbMainDofs() const +{ + return m_totalNbMainDofs; +} + +type::Vec2u MappingGraph2::getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* mstate) const +{ + if (m_rootNode == nullptr) + { + msg_error("MappingGraph") << "Graph is not built yet"; + return type::Vec2u{}; + } + + if (mstate == nullptr) + { + msg_error("MappingGraph") << "Requested mechanical state is not valid : cannot get its position in the global matrix"; + return type::Vec2u{}; + } + + if (const auto it = m_positionInGlobalMatrix.find(mstate); it != m_positionInGlobalMatrix.end()) + return it->second; + + msg_error("MappingGraph") << "Requested mechanical state (" << mstate->getPathName() << + ") is probably mapped or unknown from the graph: only main mechanical states have an associated submatrix in the global matrix"; + return type::Vec2u{}; +} + +type::Vec2u MappingGraph2::getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* a, + core::behavior::BaseMechanicalState* b) const +{ + const auto pos_a = getPositionInGlobalMatrix(a); + const auto pos_b = getPositionInGlobalMatrix(b); + return {pos_a[0], pos_b[1]}; +} + void MappingGraph2::traverseTopDown(MappingGraphVisitor& visitor) const { // pending count = number of parents not yet visited. @@ -130,6 +244,8 @@ MappingGraphNode::SPtr makeMappingGraphNode(typename TComponent::SPt void MappingGraph2::build(const InputLists& input) { + m_hasAnyMapping = input.mappings.size() > 0; + // 1. Create one wrapper node per object; index state nodes by raw ptr. std::vector*> mechanicalStateNodes; for (auto& s : input.mechanicalStates) @@ -206,12 +322,29 @@ void MappingGraph2::build(const InputLists& input) } // 4. Roots: MechanicalState nodes with no parents. - std::copy_if(mechanicalStateNodes.begin(), mechanicalStateNodes.end(), std::back_inserter(m_roots), - [](auto* node) { return node->m_parents.empty(); }); + m_totalNbMainDofs = 0; + for (const auto& node : mechanicalStateNodes) + { + if (node->m_parents.empty()) + { + m_roots.push_back(node->m_component.get()); + m_positionInGlobalMatrix[node->m_component.get()] = type::Vec2u(m_totalNbMainDofs, m_totalNbMainDofs); + m_totalNbMainDofs += node->m_component->getMatrixSize(); + } + } m_isBuilt = true; } +void MappingGraph2::build(core::objectmodel::BaseContext* rootNode) +{ + if (rootNode) + { + build(InputLists::makeFromNode(rootNode)); + } + m_rootNode = rootNode; +} + ComponentGroupMappingGraphNode::SPtr MappingGraph2::findGroupNode( const std::vector& states) { diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h index b1f4b47fea2..31437eb0346 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h @@ -65,8 +65,8 @@ class BaseMappingGraphNode : public std::enable_shared_from_this m_parents; // prerequisite nodes - std::vector m_children; // dependent nodes + sofa::type::vector m_parents; // prerequisite nodes + sofa::type::vector m_children; // dependent nodes // Mutable counter used during traversal (reset before each traversal). mutable int m_pendingCount = 0; @@ -77,6 +77,7 @@ class MappingGraphNode : public BaseMappingGraphNode { public: using SPtr = std::shared_ptr; + friend class MappingGraph2; explicit MappingGraphNode(typename TComponent::SPtr s) : m_component(std::move(s)) {} @@ -116,6 +117,8 @@ class ComponentGroupMappingGraphNode : public BaseMappingGraphNode class SOFA_SIMULATION_CORE_API MappingGraph2 { public: + using MappingInputs = type::vector; + // ------------------------------------------------------------------ // Input: flat lists collected from the scene DAG (or any other source). // Add further leaf component lists as needed. @@ -127,11 +130,43 @@ class SOFA_SIMULATION_CORE_API MappingGraph2 sofa::type::vector forceFields; sofa::type::vector masses; - static InputLists makeFromNode(sofa::simulation::Node::SPtr node); + static InputLists makeFromNode(core::objectmodel::BaseContext* node); + static InputLists makeFromNode(core::objectmodel::BaseContext::SPtr node) { return makeFromNode(node.get()); } }; MappingGraph2() = default; - explicit MappingGraph2(const InputLists& input) { build(input); } + explicit MappingGraph2(const InputLists& input); + explicit MappingGraph2(core::objectmodel::BaseContext* node); + + /// Return the node used to start the exploration of the scene graph in order to build the mapping graph + [[nodiscard]] core::objectmodel::BaseContext* getRootNode() const; + + /// Return the list of all mechanical states that are not mapped + [[nodiscard]] const sofa::type::vector& getMainMechanicalStates() const; + + /// Return the list of mechanical states which are: + /// 1) non-mapped + /// 2) input of a mapping involving the provided mechanical state as an output. + /// The search is recursive (more than one level of mapping). + MappingInputs getTopMostMechanicalStates(core::behavior::BaseMechanicalState* state) const; + + /// Return the list of mechanical states which are: + /// 1) non-mapped + /// 2) input of a mapping involving the mechanical states associated to the provided force field as an output. + /// The search is recursive (more than one level of mapping) + template requires !std::derived_from + MappingInputs getTopMostMechanicalStates(TComponent* component) const; + + [[nodiscard]] bool hasAnyMapping() const; + + /// Return the sum of the degrees of freedom of all main mechanical states + [[nodiscard]] sofa::Size getTotalNbMainDofs() const; + + /// Return where in the global matrix the provided mechanical state writes its contribution + type::Vec2u getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* mstate) const; + /// Return where in the global matrix the provided mechanical states writes its contribution + type::Vec2u getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* a, + core::behavior::BaseMechanicalState* b) const; // ------------------------------------------------------------------ // Top-down traversal: roots (unmapped states) → leaves (components). @@ -157,21 +192,28 @@ class SOFA_SIMULATION_CORE_API MappingGraph2 void traverseComponentGroups(MappingGraphVisitor& visitor) const; void traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const; - // Accessors for inspection / testing. - const std::vector& allNodes() const { return m_allNodes; } - const std::vector*>& roots() const { return m_roots; } - [[nodiscard]] bool isBuilt() const; // ------------------------------------------------------------------ // Graph construction // ------------------------------------------------------------------ void build(const InputLists& input); + void build(core::objectmodel::BaseContext* rootNode); private: + /// node used to start the exploration of the scene graph in order to build the mapping graph + core::objectmodel::BaseContext* m_rootNode { nullptr }; + bool m_isBuilt = false; + bool m_hasAnyMapping = false; + + sofa::Size m_totalNbMainDofs {}; + + /// for each main mechanical states, gives the position of its contribution in the global matrix + std::map m_positionInGlobalMatrix; + std::vector m_allNodes; - std::vector*> m_roots; + sofa::type::vector m_roots {}; std::unordered_map m_stateIndex; std::vector, @@ -194,4 +236,24 @@ class SOFA_SIMULATION_CORE_API MappingGraph2 // Both parent and child lists hold SPtr so the graph owns all nodes. static void addEdge(BaseMappingGraphNode* from, BaseMappingGraphNode* to); }; + +template requires !std::derived_from +MappingGraph2::MappingInputs MappingGraph2::getTopMostMechanicalStates(TComponent* component) const +{ + if (component == nullptr) + { + dmsg_error("MappingGraph") << "Requested mass is invalid"; + return {}; + } + + const auto& associatedMechanicalStates = component->getMechanicalStates(); + MappingInputs topMostMechanicalStates; + for (auto* mstate : associatedMechanicalStates) + { + const auto mstates = getTopMostMechanicalStates(mstate); + topMostMechanicalStates.insert(topMostMechanicalStates.end(), mstates.begin(), mstates.end()); + } + return topMostMechanicalStates; } + +} // namespace sofa::simulation diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index e363e1f4b6c..b8ddd668459 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -73,7 +73,7 @@ TEST(MappingGraph, SingleState) sofa::simpleapi::importPlugin(Sofa.Component.StateContainer); sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state"}}); - auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root); + auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root.get()); ASSERT_EQ(inputs.mechanicalStates.size(), 1); sofa::simulation::MappingGraph2 mappingGraph(inputs); ASSERT_TRUE(mappingGraph.isBuilt()); @@ -96,7 +96,7 @@ TEST(MappingGraph, SingleMappingInSingleNode) sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state2"}}); sofa::simpleapi::createObject(root, "IdentityMapping", {{"name", "mapping"}, {"input", "@state1"}, {"output", "@state2"}}); - auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root); + auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root.get()); ASSERT_EQ(inputs.mappings.size(), 1); ASSERT_EQ(inputs.mechanicalStates.size(), 2); sofa::simulation::MappingGraph2 mappingGraph(inputs); diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph_test.cpp index 0d6dc401b5b..7c01a3f7483 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph_test.cpp @@ -21,6 +21,7 @@ ******************************************************************************/ #include #include +#include #include #include #include @@ -30,7 +31,7 @@ TEST(MappingGraph, noBuild) { - const sofa::simulation::MappingGraph graph; + const sofa::simulation::MappingGraph2 graph; EXPECT_FALSE(graph.isBuilt()); EXPECT_EQ(graph.getRootNode(), nullptr); @@ -40,15 +41,14 @@ TEST(MappingGraph, noBuild) EXPECT_TRUE(graph.getTopMostMechanicalStates((sofa::core::behavior::BaseForceField*)nullptr).empty()); EXPECT_TRUE(graph.getTopMostMechanicalStates((sofa::core::behavior::BaseMass*)nullptr).empty()); - EXPECT_TRUE(graph.makeComponentGroups(sofa::core::MechanicalParams::defaultInstance()).empty()); EXPECT_FALSE(graph.hasAnyMapping()); EXPECT_EQ(graph.getTotalNbMainDofs(), 0); } TEST(MappingGraph, nullRootNode) { - sofa::simulation::MappingGraph graph; - graph.build(sofa::core::MechanicalParams::defaultInstance(), nullptr); + sofa::simulation::MappingGraph2 graph; + graph.build(nullptr); EXPECT_FALSE(graph.isBuilt()); EXPECT_EQ(graph.getRootNode(), nullptr); @@ -58,7 +58,6 @@ TEST(MappingGraph, nullRootNode) EXPECT_TRUE(graph.getTopMostMechanicalStates((sofa::core::behavior::BaseForceField*)nullptr).empty()); EXPECT_TRUE(graph.getTopMostMechanicalStates((sofa::core::behavior::BaseMass*)nullptr).empty()); - EXPECT_TRUE(graph.makeComponentGroups(sofa::core::MechanicalParams::defaultInstance()).empty()); EXPECT_FALSE(graph.hasAnyMapping()); EXPECT_EQ(graph.getTotalNbMainDofs(), 0); } @@ -67,8 +66,8 @@ TEST(MappingGraph, emptyRootNode) { const sofa::simulation::Node::SPtr root = sofa::core::objectmodel::New(); - sofa::simulation::MappingGraph graph; - graph.build(sofa::core::MechanicalParams::defaultInstance(), root.get()); + sofa::simulation::MappingGraph2 graph; + graph.build(root.get()); EXPECT_TRUE(graph.isBuilt()); EXPECT_EQ(graph.getRootNode(), root.get()); @@ -78,7 +77,6 @@ TEST(MappingGraph, emptyRootNode) EXPECT_TRUE(graph.getTopMostMechanicalStates((sofa::core::behavior::BaseForceField*)nullptr).empty()); EXPECT_TRUE(graph.getTopMostMechanicalStates((sofa::core::behavior::BaseMass*)nullptr).empty()); - EXPECT_TRUE(graph.makeComponentGroups(sofa::core::MechanicalParams::defaultInstance()).empty()); EXPECT_FALSE(graph.hasAnyMapping()); EXPECT_EQ(graph.getTotalNbMainDofs(), 0); } @@ -91,8 +89,8 @@ TEST(MappingGraph, oneMechanicalObject) root->addObject(mstate); mstate->resize(10); - sofa::simulation::MappingGraph graph; - graph.build(sofa::core::MechanicalParams::defaultInstance(), root.get()); + sofa::simulation::MappingGraph2 graph; + graph.build(root.get()); EXPECT_TRUE(graph.isBuilt()); EXPECT_EQ(graph.getRootNode(), root.get()); @@ -100,7 +98,6 @@ TEST(MappingGraph, oneMechanicalObject) EXPECT_EQ(graph.getTopMostMechanicalStates(mstate.get()), sofa::type::vector{mstate.get()}); - EXPECT_TRUE(graph.makeComponentGroups(sofa::core::MechanicalParams::defaultInstance()).empty()); EXPECT_FALSE(graph.hasAnyMapping()); EXPECT_EQ(graph.getTotalNbMainDofs(), 30); } @@ -117,8 +114,8 @@ TEST(MappingGraph, twoMechanicalObject) root->addObject(mstate2); mstate2->resize(2); - sofa::simulation::MappingGraph graph; - graph.build(sofa::core::MechanicalParams::defaultInstance(), root.get()); + sofa::simulation::MappingGraph2 graph; + graph.build(root.get()); EXPECT_TRUE(graph.isBuilt()); EXPECT_EQ(graph.getRootNode(), root.get()); @@ -128,7 +125,6 @@ TEST(MappingGraph, twoMechanicalObject) EXPECT_EQ(graph.getTopMostMechanicalStates(mstate1.get()), sofa::type::vector{mstate1.get()}); EXPECT_EQ(graph.getTopMostMechanicalStates(mstate2.get()), sofa::type::vector{mstate2.get()}); - EXPECT_TRUE(graph.makeComponentGroups(sofa::core::MechanicalParams::defaultInstance()).empty()); EXPECT_FALSE(graph.hasAnyMapping()); EXPECT_EQ(graph.getTotalNbMainDofs(), 36); } @@ -151,8 +147,8 @@ TEST(MappingGraph, oneMapping) mapping->setFrom(mstate1.get()); mapping->setTo(mstate2.get()); - sofa::simulation::MappingGraph graph; - graph.build(sofa::core::MechanicalParams::defaultInstance(), root.get()); + sofa::simulation::MappingGraph2 graph; + graph.build(root.get()); EXPECT_TRUE(graph.isBuilt()); EXPECT_EQ(graph.getRootNode(), root.get()); @@ -161,7 +157,6 @@ TEST(MappingGraph, oneMapping) EXPECT_EQ(graph.getTopMostMechanicalStates(mstate1.get()), sofa::type::vector{mstate1.get()}); EXPECT_EQ(graph.getTopMostMechanicalStates(mstate2.get()), sofa::type::vector{mstate1.get()}); - EXPECT_TRUE(graph.makeComponentGroups(sofa::core::MechanicalParams::defaultInstance()).empty()); EXPECT_TRUE(graph.hasAnyMapping()); EXPECT_EQ(graph.getTotalNbMainDofs(), 30); } @@ -213,8 +208,8 @@ TEST(MappingGraph, diamondMapping) {"input", "@/left/left @/right/right"}, {"output", "@bottom"} }); - sofa::simulation::MappingGraph graph; - graph.build(sofa::core::MechanicalParams::defaultInstance(), root.get()); + sofa::simulation::MappingGraph2 graph; + graph.build(root.get()); EXPECT_TRUE(graph.isBuilt()); EXPECT_EQ(graph.getRootNode(), root.get()); @@ -225,7 +220,6 @@ TEST(MappingGraph, diamondMapping) const sofa::type::vector expectedList {top.get(), top.get()}; EXPECT_EQ(graph.getTopMostMechanicalStates(bottom.get()), expectedList); - EXPECT_TRUE(graph.makeComponentGroups(sofa::core::MechanicalParams::defaultInstance()).empty()); EXPECT_TRUE(graph.hasAnyMapping()); EXPECT_EQ(graph.getTotalNbMainDofs(), 3 * 3); From d537b2bc75670575496dd976ce78ba4998007915 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 17 Apr 2026 17:17:45 +0200 Subject: [PATCH 04/41] start replacement --- .../linearsystem/TypedMatrixLinearSystem.inl | 6 +- .../odesolver/forward/EulerExplicitSolver.cpp | 2 +- Sofa/framework/Simulation/Core/CMakeLists.txt | 1 - .../Core/src/sofa/simulation/MappingGraph.cpp | 434 ------------------ .../Core/src/sofa/simulation/MappingGraph.h | 129 +----- .../src/sofa/simulation/MappingGraph2.cpp | 68 +++ .../Core/src/sofa/simulation/MappingGraph2.h | 28 ++ 7 files changed, 105 insertions(+), 563 deletions(-) delete mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp diff --git a/Sofa/Component/LinearSystem/src/sofa/component/linearsystem/TypedMatrixLinearSystem.inl b/Sofa/Component/LinearSystem/src/sofa/component/linearsystem/TypedMatrixLinearSystem.inl index 291532a7306..1f11234f9bd 100644 --- a/Sofa/Component/LinearSystem/src/sofa/component/linearsystem/TypedMatrixLinearSystem.inl +++ b/Sofa/Component/LinearSystem/src/sofa/component/linearsystem/TypedMatrixLinearSystem.inl @@ -71,7 +71,7 @@ void TypedMatrixLinearSystem::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::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::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()) 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 2d3b4fe7e13..649f2c55570 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 @@ -387,7 +387,7 @@ class AllOfMassesAreDiagonalVisitor final : public simulation::BaseMechanicalVis bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams* params) { sofa::simulation::MappingGraph mappingGraph; - mappingGraph.build(params, this->getContext()); + mappingGraph.build(this->getContext()); // To achieve a diagonal global mass matrix in this system: // 1) Each individual mass matrix must itself be diagonal. diff --git a/Sofa/framework/Simulation/Core/CMakeLists.txt b/Sofa/framework/Simulation/Core/CMakeLists.txt index 398979e070a..eda22d34668 100644 --- a/Sofa/framework/Simulation/Core/CMakeLists.txt +++ b/Sofa/framework/Simulation/Core/CMakeLists.txt @@ -170,7 +170,6 @@ set(SOURCE_FILES ${SRC_ROOT}/InitVisitor.cpp ${SRC_ROOT}/IntegrateBeginEvent.cpp ${SRC_ROOT}/IntegrateEndEvent.cpp - ${SRC_ROOT}/MappingGraph.cpp ${SRC_ROOT}/MappingGraph2.cpp ${SRC_ROOT}/MechanicalOperations.cpp ${SRC_ROOT}/MechanicalVPrintVisitor.cpp diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp deleted file mode 100644 index b9735b9e37f..00000000000 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ /dev/null @@ -1,434 +0,0 @@ -/****************************************************************************** -* 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 * -******************************************************************************/ -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace sofa::simulation -{ - -core::objectmodel::BaseContext* MappingGraph::getRootNode() const -{ - return m_rootNode; -} - -const sofa::type::vector& MappingGraph::getMainMechanicalStates() const -{ - return m_mainMechanicalStates; -} - -auto MappingGraph::getTopMostMechanicalStates(core::behavior::BaseMechanicalState* mstate) const -> MappingInputs -{ - if (m_rootNode == nullptr) - { - msg_error("MappingGraph") << "Graph is not built yet"; - } - - if (mstate == nullptr) - { - dmsg_error("MappingGraph") << "Requested mechanical state is invalid"; - return {}; - } - - if (const auto it = m_topMostInputsMechanicalStates.find(mstate); it != m_topMostInputsMechanicalStates.end()) - return it->second; - - return {}; -} - -auto MappingGraph::getTopMostMechanicalStates(core::behavior::BaseForceField* forceField) const -> MappingInputs -{ - if (forceField == nullptr) - { - dmsg_error("MappingGraph") << "Requested force field is invalid"; - return {}; - } - - const auto& associatedMechanicalStates = forceField->getMechanicalStates(); - MappingInputs topMostMechanicalStates; - for (auto* mstate : associatedMechanicalStates) - { - const auto mstates = getTopMostMechanicalStates(mstate); - topMostMechanicalStates.insert(topMostMechanicalStates.end(), mstates.begin(), mstates.end()); - } - return topMostMechanicalStates; -} - -auto MappingGraph::getTopMostMechanicalStates(core::behavior::BaseMass* mass) const -> MappingInputs -{ - if (mass == nullptr) - { - dmsg_error("MappingGraph") << "Requested mass is invalid"; - return {}; - } - - const auto& associatedMechanicalStates = mass->getMechanicalStates(); - MappingInputs topMostMechanicalStates; - for (auto* mstate : associatedMechanicalStates) - { - const auto mstates = getTopMostMechanicalStates(mstate); - topMostMechanicalStates.insert(topMostMechanicalStates.end(), mstates.begin(), mstates.end()); - } - return topMostMechanicalStates; -} - -class ComponentGroupsVisitor final : public simulation::BaseMechanicalVisitor -{ -public: - ComponentGroupsVisitor(const sofa::core::ExecParams* params, MappingGraph::ComponentGroups& groups) - : simulation::BaseMechanicalVisitor(params) - , m_groups(groups) - {} - - Result fwdMass(simulation::Node*, sofa::core::behavior::BaseMass* mass) override - { - if (mass) - { - for (auto mstate : mass->getMechanicalStates()) - { - if (mstate) - { - m_groups[mstate].masses.push_back(mass); - } - } - } - return Result::RESULT_CONTINUE; - } - Result fwdForceField(simulation::Node*, sofa::core::behavior::BaseForceField* ff) override - { - if (ff) - { - for (auto mstate : ff->getMechanicalStates()) - { - if (mstate) - { - m_groups[mstate].forceFields.push_back(ff); - } - } - } - return Result::RESULT_CONTINUE; - } - -private: - MappingGraph::ComponentGroups& m_groups; -}; - -MappingGraph::ComponentGroups MappingGraph::makeComponentGroups(const sofa::core::ExecParams* params) const -{ - ComponentGroups groups; - if (m_rootNode) - { - ComponentGroupsVisitor(params, groups).execute(m_rootNode); - } - return groups; -} - -bool MappingGraph::hasAnyMapping() const -{ - return m_hasAnyMapping; -} - -bool MappingGraph::hasAnyMappingInput(core::behavior::BaseMechanicalState* mstate) const -{ - if (m_rootNode == nullptr) - { - msg_error("MappingGraph") << "Graph is not built yet"; - return false; - } - - if (mstate == nullptr) - { - msg_error("MappingGraph") << "Requested mechanical state is not valid : cannot get its position in the global matrix"; - return false; - } - - //only main (non mapped) mechanical states are in this map - return !m_positionInGlobalMatrix.contains(mstate); -} - -bool MappingGraph::hasAnyMappingInput(core::behavior::BaseForceField* forceField) const -{ - for (auto* mstate : forceField->getMechanicalStates()) - { - if (mstate) - { - if (hasAnyMappingInput(mstate)) - { - return true; - } - } - } - return false; -} - -bool MappingGraph::hasAnyMappingInput(core::behavior::BaseMass* mass) const -{ - for (auto* mstate : mass->getMechanicalStates()) - { - if (mstate) - { - if (hasAnyMappingInput(mstate)) - { - return true; - } - } - } - return false; -} - -bool MappingGraph::isMechanicalStateInContext(core::behavior::BaseMechanicalState* mstate) const -{ - return std::find(m_mechanicalStates.begin(), m_mechanicalStates.end(), mstate) != m_mechanicalStates.end(); -} - -bool MappingGraph::isMappingInput(BaseMechanicalState* mappingInput, BaseMechanicalState* mappingOutput) const -{ - const auto it = m_adjacencyList.find(mappingOutput); - if (it != m_adjacencyList.end()) - { - const auto& inputs = it->second; - if (std::find(inputs.begin(), inputs.end(), mappingInput) != inputs.end()) - { - return true; - } - - for (auto* directInput : inputs) - { - if (isMappingInput(mappingInput, directInput)) - { - return true; - } - } - } - return false; -} - -sofa::type::vector MappingGraph::getMappingInputs(BaseMechanicalState* mstate) const -{ - const auto it = m_adjacencyList.find(mstate); - if (it != m_adjacencyList.end()) - { - return it->second; - } - return {}; -} - -sofa::type::vector MappingGraph::getBottomUpMappingsFrom( - BaseMechanicalState* mstate) const -{ - if (mstate) - { - sofa::type::vector allMappings; - - sofa::type::vector connectedMappings; - for (auto* mapping : m_mappings) - { - if (mapping) - { - for (const auto* child : mapping->getMechTo()) - { - if (child != nullptr && child == mstate) - { - connectedMappings.push_back(mapping); - break; - } - } - } - } - - allMappings.insert(allMappings.end(), connectedMappings.begin(), connectedMappings.end()); - for (auto* mapping : connectedMappings) - { - for (auto* parent : mapping->getMechFrom()) - { - const auto mappings = getBottomUpMappingsFrom(parent); - allMappings.insert(allMappings.end(), mappings.begin(), mappings.end()); - } - } - - return allMappings; - } - - return {}; -} - -type::Vec2u MappingGraph::getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* mstate) const -{ - if (m_rootNode == nullptr) - { - msg_error("MappingGraph") << "Graph is not built yet"; - return type::Vec2u{}; - } - - if (mstate == nullptr) - { - msg_error("MappingGraph") << "Requested mechanical state is not valid : cannot get its position in the global matrix"; - return type::Vec2u{}; - } - - if (const auto it = m_positionInGlobalMatrix.find(mstate); it != m_positionInGlobalMatrix.end()) - return it->second; - - msg_error("MappingGraph") << "Requested mechanical state (" << mstate->getPathName() << - ") is probably mapped or unknown from the graph: only main mechanical states have an associated submatrix in the global matrix"; - return type::Vec2u{}; -} - -type::Vec2u MappingGraph::getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* a, - core::behavior::BaseMechanicalState* b) const -{ - const auto pos_a = getPositionInGlobalMatrix(a); - const auto pos_b = getPositionInGlobalMatrix(b); - return {pos_a[0], pos_b[1]}; -} - -bool MappingGraph::isBuilt() const -{ - return m_rootNode != nullptr; -} - -void MappingGraph::build(const sofa::core::ExecParams* params, core::objectmodel::BaseContext* rootNode) -{ - SOFA_UNUSED(params); - - m_rootNode = rootNode; - - m_mainMechanicalStates.clear(); - m_mappings.clear(); - m_adjacencyList.clear(); - m_topMostInputsMechanicalStates.clear(); - m_positionInGlobalMatrix.clear(); - - m_totalNbMainDofs = 0; - m_hasAnyMapping = false; - - if (m_rootNode) - { - m_rootNode->getObjects(m_mappings, core::objectmodel::BaseContext::SearchDirection::SearchDown); - } - - buildAjacencyList(); - - m_mechanicalStates.clear(); - if (m_rootNode) - { - m_rootNode->getObjects(m_mechanicalStates, core::objectmodel::BaseContext::SearchDirection::SearchDown); - } - - buildMStateRelationships(); -} - -void MappingGraph::buildAjacencyList() -{ - for (auto* mapping : m_mappings) - { - if (mapping) - { - m_hasAnyMapping = true; - - // The mechanical states which are parents of another mechanical state through a mapping are stored in a map for later use - for (auto* child : mapping->getMechTo()) - { - if (child != nullptr) - { - for (auto* parent : mapping->getMechFrom()) - { - if (parent != nullptr) - { - m_adjacencyList[child].push_back(parent); - } - } - } - } - } - } -} - -void MappingGraph::buildMStateRelationships() -{ - for (auto* mstate : m_mechanicalStates) - { - if (mstate == nullptr) - { - return; - } - - auto it = m_adjacencyList.find(mstate); - - if (it == m_adjacencyList.end()) - { - //mstate has not been found in the map: it's not an output of any mapping - const auto matrixSize = mstate->getMatrixSize(); - - m_mainMechanicalStates.push_back(mstate); - m_positionInGlobalMatrix[mstate] = sofa::type::Vec2u(m_totalNbMainDofs, m_totalNbMainDofs); - - m_totalNbMainDofs += matrixSize; - - m_topMostInputsMechanicalStates[mstate].push_back(mstate); - } - else - { - //mstate is the output of at least one mapping and has at least one mechanical state as an input - MappingGraph::MappingInputs inputs = it->second; - if (inputs.empty()) - { - msg_error("MappingGraph") << "Mechanical state " << mstate->getPathName() << " is involved in a mapping, but does not have any valid input mechanical states"; - } - else - { - // continue to check that the input of the mstate is not itself an output of another mstate, and so on - while(!inputs.empty()) - { - auto* visitedMState = inputs.back(); - inputs.pop_back(); - it = m_adjacencyList.find(visitedMState); - if (it != m_adjacencyList.end()) - { - for (auto* p : it->second) - inputs.push_back(p); - } - else - { - if (isMechanicalStateInContext(visitedMState)) - { - m_topMostInputsMechanicalStates[mstate].push_back(visitedMState); - } - else - { - //the top most input is not in the current context - } - } - } - } - - } - } -} -} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 75d0aac75c0..260aab4e8de 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -21,144 +21,25 @@ ******************************************************************************/ #pragma once #include -#include -#include +#include namespace sofa::simulation { +using MappingGraph = MappingGraph2; -using core::behavior::BaseMechanicalState; - -/** - * Connexions between objects through mappings - * - * Graph must be built with the build() function. - */ -class SOFA_SIMULATION_CORE_API MappingGraph -{ - -public: - using MappingInputs = type::vector; - - /// Return the node used to start the exploration of the scene graph in order to build the mapping graph - [[nodiscard]] core::objectmodel::BaseContext* getRootNode() const; - /// Return the list of all mechanical states which are not mapped - [[nodiscard]] const sofa::type::vector& getMainMechanicalStates() const; - - /// Return the list of mechanical states which are: - /// 1) non-mapped - /// 2) input of a mapping involving the provided mechanical state as an output. - /// The search is recursive (more than one level of mapping) and is done during mapping graph construction. - MappingInputs getTopMostMechanicalStates(BaseMechanicalState*) const; - - /// Return the list of mechanical states which are: - /// 1) non-mapped - /// 2) input of a mapping involving the mechanical states associated to the provided force field as an output. - /// The search is recursive (more than one level of mapping) and is done during mapping graph construction. - MappingInputs getTopMostMechanicalStates(core::behavior::BaseForceField*) const; - - /// Return the list of mechanical states which are: - /// 1) non-mapped - /// 2) input of a mapping involving the mechanical states associated to the provided mass as an output. - /// The search is recursive (more than one level of mapping) and is done during mapping graph construction. - MappingInputs getTopMostMechanicalStates(core::behavior::BaseMass*) const; - - struct SameGroupComponents - { - sofa::type::vector forceFields; - sofa::type::vector masses; - }; - - using ComponentGroups = std::map; - - /// Create groups of components associated to the same mechanical state - ComponentGroups makeComponentGroups(const sofa::core::ExecParams* params) const; - - [[nodiscard]] - bool hasAnyMapping() const; - - /// Return true if the provided mechanical state is an output of a mapping - bool hasAnyMappingInput(BaseMechanicalState*) const; - /// Return true if the mechanical states associated to the provided force field is an output of a mapping - bool hasAnyMappingInput(core::behavior::BaseForceField*) const; - /// Return true if the mechanical states associated to the provided mass is an output of a mapping - bool hasAnyMappingInput(core::behavior::BaseMass*) const; - - /// Return true if the provided mechanical state has been visited when building the mapping graph - bool isMechanicalStateInContext(BaseMechanicalState*) const; - - /// Return true if @input is a mapping input of @output. Multiple intermediate mappings are supported - /// In term of graph connectivity, return true if the two nodes of the directed graph are connected - bool isMappingInput(BaseMechanicalState* mappingInput, BaseMechanicalState* mappingOutput) const; - - /// Returns all mechanical states which are input of a mapping where the mechanical state in - /// parameter is an output - MappingInputs getMappingInputs(BaseMechanicalState*) const; - - sofa::type::vector getBottomUpMappingsFrom(BaseMechanicalState*) const; - - /// Return the sum of the degrees of freedom of all main mechanical states - [[nodiscard]] - sofa::Size getTotalNbMainDofs() const { return m_totalNbMainDofs; } - - /// Return where in the global matrix the provided mechanical state writes its contribution - type::Vec2u getPositionInGlobalMatrix(BaseMechanicalState*) const; - /// Return where in the global matrix the provided mechanical states writes its contribution - type::Vec2u getPositionInGlobalMatrix(BaseMechanicalState* a, BaseMechanicalState* b) const; - - MappingGraph() = default; - - bool isBuilt() const; - - /// Build the graph: mandatory to get valid data from the functions that use the graph - void build(const sofa::core::ExecParams* params, core::objectmodel::BaseContext* rootNode); - -private: - - /// node used to start the exploration of the scene graph in order to build the mapping graph - core::objectmodel::BaseContext* m_rootNode { nullptr }; - - /// List of all mechanical states in the root context. - std::vector m_mechanicalStates; - - /// List of all mappings in the root context. - sofa::type::vector m_mappings; - - /// Key: any mechanical state - /// Value: The list of mapping inputs - std::map m_adjacencyList; - - /// List of mechanical states that are non-mapped. They can be involved as a mapping input, but not as an output. - sofa::type::vector m_mainMechanicalStates; - - /// Association between a mechanical state (the key) and a list of mapping input which are non-mapped. In this list, - /// the mechanical states are involved as an input, but not as an output. The mechanical state in the key is an - /// output of mappings (even over multiple levels). - std::map< BaseMechanicalState*, MappingInputs> m_topMostInputsMechanicalStates; - - /// for each main mechanical states, gives the position of its contribution in the global matrix - std::map< BaseMechanicalState*, type::Vec2u > m_positionInGlobalMatrix; - - sofa::Size m_totalNbMainDofs {}; - bool m_hasAnyMapping = false; - - void buildAjacencyList(); - void buildMStateRelationships(); - -}; template class MappingJacobians { - const BaseMechanicalState& m_mappedState; + const core::behavior::BaseMechanicalState& m_mappedState; std::map< core::behavior::BaseMechanicalState*, std::shared_ptr > m_map; public: MappingJacobians() = delete; - MappingJacobians(const BaseMechanicalState& mappedState) : m_mappedState(mappedState) {} + MappingJacobians(const core::behavior::BaseMechanicalState& mappedState) : m_mappedState(mappedState) {} void addJacobianToTopMostParent(std::shared_ptr jacobian, core::behavior::BaseMechanicalState* topMostParent) { @@ -174,4 +55,4 @@ class MappingJacobians } }; -} //namespace sofa::component::linearsolver +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp index 46ba97df931..b5a09111a44 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp @@ -93,6 +93,23 @@ bool MappingGraph2::hasAnyMapping() const { return m_hasAnyMapping; } +bool MappingGraph2::hasAnyMappingInput(core::behavior::BaseMechanicalState* mstate) const +{ + if (m_rootNode == nullptr) + { + msg_error("MappingGraph") << "Graph is not built yet"; + return false; + } + + if (mstate == nullptr) + { + msg_error("MappingGraph") << "Requested mechanical state is not valid : cannot get its position in the global matrix"; + return false; + } + + //only main (non mapped) mechanical states are in this map + return !m_positionInGlobalMatrix.contains(mstate); +} sofa::Size MappingGraph2::getTotalNbMainDofs() const { @@ -129,6 +146,57 @@ type::Vec2u MappingGraph2::getPositionInGlobalMatrix(core::behavior::BaseMechani return {pos_a[0], pos_b[1]}; } +sofa::type::vector MappingGraph2::getBottomUpMappingsFrom( + core::behavior::BaseMechanicalState* state) const +{ + auto* sn = findStateNode(state); + if (sn) + { + struct CollectMapping : public MappingGraphVisitor + { + void visit(core::BaseMapping& mapping) override + { + mappings.push_back(&mapping); + } + + sofa::type::vector mappings; + } visitor; + + for (auto& node : m_allNodes) + { + node->m_pendingCount = 0; + } + + std::queue nodes; + nodes.push(sn); + + while (!nodes.empty()) + { + BaseMappingGraphNode* current = nodes.front(); + nodes.pop(); + + ++(current->m_pendingCount); + + if (current->m_parents.empty()) + { + current->accept(visitor); + } + + for (auto& parent : current->m_parents) + { + if (parent->m_pendingCount == 0) + { + nodes.push(parent.get()); + } + } + } + + return visitor.mappings; + } + + return {}; +} + void MappingGraph2::traverseTopDown(MappingGraphVisitor& visitor) const { // pending count = number of parents not yet visited. diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h index 31437eb0346..4cb38ff4179 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h @@ -159,6 +159,14 @@ class SOFA_SIMULATION_CORE_API MappingGraph2 [[nodiscard]] bool hasAnyMapping() const; + /// Return true if the provided mechanical state is an output of a mapping + bool hasAnyMappingInput(core::behavior::BaseMechanicalState* mstate) const; + + /// Return true if the mechanical states associated with the provided component is an output of a mapping + template requires !std::derived_from + bool hasAnyMappingInput(TComponent*) const; + + /// Return the sum of the degrees of freedom of all main mechanical states [[nodiscard]] sofa::Size getTotalNbMainDofs() const; @@ -168,6 +176,10 @@ class SOFA_SIMULATION_CORE_API MappingGraph2 type::Vec2u getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* a, core::behavior::BaseMechanicalState* b) const; + sofa::type::vector getBottomUpMappingsFrom( + core::behavior::BaseMechanicalState*) const; + + // ------------------------------------------------------------------ // Top-down traversal: roots (unmapped states) → leaves (components). // @@ -256,4 +268,20 @@ MappingGraph2::MappingInputs MappingGraph2::getTopMostMechanicalStates(TComponen return topMostMechanicalStates; } +template requires !std::derived_from +bool MappingGraph2::hasAnyMappingInput(TComponent* component) const +{ + for (auto* mstate : component->getMechanicalStates()) + { + if (mstate) + { + if (hasAnyMappingInput(mstate)) + { + return true; + } + } + } + return false; +} + } // namespace sofa::simulation From 704bab1c24bc149ddb37765a534863765e2fe1ce Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 17 Apr 2026 17:20:03 +0200 Subject: [PATCH 05/41] remove template --- .../src/sofa/simulation/MappingGraph2.cpp | 34 ++++++++++++++ .../Core/src/sofa/simulation/MappingGraph2.h | 44 ++----------------- 2 files changed, 37 insertions(+), 41 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp index b5a09111a44..83cce0889c4 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp @@ -89,6 +89,25 @@ MappingGraph2::MappingInputs MappingGraph2::getTopMostMechanicalStates( return {}; } +MappingGraph2::MappingInputs MappingGraph2::getTopMostMechanicalStates( + core::behavior::StateAccessor* stateAccessor) const +{ + if (stateAccessor == nullptr) + { + dmsg_error("MappingGraph") << "Requested mass is invalid"; + return {}; + } + + const auto& associatedMechanicalStates = stateAccessor->getMechanicalStates(); + MappingInputs topMostMechanicalStates; + for (auto* mstate : associatedMechanicalStates) + { + const auto mstates = getTopMostMechanicalStates(mstate); + topMostMechanicalStates.insert(topMostMechanicalStates.end(), mstates.begin(), mstates.end()); + } + return topMostMechanicalStates; +} + bool MappingGraph2::hasAnyMapping() const { return m_hasAnyMapping; @@ -111,6 +130,21 @@ bool MappingGraph2::hasAnyMappingInput(core::behavior::BaseMechanicalState* msta return !m_positionInGlobalMatrix.contains(mstate); } +bool MappingGraph2::hasAnyMappingInput(core::behavior::StateAccessor* stateAccessor) const +{ + for (auto* mstate : stateAccessor->getMechanicalStates()) + { + if (mstate) + { + if (hasAnyMappingInput(mstate)) + { + return true; + } + } + } + return false; +} + sofa::Size MappingGraph2::getTotalNbMainDofs() const { return m_totalNbMainDofs; diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h index 4cb38ff4179..6b07805babb 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h @@ -152,10 +152,9 @@ class SOFA_SIMULATION_CORE_API MappingGraph2 /// Return the list of mechanical states which are: /// 1) non-mapped - /// 2) input of a mapping involving the mechanical states associated to the provided force field as an output. + /// 2) input of a mapping involving the mechanical states associated to the provided component as an output. /// The search is recursive (more than one level of mapping) - template requires !std::derived_from - MappingInputs getTopMostMechanicalStates(TComponent* component) const; + MappingInputs getTopMostMechanicalStates(core::behavior::StateAccessor* stateAccessor) const; [[nodiscard]] bool hasAnyMapping() const; @@ -163,9 +162,7 @@ class SOFA_SIMULATION_CORE_API MappingGraph2 bool hasAnyMappingInput(core::behavior::BaseMechanicalState* mstate) const; /// Return true if the mechanical states associated with the provided component is an output of a mapping - template requires !std::derived_from - bool hasAnyMappingInput(TComponent*) const; - + bool hasAnyMappingInput(core::behavior::StateAccessor* stateAccessor) const; /// Return the sum of the degrees of freedom of all main mechanical states [[nodiscard]] sofa::Size getTotalNbMainDofs() const; @@ -249,39 +246,4 @@ class SOFA_SIMULATION_CORE_API MappingGraph2 static void addEdge(BaseMappingGraphNode* from, BaseMappingGraphNode* to); }; -template requires !std::derived_from -MappingGraph2::MappingInputs MappingGraph2::getTopMostMechanicalStates(TComponent* component) const -{ - if (component == nullptr) - { - dmsg_error("MappingGraph") << "Requested mass is invalid"; - return {}; - } - - const auto& associatedMechanicalStates = component->getMechanicalStates(); - MappingInputs topMostMechanicalStates; - for (auto* mstate : associatedMechanicalStates) - { - const auto mstates = getTopMostMechanicalStates(mstate); - topMostMechanicalStates.insert(topMostMechanicalStates.end(), mstates.begin(), mstates.end()); - } - return topMostMechanicalStates; -} - -template requires !std::derived_from -bool MappingGraph2::hasAnyMappingInput(TComponent* component) const -{ - for (auto* mstate : component->getMechanicalStates()) - { - if (mstate) - { - if (hasAnyMappingInput(mstate)) - { - return true; - } - } - } - return false; -} - } // namespace sofa::simulation From f67153ec1eaa30fee382b9390963b939cbfc7506 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 17 Apr 2026 17:24:25 +0200 Subject: [PATCH 06/41] continue replacement --- Sofa/framework/Simulation/Core/CMakeLists.txt | 3 +- .../{MappingGraph2.cpp => MappingGraph.cpp} | 54 ++-- .../Core/src/sofa/simulation/MappingGraph.h | 221 +++++++++++++++- .../Core/src/sofa/simulation/MappingGraph2.h | 249 ------------------ .../Core/test/MappingGraph2_test.cpp | 28 +- .../Core/test/MappingGraph_test.cpp | 16 +- 6 files changed, 268 insertions(+), 303 deletions(-) rename Sofa/framework/Simulation/Core/src/sofa/simulation/{MappingGraph2.cpp => MappingGraph.cpp} (86%) delete mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h diff --git a/Sofa/framework/Simulation/Core/CMakeLists.txt b/Sofa/framework/Simulation/Core/CMakeLists.txt index eda22d34668..e16319d1d4e 100644 --- a/Sofa/framework/Simulation/Core/CMakeLists.txt +++ b/Sofa/framework/Simulation/Core/CMakeLists.txt @@ -67,7 +67,6 @@ set(HEADER_FILES ${SRC_ROOT}/SceneCheckRegistry.h ${SRC_ROOT}/SceneCheckMainRegistry.h ${SRC_ROOT}/MappingGraph.h - ${SRC_ROOT}/MappingGraph2.h ${SRC_ROOT}/events/BuildConstraintSystemEndEvent.h ${SRC_ROOT}/events/SimulationInitDoneEvent.h @@ -170,7 +169,7 @@ set(SOURCE_FILES ${SRC_ROOT}/InitVisitor.cpp ${SRC_ROOT}/IntegrateBeginEvent.cpp ${SRC_ROOT}/IntegrateEndEvent.cpp - ${SRC_ROOT}/MappingGraph2.cpp + ${SRC_ROOT}/MappingGraph.cpp ${SRC_ROOT}/MechanicalOperations.cpp ${SRC_ROOT}/MechanicalVPrintVisitor.cpp ${SRC_ROOT}/MechanicalVisitor.cpp diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp similarity index 86% rename from Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp rename to Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 83cce0889c4..68041fbde2d 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -1,11 +1,11 @@ -#include +#include #include namespace sofa::simulation { -MappingGraph2::InputLists MappingGraph2::InputLists::makeFromNode(core::objectmodel::BaseContext* node) +MappingGraph::InputLists MappingGraph::InputLists::makeFromNode(core::objectmodel::BaseContext* node) { InputLists inputLists; if (node) @@ -18,27 +18,27 @@ MappingGraph2::InputLists MappingGraph2::InputLists::makeFromNode(core::objectmo return inputLists; } -MappingGraph2::MappingGraph2(const InputLists& input) +MappingGraph::MappingGraph(const InputLists& input) { build(input); } -MappingGraph2::MappingGraph2(core::objectmodel::BaseContext* node) +MappingGraph::MappingGraph(core::objectmodel::BaseContext* node) { build(node); } -core::objectmodel::BaseContext* MappingGraph2::getRootNode() const +core::objectmodel::BaseContext* MappingGraph::getRootNode() const { return m_rootNode; } const sofa::type::vector& -MappingGraph2::getMainMechanicalStates() const +MappingGraph::getMainMechanicalStates() const { return m_roots; } -MappingGraph2::MappingInputs MappingGraph2::getTopMostMechanicalStates( +MappingGraph::MappingInputs MappingGraph::getTopMostMechanicalStates( core::behavior::BaseMechanicalState* state) const { auto* sn = findStateNode(state); @@ -89,7 +89,7 @@ MappingGraph2::MappingInputs MappingGraph2::getTopMostMechanicalStates( return {}; } -MappingGraph2::MappingInputs MappingGraph2::getTopMostMechanicalStates( +MappingGraph::MappingInputs MappingGraph::getTopMostMechanicalStates( core::behavior::StateAccessor* stateAccessor) const { if (stateAccessor == nullptr) @@ -108,11 +108,11 @@ MappingGraph2::MappingInputs MappingGraph2::getTopMostMechanicalStates( return topMostMechanicalStates; } -bool MappingGraph2::hasAnyMapping() const +bool MappingGraph::hasAnyMapping() const { return m_hasAnyMapping; } -bool MappingGraph2::hasAnyMappingInput(core::behavior::BaseMechanicalState* mstate) const +bool MappingGraph::hasAnyMappingInput(core::behavior::BaseMechanicalState* mstate) const { if (m_rootNode == nullptr) { @@ -130,7 +130,7 @@ bool MappingGraph2::hasAnyMappingInput(core::behavior::BaseMechanicalState* msta return !m_positionInGlobalMatrix.contains(mstate); } -bool MappingGraph2::hasAnyMappingInput(core::behavior::StateAccessor* stateAccessor) const +bool MappingGraph::hasAnyMappingInput(core::behavior::StateAccessor* stateAccessor) const { for (auto* mstate : stateAccessor->getMechanicalStates()) { @@ -145,12 +145,12 @@ bool MappingGraph2::hasAnyMappingInput(core::behavior::StateAccessor* stateAcces return false; } -sofa::Size MappingGraph2::getTotalNbMainDofs() const +sofa::Size MappingGraph::getTotalNbMainDofs() const { return m_totalNbMainDofs; } -type::Vec2u MappingGraph2::getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* mstate) const +type::Vec2u MappingGraph::getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* mstate) const { if (m_rootNode == nullptr) { @@ -172,7 +172,7 @@ type::Vec2u MappingGraph2::getPositionInGlobalMatrix(core::behavior::BaseMechani return type::Vec2u{}; } -type::Vec2u MappingGraph2::getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* a, +type::Vec2u MappingGraph::getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* a, core::behavior::BaseMechanicalState* b) const { const auto pos_a = getPositionInGlobalMatrix(a); @@ -180,7 +180,7 @@ type::Vec2u MappingGraph2::getPositionInGlobalMatrix(core::behavior::BaseMechani return {pos_a[0], pos_b[1]}; } -sofa::type::vector MappingGraph2::getBottomUpMappingsFrom( +sofa::type::vector MappingGraph::getBottomUpMappingsFrom( core::behavior::BaseMechanicalState* state) const { auto* sn = findStateNode(state); @@ -231,7 +231,7 @@ sofa::type::vector MappingGraph2::getBottomUpMappingsFrom( return {}; } -void MappingGraph2::traverseTopDown(MappingGraphVisitor& visitor) const +void MappingGraph::traverseTopDown(MappingGraphVisitor& visitor) const { // pending count = number of parents not yet visited. std::queue ready; @@ -247,7 +247,7 @@ void MappingGraph2::traverseTopDown(MappingGraphVisitor& visitor) const processQueue(ready, visitor, /*topDown=*/true); } -void MappingGraph2::traverseBottomUp(MappingGraphVisitor& visitor) const +void MappingGraph::traverseBottomUp(MappingGraphVisitor& visitor) const { std::queue ready; for (auto& node : m_allNodes) @@ -282,7 +282,7 @@ void MappingGraph2::traverseBottomUp(MappingGraphVisitor& visitor) const } } -void MappingGraph2::traverseComponentGroups(MappingGraphVisitor& visitor) const +void MappingGraph::traverseComponentGroups(MappingGraphVisitor& visitor) const { for (auto& [states, node] : m_groupIndex) { @@ -292,7 +292,7 @@ void MappingGraph2::traverseComponentGroups(MappingGraphVisitor& visitor) const } } } -void MappingGraph2::traverseComponentGroups(MappingGraphVisitor& visitor, +void MappingGraph::traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const { if (taskScheduler) @@ -312,7 +312,7 @@ void MappingGraph2::traverseComponentGroups(MappingGraphVisitor& visitor, } } -void MappingGraph2::processQueue(std::queue& ready, MappingGraphVisitor& visitor, +void MappingGraph::processQueue(std::queue& ready, MappingGraphVisitor& visitor, bool topDown) { while (!ready.empty()) @@ -336,7 +336,7 @@ void MappingGraph2::processQueue(std::queue& ready, Mappi } -bool MappingGraph2::isBuilt() const { return m_isBuilt; } +bool MappingGraph::isBuilt() const { return m_isBuilt; } template MappingGraphNode::SPtr makeMappingGraphNode(typename TComponent::SPtr s) @@ -344,7 +344,7 @@ MappingGraphNode::SPtr makeMappingGraphNode(typename TComponent::SPt return MappingGraphNode::SPtr( new MappingGraphNode(s) ); } -void MappingGraph2::build(const InputLists& input) +void MappingGraph::build(const InputLists& input) { m_hasAnyMapping = input.mappings.size() > 0; @@ -438,7 +438,7 @@ void MappingGraph2::build(const InputLists& input) m_isBuilt = true; } -void MappingGraph2::build(core::objectmodel::BaseContext* rootNode) +void MappingGraph::build(core::objectmodel::BaseContext* rootNode) { if (rootNode) { @@ -447,7 +447,7 @@ void MappingGraph2::build(core::objectmodel::BaseContext* rootNode) m_rootNode = rootNode; } -ComponentGroupMappingGraphNode::SPtr MappingGraph2::findGroupNode( +ComponentGroupMappingGraphNode::SPtr MappingGraph::findGroupNode( const std::vector& states) { auto it = std::find_if(m_groupIndex.begin(), m_groupIndex.end(), @@ -469,7 +469,7 @@ ComponentGroupMappingGraphNode::SPtr MappingGraph2::findGroupNode( return group; } -ComponentGroupMappingGraphNode::SPtr MappingGraph2::findInGroupNodes( +ComponentGroupMappingGraphNode::SPtr MappingGraph::findInGroupNodes( const core::behavior::BaseMechanicalState::SPtr state) { auto it = std::find_if(m_groupIndex.begin(), m_groupIndex.end(), @@ -482,13 +482,13 @@ ComponentGroupMappingGraphNode::SPtr MappingGraph2::findInGroupNodes( return nullptr; } -BaseMappingGraphNode* MappingGraph2::findStateNode(core::behavior::BaseMechanicalState* raw) const +BaseMappingGraphNode* MappingGraph::findStateNode(core::behavior::BaseMechanicalState* raw) const { auto it = m_stateIndex.find(raw); return (it != m_stateIndex.end()) ? it->second : nullptr; } -void MappingGraph2::addEdge(BaseMappingGraphNode* from, BaseMappingGraphNode* to) +void MappingGraph::addEdge(BaseMappingGraphNode* from, BaseMappingGraphNode* to) { from->m_children.push_back(to->shared_from_this()); to->m_parents.push_back(from->shared_from_this()); diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 260aab4e8de..84366cd7fc2 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -21,12 +21,227 @@ ******************************************************************************/ #pragma once #include - -#include +#include +#include +#include +#include +#include +#include namespace sofa::simulation { -using MappingGraph = MappingGraph2; +class TaskScheduler; + +// --------------------------------------------------------------------------- +// Visitor interface +// --------------------------------------------------------------------------- +class MappingGraphVisitor +{ +public: + virtual ~MappingGraphVisitor() = default; + + virtual void visit(core::behavior::BaseMechanicalState&) {} + virtual void visit(core::BaseMapping&) {} + virtual void visit(core::behavior::BaseForceField&) {} + virtual void visit(core::behavior::BaseMass&) {} +}; + +// --------------------------------------------------------------------------- +// Graph node +// --------------------------------------------------------------------------- +class BaseMappingGraphNode : public std::enable_shared_from_this +{ +public: + using SPtr = std::shared_ptr; + friend class MappingGraph; + virtual ~BaseMappingGraphNode() = default; + + virtual void accept(MappingGraphVisitor& visitor) const = 0; + + virtual std::string getName() const { return {}; } + +private: + sofa::type::vector m_parents; // prerequisite nodes + sofa::type::vector m_children; // dependent nodes + + // Mutable counter used during traversal (reset before each traversal). + mutable int m_pendingCount = 0; +}; + +template +class MappingGraphNode : public BaseMappingGraphNode +{ +public: + using SPtr = std::shared_ptr; + friend class MappingGraph; + explicit MappingGraphNode(typename TComponent::SPtr s) + : m_component(std::move(s)) + {} + + void accept(MappingGraphVisitor& visitor) const override + { + if (m_component) + { + visitor.visit(*m_component); + } + } + + std::string getName() const override + { + return m_component->getName(); + } + +private: + typename TComponent::SPtr m_component; +}; + +class ComponentGroupMappingGraphNode : public BaseMappingGraphNode +{ +public: + using SPtr = std::shared_ptr; + void accept(MappingGraphVisitor& visitor) const override { SOFA_UNUSED(visitor); } + + std::string getName() const override + { + return "group"; + } +}; + +// --------------------------------------------------------------------------- +// Mapping graph +// --------------------------------------------------------------------------- +class SOFA_SIMULATION_CORE_API MappingGraph +{ +public: + using MappingInputs = type::vector; + + // ------------------------------------------------------------------ + // Input: flat lists collected from the scene DAG (or any other source). + // Add further leaf component lists as needed. + // ------------------------------------------------------------------ + struct SOFA_SIMULATION_CORE_API InputLists + { + sofa::type::vector mechanicalStates; + sofa::type::vector mappings; + sofa::type::vector forceFields; + sofa::type::vector masses; + + static InputLists makeFromNode(core::objectmodel::BaseContext* node); + static InputLists makeFromNode(core::objectmodel::BaseContext::SPtr node) { return makeFromNode(node.get()); } + }; + + MappingGraph() = default; + explicit MappingGraph(const InputLists& input); + explicit MappingGraph(core::objectmodel::BaseContext* node); + + /// Return the node used to start the exploration of the scene graph in order to build the mapping graph + [[nodiscard]] core::objectmodel::BaseContext* getRootNode() const; + + /// Return the list of all mechanical states that are not mapped + [[nodiscard]] const sofa::type::vector& getMainMechanicalStates() const; + + /// Return the list of mechanical states which are: + /// 1) non-mapped + /// 2) input of a mapping involving the provided mechanical state as an output. + /// The search is recursive (more than one level of mapping). + MappingInputs getTopMostMechanicalStates(core::behavior::BaseMechanicalState* state) const; + + /// Return the list of mechanical states which are: + /// 1) non-mapped + /// 2) input of a mapping involving the mechanical states associated to the provided component as an output. + /// The search is recursive (more than one level of mapping) + MappingInputs getTopMostMechanicalStates(core::behavior::StateAccessor* stateAccessor) const; + + [[nodiscard]] bool hasAnyMapping() const; + + /// Return true if the provided mechanical state is an output of a mapping + bool hasAnyMappingInput(core::behavior::BaseMechanicalState* mstate) const; + + /// Return true if the mechanical states associated with the provided component is an output of a mapping + bool hasAnyMappingInput(core::behavior::StateAccessor* stateAccessor) const; + + /// Return the sum of the degrees of freedom of all main mechanical states + [[nodiscard]] sofa::Size getTotalNbMainDofs() const; + + /// Return where in the global matrix the provided mechanical state writes its contribution + type::Vec2u getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* mstate) const; + /// Return where in the global matrix the provided mechanical states writes its contribution + type::Vec2u getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* a, + core::behavior::BaseMechanicalState* b) const; + + sofa::type::vector getBottomUpMappingsFrom( + core::behavior::BaseMechanicalState*) const; + + + // ------------------------------------------------------------------ + // Top-down traversal: roots (unmapped states) → leaves (components). + // + // Invariants: + // • A BaseMechanicalState is visited only after every BaseMapping + // that produces it as output has been visited. + // • A BaseMapping is visited only after every input + // BaseMechanicalState has been visited. + // • A leaf component is visited only after every BaseMechanicalState + // connected to it has been visited. + // • Each node is visited exactly once. + // ------------------------------------------------------------------ + void traverseTopDown(MappingGraphVisitor& visitor) const; + + // ------------------------------------------------------------------ + // Bottom-up traversal: leaves → roots. + // + // Mirror invariants of top-down, reversed. + // ------------------------------------------------------------------ + void traverseBottomUp(MappingGraphVisitor& visitor) const; + + void traverseComponentGroups(MappingGraphVisitor& visitor) const; + void traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const; + + [[nodiscard]] bool isBuilt() const; + + // ------------------------------------------------------------------ + // Graph construction + // ------------------------------------------------------------------ + void build(const InputLists& input); + void build(core::objectmodel::BaseContext* rootNode); + +private: + /// node used to start the exploration of the scene graph in order to build the mapping graph + core::objectmodel::BaseContext* m_rootNode { nullptr }; + + bool m_isBuilt = false; + bool m_hasAnyMapping = false; + + sofa::Size m_totalNbMainDofs {}; + + /// for each main mechanical states, gives the position of its contribution in the global matrix + std::map m_positionInGlobalMatrix; + + std::vector m_allNodes; + sofa::type::vector m_roots {}; + std::unordered_map m_stateIndex; + std::vector, + ComponentGroupMappingGraphNode::SPtr>> m_groupIndex; + + // ------------------------------------------------------------------ + // Kahn-style BFS used by both traversal directions. + // topDown == true → follow children, decrement their pendingCount. + // topDown == false → follow parents, decrement their pendingCount. + // ------------------------------------------------------------------ + static void processQueue(std::queue& ready, + MappingGraphVisitor& visitor, + bool topDown); + + ComponentGroupMappingGraphNode::SPtr findGroupNode(const std::vector& states); + ComponentGroupMappingGraphNode::SPtr findInGroupNodes(const core::behavior::BaseMechanicalState::SPtr state); + BaseMappingGraphNode* findStateNode(core::behavior::BaseMechanicalState* raw) const; + + // Directed edge from → to. + // Both parent and child lists hold SPtr so the graph owns all nodes. + static void addEdge(BaseMappingGraphNode* from, BaseMappingGraphNode* to); +}; + template diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h deleted file mode 100644 index 6b07805babb..00000000000 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph2.h +++ /dev/null @@ -1,249 +0,0 @@ -/****************************************************************************** -* 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 -#include -#include -#include - -#include -#include -#include -#include - -namespace sofa::simulation -{ -class TaskScheduler; - -// --------------------------------------------------------------------------- -// Visitor interface -// --------------------------------------------------------------------------- -class MappingGraphVisitor -{ -public: - virtual ~MappingGraphVisitor() = default; - - virtual void visit(core::behavior::BaseMechanicalState&) {} - virtual void visit(core::BaseMapping&) {} - virtual void visit(core::behavior::BaseForceField&) {} - virtual void visit(core::behavior::BaseMass&) {} -}; - -// --------------------------------------------------------------------------- -// Graph node -// --------------------------------------------------------------------------- -class BaseMappingGraphNode : public std::enable_shared_from_this -{ -public: - using SPtr = std::shared_ptr; - friend class MappingGraph2; - virtual ~BaseMappingGraphNode() = default; - - virtual void accept(MappingGraphVisitor& visitor) const = 0; - - virtual std::string getName() const { return {}; } - -private: - sofa::type::vector m_parents; // prerequisite nodes - sofa::type::vector m_children; // dependent nodes - - // Mutable counter used during traversal (reset before each traversal). - mutable int m_pendingCount = 0; -}; - -template -class MappingGraphNode : public BaseMappingGraphNode -{ -public: - using SPtr = std::shared_ptr; - friend class MappingGraph2; - explicit MappingGraphNode(typename TComponent::SPtr s) - : m_component(std::move(s)) - {} - - void accept(MappingGraphVisitor& visitor) const override - { - if (m_component) - { - visitor.visit(*m_component); - } - } - - std::string getName() const override - { - return m_component->getName(); - } - -private: - typename TComponent::SPtr m_component; -}; - -class ComponentGroupMappingGraphNode : public BaseMappingGraphNode -{ -public: - using SPtr = std::shared_ptr; - void accept(MappingGraphVisitor& visitor) const override { SOFA_UNUSED(visitor); } - - std::string getName() const override - { - return "group"; - } -}; - -// --------------------------------------------------------------------------- -// Mapping graph -// --------------------------------------------------------------------------- -class SOFA_SIMULATION_CORE_API MappingGraph2 -{ -public: - using MappingInputs = type::vector; - - // ------------------------------------------------------------------ - // Input: flat lists collected from the scene DAG (or any other source). - // Add further leaf component lists as needed. - // ------------------------------------------------------------------ - struct SOFA_SIMULATION_CORE_API InputLists - { - sofa::type::vector mechanicalStates; - sofa::type::vector mappings; - sofa::type::vector forceFields; - sofa::type::vector masses; - - static InputLists makeFromNode(core::objectmodel::BaseContext* node); - static InputLists makeFromNode(core::objectmodel::BaseContext::SPtr node) { return makeFromNode(node.get()); } - }; - - MappingGraph2() = default; - explicit MappingGraph2(const InputLists& input); - explicit MappingGraph2(core::objectmodel::BaseContext* node); - - /// Return the node used to start the exploration of the scene graph in order to build the mapping graph - [[nodiscard]] core::objectmodel::BaseContext* getRootNode() const; - - /// Return the list of all mechanical states that are not mapped - [[nodiscard]] const sofa::type::vector& getMainMechanicalStates() const; - - /// Return the list of mechanical states which are: - /// 1) non-mapped - /// 2) input of a mapping involving the provided mechanical state as an output. - /// The search is recursive (more than one level of mapping). - MappingInputs getTopMostMechanicalStates(core::behavior::BaseMechanicalState* state) const; - - /// Return the list of mechanical states which are: - /// 1) non-mapped - /// 2) input of a mapping involving the mechanical states associated to the provided component as an output. - /// The search is recursive (more than one level of mapping) - MappingInputs getTopMostMechanicalStates(core::behavior::StateAccessor* stateAccessor) const; - - [[nodiscard]] bool hasAnyMapping() const; - - /// Return true if the provided mechanical state is an output of a mapping - bool hasAnyMappingInput(core::behavior::BaseMechanicalState* mstate) const; - - /// Return true if the mechanical states associated with the provided component is an output of a mapping - bool hasAnyMappingInput(core::behavior::StateAccessor* stateAccessor) const; - - /// Return the sum of the degrees of freedom of all main mechanical states - [[nodiscard]] sofa::Size getTotalNbMainDofs() const; - - /// Return where in the global matrix the provided mechanical state writes its contribution - type::Vec2u getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* mstate) const; - /// Return where in the global matrix the provided mechanical states writes its contribution - type::Vec2u getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* a, - core::behavior::BaseMechanicalState* b) const; - - sofa::type::vector getBottomUpMappingsFrom( - core::behavior::BaseMechanicalState*) const; - - - // ------------------------------------------------------------------ - // Top-down traversal: roots (unmapped states) → leaves (components). - // - // Invariants: - // • A BaseMechanicalState is visited only after every BaseMapping - // that produces it as output has been visited. - // • A BaseMapping is visited only after every input - // BaseMechanicalState has been visited. - // • A leaf component is visited only after every BaseMechanicalState - // connected to it has been visited. - // • Each node is visited exactly once. - // ------------------------------------------------------------------ - void traverseTopDown(MappingGraphVisitor& visitor) const; - - // ------------------------------------------------------------------ - // Bottom-up traversal: leaves → roots. - // - // Mirror invariants of top-down, reversed. - // ------------------------------------------------------------------ - void traverseBottomUp(MappingGraphVisitor& visitor) const; - - void traverseComponentGroups(MappingGraphVisitor& visitor) const; - void traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const; - - [[nodiscard]] bool isBuilt() const; - - // ------------------------------------------------------------------ - // Graph construction - // ------------------------------------------------------------------ - void build(const InputLists& input); - void build(core::objectmodel::BaseContext* rootNode); - -private: - /// node used to start the exploration of the scene graph in order to build the mapping graph - core::objectmodel::BaseContext* m_rootNode { nullptr }; - - bool m_isBuilt = false; - bool m_hasAnyMapping = false; - - sofa::Size m_totalNbMainDofs {}; - - /// for each main mechanical states, gives the position of its contribution in the global matrix - std::map m_positionInGlobalMatrix; - - std::vector m_allNodes; - sofa::type::vector m_roots {}; - std::unordered_map m_stateIndex; - std::vector, - ComponentGroupMappingGraphNode::SPtr>> m_groupIndex; - - // ------------------------------------------------------------------ - // Kahn-style BFS used by both traversal directions. - // topDown == true → follow children, decrement their pendingCount. - // topDown == false → follow parents, decrement their pendingCount. - // ------------------------------------------------------------------ - static void processQueue(std::queue& ready, - MappingGraphVisitor& visitor, - bool topDown); - - ComponentGroupMappingGraphNode::SPtr findGroupNode(const std::vector& states); - ComponentGroupMappingGraphNode::SPtr findInGroupNodes(const core::behavior::BaseMechanicalState::SPtr state); - BaseMappingGraphNode* findStateNode(core::behavior::BaseMechanicalState* raw) const; - - // Directed edge from → to. - // Both parent and child lists hold SPtr so the graph owns all nodes. - static void addEdge(BaseMappingGraphNode* from, BaseMappingGraphNode* to); -}; - -} // namespace sofa::simulation diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index b8ddd668459..638c0700d41 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -22,21 +22,21 @@ #include #include #include -#include +#include namespace sofa { TEST(MappingGraph, DefaultConstructor) { - sofa::simulation::MappingGraph2 mappingGraph; + sofa::simulation::MappingGraph mappingGraph; EXPECT_FALSE(mappingGraph.isBuilt()); } TEST(MappingGraph, Build) { - sofa::simulation::MappingGraph2 mappingGraph; - sofa::simulation::MappingGraph2::InputLists input; + sofa::simulation::MappingGraph mappingGraph; + sofa::simulation::MappingGraph::InputLists input; mappingGraph.build(input); EXPECT_TRUE(mappingGraph.isBuilt()); } @@ -73,9 +73,9 @@ TEST(MappingGraph, SingleState) sofa::simpleapi::importPlugin(Sofa.Component.StateContainer); sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state"}}); - auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root.get()); + auto inputs = sofa::simulation::MappingGraph::InputLists::makeFromNode(root.get()); ASSERT_EQ(inputs.mechanicalStates.size(), 1); - sofa::simulation::MappingGraph2 mappingGraph(inputs); + sofa::simulation::MappingGraph mappingGraph(inputs); ASSERT_TRUE(mappingGraph.isBuilt()); CollectNamesVisitor visitor; @@ -96,10 +96,10 @@ TEST(MappingGraph, SingleMappingInSingleNode) sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state2"}}); sofa::simpleapi::createObject(root, "IdentityMapping", {{"name", "mapping"}, {"input", "@state1"}, {"output", "@state2"}}); - auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root.get()); + auto inputs = sofa::simulation::MappingGraph::InputLists::makeFromNode(root.get()); ASSERT_EQ(inputs.mappings.size(), 1); ASSERT_EQ(inputs.mechanicalStates.size(), 2); - sofa::simulation::MappingGraph2 mappingGraph(inputs); + sofa::simulation::MappingGraph mappingGraph(inputs); ASSERT_TRUE(mappingGraph.isBuilt()); CollectNamesVisitor visitor; @@ -130,10 +130,10 @@ TEST(MappingGraph, SingleMappingWithIntermediateNode) sofa::simpleapi::createObject(node1, "MechanicalObject", {{"name", "state2"}}); sofa::simpleapi::createObject(node1, "IdentityMapping", {{"name", "mapping"}, {"input", "@state1"}, {"output", "@state2"}}); - auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root); + auto inputs = sofa::simulation::MappingGraph::InputLists::makeFromNode(root); ASSERT_EQ(inputs.mappings.size(), 1); ASSERT_EQ(inputs.mechanicalStates.size(), 2); - sofa::simulation::MappingGraph2 mappingGraph(inputs); + sofa::simulation::MappingGraph mappingGraph(inputs); ASSERT_TRUE(mappingGraph.isBuilt()); CollectNamesVisitor visitor; @@ -164,10 +164,10 @@ TEST(MappingGraph, SingleMappingWithIntermediateNodeInverseInputOutput) sofa::simpleapi::createObject(node1, "MechanicalObject", {{"name", "state2"}}); sofa::simpleapi::createObject(node1, "IdentityMapping", {{"name", "mapping"}, {"input", "@state2"}, {"output", "@state1"}}); - auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root); + auto inputs = sofa::simulation::MappingGraph::InputLists::makeFromNode(root); ASSERT_EQ(inputs.mappings.size(), 1); ASSERT_EQ(inputs.mechanicalStates.size(), 2); - sofa::simulation::MappingGraph2 mappingGraph(inputs); + sofa::simulation::MappingGraph mappingGraph(inputs); ASSERT_TRUE(mappingGraph.isBuilt()); CollectNamesVisitor visitor; @@ -206,11 +206,11 @@ TEST(MappingGraph, ComplexGraph) sofa::simulation::node::initRoot(root.get()); - auto inputs = sofa::simulation::MappingGraph2::InputLists::makeFromNode(root); + auto inputs = sofa::simulation::MappingGraph::InputLists::makeFromNode(root); ASSERT_EQ(inputs.mappings.size(), 1); ASSERT_EQ(inputs.mechanicalStates.size(), 2); ASSERT_EQ(inputs.forceFields.size(), 4); - sofa::simulation::MappingGraph2 mappingGraph(inputs); + sofa::simulation::MappingGraph mappingGraph(inputs); ASSERT_TRUE(mappingGraph.isBuilt()); CollectNamesVisitor visitor; diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph_test.cpp index 7c01a3f7483..9ca599c1a67 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph_test.cpp @@ -21,7 +21,7 @@ ******************************************************************************/ #include #include -#include +#include #include #include #include @@ -31,7 +31,7 @@ TEST(MappingGraph, noBuild) { - const sofa::simulation::MappingGraph2 graph; + const sofa::simulation::MappingGraph graph; EXPECT_FALSE(graph.isBuilt()); EXPECT_EQ(graph.getRootNode(), nullptr); @@ -47,7 +47,7 @@ TEST(MappingGraph, noBuild) TEST(MappingGraph, nullRootNode) { - sofa::simulation::MappingGraph2 graph; + sofa::simulation::MappingGraph graph; graph.build(nullptr); EXPECT_FALSE(graph.isBuilt()); @@ -66,7 +66,7 @@ TEST(MappingGraph, emptyRootNode) { const sofa::simulation::Node::SPtr root = sofa::core::objectmodel::New(); - sofa::simulation::MappingGraph2 graph; + sofa::simulation::MappingGraph graph; graph.build(root.get()); EXPECT_TRUE(graph.isBuilt()); @@ -89,7 +89,7 @@ TEST(MappingGraph, oneMechanicalObject) root->addObject(mstate); mstate->resize(10); - sofa::simulation::MappingGraph2 graph; + sofa::simulation::MappingGraph graph; graph.build(root.get()); EXPECT_TRUE(graph.isBuilt()); @@ -114,7 +114,7 @@ TEST(MappingGraph, twoMechanicalObject) root->addObject(mstate2); mstate2->resize(2); - sofa::simulation::MappingGraph2 graph; + sofa::simulation::MappingGraph graph; graph.build(root.get()); EXPECT_TRUE(graph.isBuilt()); @@ -147,7 +147,7 @@ TEST(MappingGraph, oneMapping) mapping->setFrom(mstate1.get()); mapping->setTo(mstate2.get()); - sofa::simulation::MappingGraph2 graph; + sofa::simulation::MappingGraph graph; graph.build(root.get()); EXPECT_TRUE(graph.isBuilt()); @@ -208,7 +208,7 @@ TEST(MappingGraph, diamondMapping) {"input", "@/left/left @/right/right"}, {"output", "@bottom"} }); - sofa::simulation::MappingGraph2 graph; + sofa::simulation::MappingGraph graph; graph.build(root.get()); EXPECT_TRUE(graph.isBuilt()); From 78bfb644f748662fd71a218154345b60caaa1081 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 17 Apr 2026 19:14:35 +0200 Subject: [PATCH 07/41] try to fix on non-Windows --- .../Simulation/Core/src/sofa/simulation/MappingGraph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 68041fbde2d..dae8aa66e0a 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -339,9 +339,9 @@ void MappingGraph::processQueue(std::queue& ready, Mappin bool MappingGraph::isBuilt() const { return m_isBuilt; } template -MappingGraphNode::SPtr makeMappingGraphNode(typename TComponent::SPtr s) +typename MappingGraphNode::SPtr makeMappingGraphNode(typename TComponent::SPtr s) { - return MappingGraphNode::SPtr( new MappingGraphNode(s) ); + return typename MappingGraphNode::SPtr( new MappingGraphNode(s) ); } void MappingGraph::build(const InputLists& input) From 9c2b407905ee43e8b39089d32d4a5b3e464e8f1e Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Mon, 27 Apr 2026 16:10:28 +0200 Subject: [PATCH 08/41] fix getBottomUpMappingsFrom --- .../Simulation/Core/src/sofa/simulation/MappingGraph.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index dae8aa66e0a..dddb79e261b 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -186,7 +186,7 @@ sofa::type::vector MappingGraph::getBottomUpMappingsFrom( auto* sn = findStateNode(state); if (sn) { - struct CollectMapping : public MappingGraphVisitor + struct CollectMapping final : public MappingGraphVisitor { void visit(core::BaseMapping& mapping) override { @@ -211,10 +211,7 @@ sofa::type::vector MappingGraph::getBottomUpMappingsFrom( ++(current->m_pendingCount); - if (current->m_parents.empty()) - { - current->accept(visitor); - } + current->accept(visitor); for (auto& parent : current->m_parents) { From 9b97f754de1ceba3a41b3c1bb8141861b5ea20b1 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Mon, 27 Apr 2026 16:24:58 +0200 Subject: [PATCH 09/41] documentation --- .../Core/src/sofa/simulation/MappingGraph.h | 376 ++++++++++++++---- 1 file changed, 308 insertions(+), 68 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 84366cd7fc2..6d79351c811 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -19,6 +19,18 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ + +/** + * @file MappingGraph.h + * @brief Implements a mapping graph structure for simulating mechanical systems in SOFA. + * + * This header defines the core structures necessary to represent how various + * physical components and behavioral states are linked together during simulation. + * It allows for top-down (prerequisite check) and bottom-up (dependency accumulation) + * traversals of a component hierarchy, ensuring that inputs are calculated correctly + * before they are needed by dependent nodes or mappings. + */ + #pragma once #include #include @@ -32,52 +44,106 @@ namespace sofa::simulation { class TaskScheduler; -// --------------------------------------------------------------------------- -// Visitor interface -// --------------------------------------------------------------------------- +/** + * @brief Visitor interface for traversing the mapping graph. + * + * This visitor pattern is used to allow algorithms to process nodes in a + * structured way without modifying their structure or coupling traversal logic + * with specific component types. Implementations must override visit methods + * corresponding to the behaviors they intend to handle. + */ class MappingGraphVisitor { public: virtual ~MappingGraphVisitor() = default; + /** + * @brief Visits a mechanical state node. + * @param mstate The mechanical state component to visit. + */ virtual void visit(core::behavior::BaseMechanicalState&) {} + + /** + * @brief Visits a base mapping node. + * @param mapping The mapping component to visit. + */ virtual void visit(core::BaseMapping&) {} + + /** + * @brief Visits a force field behavior node. + * @param ff The force field component to visit. + */ virtual void visit(core::behavior::BaseForceField&) {} + + /** + * @brief Visits a mass behavior node. + * @param m The mass component to visit. + */ virtual void visit(core::behavior::BaseMass&) {} }; -// --------------------------------------------------------------------------- -// Graph node -// --------------------------------------------------------------------------- +/** + * @brief Abstract base class for nodes in the mapping graph. + * + * Provides common functionality for tracking prerequisites (parents) and dependencies + * (children), forming a Directed Acyclic Graph (DAG). It implements the Visitor pattern. + */ class BaseMappingGraphNode : public std::enable_shared_from_this { public: using SPtr = std::shared_ptr; friend class MappingGraph; + virtual ~BaseMappingGraphNode() = default; + /** + * @brief Accepts a visitor, allowing the graph to be processed by an external algorithm. + * @param visitor The concrete visitor implementation. + */ virtual void accept(MappingGraphVisitor& visitor) const = 0; + /** + * @brief Gets the name of the component represented by this node. + * @return The name string. + */ virtual std::string getName() const { return {}; } private: - sofa::type::vector m_parents; // prerequisite nodes - sofa::type::vector m_children; // dependent nodes + sofa::type::vector m_parents; ///< prerequisite nodes (nodes pointing to this one) + sofa::type::vector m_children; ///< dependent nodes (nodes pointed from this one) // Mutable counter used during traversal (reset before each traversal). mutable int m_pendingCount = 0; }; +/** + * @brief Template class representing a graph node associated with any SOFA component. + * + * This wrapper allows the `MappingGraph` to manage nodes for various types of + * components (e.g., MechanicalState, ForceField) polymorphically while maintaining + * type safety and providing standard graph node interfaces. + * + * @tparam TComponent The actual SOFA component class pointer type. + */ template class MappingGraphNode : public BaseMappingGraphNode { public: using SPtr = std::shared_ptr; friend class MappingGraph; + + /** + * @brief Constructs a node wrapper for the given component. + * @param s A shared pointer to the component instance. + */ explicit MappingGraphNode(typename TComponent::SPtr s) : m_component(std::move(s)) {} + /** + * @brief Implements accept by calling visit on the wrapped component. + * @param visitor The concrete visitor implementation. + */ void accept(MappingGraphVisitor& visitor) const override { if (m_component) @@ -86,89 +152,183 @@ class MappingGraphNode : public BaseMappingGraphNode } } + /** + * @brief Returns the name of the wrapped component. + * @return The name string from the component. + */ std::string getName() const override { return m_component->getName(); } private: - typename TComponent::SPtr m_component; + typename TComponent::SPtr m_component; ///< The actual SOFA component instance pointer. }; +/** + * @brief A node wrapper used for representing groups of components or abstract groupings. + */ class ComponentGroupMappingGraphNode : public BaseMappingGraphNode { public: using SPtr = std::shared_ptr; + void accept(MappingGraphVisitor& visitor) const override { SOFA_UNUSED(visitor); } + /** + * @brief Returns the fixed name "group" for this type of node. + * @return The string "group". + */ std::string getName() const override { return "group"; } }; -// --------------------------------------------------------------------------- -// Mapping graph -// --------------------------------------------------------------------------- +/** + * @brief Represents the overall mechanical simulation graph structure (Mapping Graph). + * + * This class builds and manages a dependency graph connecting all major components + * (MechanicalStates, Mappings, ForceFields, Masses) within an SOFA scene. + * It allows for systematic traversal (Top-down/Bottom-up) to determine the correct + * order of calculation required during simulation initialization or execution. + */ class SOFA_SIMULATION_CORE_API MappingGraph { public: using MappingInputs = type::vector; - // ------------------------------------------------------------------ - // Input: flat lists collected from the scene DAG (or any other source). - // Add further leaf component lists as needed. - // ------------------------------------------------------------------ + /** + * @brief Container struct holding lists of all potential input components + * collected from a scene context. + */ struct SOFA_SIMULATION_CORE_API InputLists { - sofa::type::vector mechanicalStates; - sofa::type::vector mappings; - sofa::type::vector forceFields; - sofa::type::vector masses; - + sofa::type::vector mechanicalStates; ///< All Mechanical State inputs. + sofa::type::vector mappings; ///< All Mapping components. + sofa::type::vector forceFields; ///< All Force Field components. + sofa::type::vector masses; ///< All Mass components. + + /** + * @brief Creates InputLists from a context pointer. + * @param node The SOFA object model base context associated with the component list. + * @return A populated InputLists structure. + */ static InputLists makeFromNode(core::objectmodel::BaseContext* node); + + /** + * @brief Creates InputLists from a shared pointer context. + * @param node The SOFA object model base context smart pointer. + * @return A populated InputLists structure. + */ static InputLists makeFromNode(core::objectmodel::BaseContext::SPtr node) { return makeFromNode(node.get()); } }; + /** + * @brief Default constructor initializes an empty graph. + */ MappingGraph() = default; + + /** + * @brief Constructs the graph using pre-collected input lists. + * @param input The list of all components found in the scene. + */ explicit MappingGraph(const InputLists& input); + + /** + * @brief Constructs the graph by traversing a starting node in the SOFA object model context. + * @param node The root context node from which to build the graph. + */ explicit MappingGraph(core::objectmodel::BaseContext* node); - /// Return the node used to start the exploration of the scene graph in order to build the mapping graph + /** + * @brief Returns the root node used during the initial construction of the graph. + * @return A pointer to the root object model context. + */ [[nodiscard]] core::objectmodel::BaseContext* getRootNode() const; - /// Return the list of all mechanical states that are not mapped + /** + * @brief Gets the list of all main mechanical states that are not used as outputs + * in any mapping (i.e., they are root inputs). + * @return Const reference to the vector of non-mapped mechanical state pointers. + */ [[nodiscard]] const sofa::type::vector& getMainMechanicalStates() const; - /// Return the list of mechanical states which are: - /// 1) non-mapped - /// 2) input of a mapping involving the provided mechanical state as an output. - /// The search is recursive (more than one level of mapping). + /** + * @brief Recursively finds top-most mechanical states that are unmapped but serve as + * inputs to a mapping involving the provided state. + * + * This search is recursive, handling multiple levels of dependencies. + * + * @param state The starting mechanical state for dependency checking. + * @return A list of top-most unmapped mechanical states required by `state`. + */ MappingInputs getTopMostMechanicalStates(core::behavior::BaseMechanicalState* state) const; - /// Return the list of mechanical states which are: - /// 1) non-mapped - /// 2) input of a mapping involving the mechanical states associated to the provided component as an output. - /// The search is recursive (more than one level of mapping) + /** + * @brief Recursively finds top-most mechanical states that are unmapped but serve as + * inputs to a mapping involving the mechanical states associated with a given accessor. + * + * This search is recursive, handling multiple levels of dependencies. + * + * @param stateAccessor The starting state accessor for dependency checking. + * @return A list of top-most unmapped mechanical states required by `stateAccessor`. + */ MappingInputs getTopMostMechanicalStates(core::behavior::StateAccessor* stateAccessor) const; + /** + * @brief Checks if any mapping exists anywhere in the graph structure. + * @return True if at least one mapping is present, false otherwise. + */ [[nodiscard]] bool hasAnyMapping() const; - /// Return true if the provided mechanical state is an output of a mapping + /** + * @brief Determines if a specific mechanical state is an output of any mapping node + * connected to the graph. + * @param mstate The mechanical state to check. + * @return True if `mstate` is an output, false otherwise. + */ bool hasAnyMappingInput(core::behavior::BaseMechanicalState* mstate) const; - /// Return true if the mechanical states associated with the provided component is an output of a mapping + /** + * @brief Determines if the mechanical states associated with a component are outputs + * of any mapping node connected to the graph. + * @param stateAccessor The state accessor for the component to check. + * @return True if the associated states are mapped output, false otherwise. + */ bool hasAnyMappingInput(core::behavior::StateAccessor* stateAccessor) const; - /// Return the sum of the degrees of freedom of all main mechanical states + /** + * @brief Calculates the total number of degrees of freedom (DoF) contributed + * by all main mechanical states in the graph. + * @return The sum of DoFs across all primary states. + */ [[nodiscard]] sofa::Size getTotalNbMainDofs() const; - /// Return where in the global matrix the provided mechanical state writes its contribution + /** + * @brief Finds the global matrix indices (row/column) where a specific state + * contributes its degrees of freedom. + * @param mstate The mechanical state. + * @return A pair representing (global row index, global column index). + */ type::Vec2u getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* mstate) const; - /// Return where in the global matrix the provided mechanical states writes its contribution - type::Vec2u getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* a, - core::behavior::BaseMechanicalState* b) const; + /** + * @brief Finds the global matrix indices where two specified states contribute + * their degrees of freedom. (Used for cross-axis checks). + * @param a The first mechanical state. + * @param b The second mechanical state. + * @return A pair representing (global row index, global column index) for the combined contribution. + */ + type::Vec2u getPositionInGlobalMatrix(core::behavior::BaseMechanicalState* a, + core::behavior::BaseMechanicalState* b) const; + + /** + * @brief Retrieves all mapping components that depend on (receive input from) + * the provided mechanical state. This is used for bottom-up dependency checks. + * @param mstate The mechanical state acting as an input source. + * @return A vector of shared pointers to dependent BaseMapping nodes. + */ sofa::type::vector getBottomUpMappingsFrom( core::behavior::BaseMechanicalState*) const; @@ -176,91 +336,171 @@ class SOFA_SIMULATION_CORE_API MappingGraph // ------------------------------------------------------------------ // Top-down traversal: roots (unmapped states) → leaves (components). // - // Invariants: - // • A BaseMechanicalState is visited only after every BaseMapping - // that produces it as output has been visited. - // • A BaseMapping is visited only after every input - // BaseMechanicalState has been visited. - // • A leaf component is visited only after every BaseMechanicalState - // connected to it has been visited. - // • Each node is visited exactly once. - // ------------------------------------------------------------------ + // Ensures that a BaseMechanicalState is only processed after all mappings + // that produce it as output have been processed, and similarly for mappings + // and leaf components. This guarantees correct dependency order. void traverseTopDown(MappingGraphVisitor& visitor) const; // ------------------------------------------------------------------ // Bottom-up traversal: leaves → roots. // - // Mirror invariants of top-down, reversed. - // ------------------------------------------------------------------ + // Provides the reverse dependency ordering check, ensuring that prerequisite + // states are processed before the components that require them. void traverseBottomUp(MappingGraphVisitor& visitor) const; + /** + * @brief Performs a full graph traversal specifically designed to visit and process + * component groups defined in the scene hierarchy. + * @param visitor The concrete visitor implementation. + */ void traverseComponentGroups(MappingGraphVisitor& visitor) const; + + /** + * @brief Performs a full graph traversal for component groups, optionally coordinating + * with a TaskScheduler to manage execution order. + * @param visitor The concrete visitor implementation. + * @param taskScheduler Optional scheduler instance for tasks requiring explicit ordering. + */ void traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const; + /** + * @brief Checks if the graph has been successfully built and analyzed. + * @return True if building is complete, false otherwise. + */ [[nodiscard]] bool isBuilt() const; // ------------------------------------------------------------------ - // Graph construction + // Graph construction methods: // ------------------------------------------------------------------ + + /** + * @brief Builds the mapping graph using a provided set of input components. + * @param input The collected list of all potential SOFA components. + */ void build(const InputLists& input); + + /** + * @brief Builds the mapping graph by traversing and collecting components + * starting from a specific root node in the object model hierarchy. + * @param rootNode The starting context node. + */ void build(core::objectmodel::BaseContext* rootNode); private: - /// node used to start the exploration of the scene graph in order to build the mapping graph + ///< Root node used to start graph exploration during construction. core::objectmodel::BaseContext* m_rootNode { nullptr }; - bool m_isBuilt = false; - bool m_hasAnyMapping = false; + bool m_isBuilt = false; ///< Flag indicating if the graph structure is finalized. + bool m_hasAnyMapping = false; ///< Flag indicating if any mapping exists in the graph. - sofa::Size m_totalNbMainDofs {}; + sofa::Size m_totalNbMainDofs {}; ///< Total number of primary degrees of freedom managed by the system. - /// for each main mechanical states, gives the position of its contribution in the global matrix + /** + * @brief Map storing the global indices (row, column) for each main mechanical state's contribution matrix block. + */ std::map m_positionInGlobalMatrix; - std::vector m_allNodes; - sofa::type::vector m_roots {}; - std::unordered_map m_stateIndex; + // Graph ownership structures: + std::vector m_allNodes; ///< All nodes in the graph. + sofa::type::vector m_roots {}; ///< List of initial, unmapped mechanical states (graph roots). + std::unordered_map m_stateIndex; ///< Quick lookup for a state's node. std::vector, - ComponentGroupMappingGraphNode::SPtr>> m_groupIndex; + ComponentGroupMappingGraphNode::SPtr>> m_groupIndex; ///< Indexing mechanism for group nodes. // ------------------------------------------------------------------ - // Kahn-style BFS used by both traversal directions. - // topDown == true → follow children, decrement their pendingCount. - // topDown == false → follow parents, decrement their pendingCount. - // ------------------------------------------------------------------ + /** + * @brief Performs a breadth-first search (BFS) traversal, processing nodes in dependency order. + * + * This static helper method is used for both top-down and bottom-up traversals. + * + * @param ready The queue of nodes that are currently ready to be visited/processed. + * @param visitor The visitor implementation used by the process. + * @param topDown If true, processes children (top-down); if false, processes parents (bottom-up). + */ static void processQueue(std::queue& ready, MappingGraphVisitor& visitor, bool topDown); + /** + * @brief Locates or creates a component group node encompassing the given set of states. + * @param states The mechanical states belonging to the group. + * @return A shared pointer to the found/created ComponentGroupMappingGraphNode. + */ ComponentGroupMappingGraphNode::SPtr findGroupNode(const std::vector& states); + + /** + * @brief Locates or creates a component group node for a single state's context. + * @param state The mechanical state defining the scope of the group. + * @return A shared pointer to the found/created ComponentGroupMappingGraphNode. + */ ComponentGroupMappingGraphNode::SPtr findInGroupNodes(const core::behavior::BaseMechanicalState::SPtr state); + + /** + * @brief Finds the graph node corresponding to a raw mechanical state pointer. + * @param raw The mechanical state raw pointer. + * @return Pointer to the associated BaseMappingGraphNode, or nullptr if not found. + */ BaseMappingGraphNode* findStateNode(core::behavior::BaseMechanicalState* raw) const; - // Directed edge from → to. - // Both parent and child lists hold SPtr so the graph owns all nodes. + /** + * @brief Adds a directed edge between two nodes in the graph structure (from -> to). + * + * Both 'from' and 'to' pointers are added to the respective parent/child lists, + * ensuring that the graph manages ownership of all nodes via `SPtr`. + * @param from The starting node. + * @param to The ending node. + */ static void addEdge(BaseMappingGraphNode* from, BaseMappingGraphNode* to); }; - +/** + * @brief Manages Jacobian matrix contributions for a single mechanical state. + * + * This class holds and retrieves the Jacobian matrices calculated during + * graph construction. It maps input states (which require Jacobians) to their + * corresponding Jacobian calculation object. + * + * @tparam JacobianMatrixType The concrete type used for the Jacobian matrix structure. + */ template class MappingJacobians { - const core::behavior::BaseMechanicalState& m_mappedState; + const core::behavior::BaseMechanicalState& m_mappedState; ///< The mechanical state whose Jacobians are being managed. + /** + * @brief Map from an input mechanical state to its calculated Jacobian matrix. + */ std::map< core::behavior::BaseMechanicalState*, std::shared_ptr > m_map; public: - + /** + * @brief Deleted constructor enforces usage via the parameterized constructor. + */ MappingJacobians() = delete; + + /** + * @brief Constructs the Jacobian manager for a specific mechanical state. + * @param mappedState Reference to the mechanical state being managed. + */ MappingJacobians(const core::behavior::BaseMechanicalState& mappedState) : m_mappedState(mappedState) {} + /** + * @brief Associates a calculated Jacobian matrix with a top-most parent state. + * @param jacobian The shared pointer to the Jacobian matrix. + * @param topMostParent The mechanical state that uses this Jacobian as input. + */ void addJacobianToTopMostParent(std::shared_ptr jacobian, core::behavior::BaseMechanicalState* topMostParent) { m_map[topMostParent] = jacobian; } + /** + * @brief Retrieves the Jacobian matrix associated with a given mechanical state. + * @param mstate The state whose Jacobian is requested. + * @return A shared pointer to the Jacobian, or nullptr if not found. + */ std::shared_ptr getJacobianFrom(core::behavior::BaseMechanicalState* mstate) const { const auto it = m_map.find(mstate); From ace5a9dfe84ab295b6f549919f04cded05e8f971 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Tue, 28 Apr 2026 10:29:48 +0200 Subject: [PATCH 10/41] remove debug log --- .../Simulation/Core/src/sofa/simulation/MappingGraph.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index dddb79e261b..92ab4e00e32 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -452,7 +452,6 @@ ComponentGroupMappingGraphNode::SPtr MappingGraph::findGroupNode( if (it != m_groupIndex.end()) return it->second; - std::cout << "Creating group node for " << sofa::helper::join(states.begin(), states.end(), [](auto state){ return state->getName(); }, ", ") << std::endl; auto group = ComponentGroupMappingGraphNode::SPtr{new ComponentGroupMappingGraphNode}; for (const auto& state : states) { From 7ee1ecc7c10b199cfb08c2ae1914f6535ce246ab Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Tue, 28 Apr 2026 16:44:11 +0200 Subject: [PATCH 11/41] factorization --- .../Core/src/sofa/simulation/MappingGraph.cpp | 74 +++++-------------- .../Core/src/sofa/simulation/MappingGraph.h | 30 ++++++-- 2 files changed, 43 insertions(+), 61 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 92ab4e00e32..bf63d51cb79 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -1,7 +1,8 @@ #include - #include +#include + namespace sofa::simulation { @@ -36,7 +37,7 @@ core::objectmodel::BaseContext* MappingGraph::getRootNode() const const sofa::type::vector& MappingGraph::getMainMechanicalStates() const { - return m_roots; + return m_rootStates; } MappingGraph::MappingInputs MappingGraph::getTopMostMechanicalStates( core::behavior::BaseMechanicalState* state) const @@ -230,7 +231,12 @@ sofa::type::vector MappingGraph::getBottomUpMappingsFrom( void MappingGraph::traverseTopDown(MappingGraphVisitor& visitor) const { - // pending count = number of parents not yet visited. + std::queue ready = prepareRootForTraversal(); + processQueue(ready, [&visitor](const BaseMappingGraphNode* node){ node->accept(visitor); }); +} + +std::queue MappingGraph::prepareRootForTraversal() const +{ std::queue ready; for (auto& node : m_allNodes) { @@ -241,41 +247,23 @@ void MappingGraph::traverseTopDown(MappingGraphVisitor& visitor) const } } - processQueue(ready, visitor, /*topDown=*/true); + return ready; } void MappingGraph::traverseBottomUp(MappingGraphVisitor& visitor) const { - std::queue ready; - for (auto& node : m_allNodes) - { - node->m_pendingCount = static_cast(node->m_parents.size()); - if (node->m_pendingCount == 0) - { - ready.push(node.get()); - } - } + //the strategy consists in traversing the graph from top to bottom and + //register the traversed nodes in a list. The bottom-up traversal corresponds to the + //reversed list. - sofa::type::vector nodes; - while (!ready.empty()) - { - BaseMappingGraphNode* current = ready.front(); - nodes.push_back(current); - ready.pop(); + std::queue ready = prepareRootForTraversal(); - for (auto& child : current->m_children) - { - --(child->m_pendingCount); - if (child->m_pendingCount == 0) - { - ready.push(child.get()); - } - } - } + sofa::type::vector nodes; + processQueue(ready, [&nodes](BaseMappingGraphNode* node){ nodes.push_back(node); }); - for (auto it = nodes.crbegin(); it != nodes.crend(); ++it) + for (const auto* node : std::views::reverse(nodes)) { - (*it)->accept(visitor); + node->accept(visitor); } } @@ -309,30 +297,6 @@ void MappingGraph::traverseComponentGroups(MappingGraphVisitor& visitor, } } -void MappingGraph::processQueue(std::queue& ready, MappingGraphVisitor& visitor, - bool topDown) -{ - while (!ready.empty()) - { - BaseMappingGraphNode* current = ready.front(); - std::cout << "Processing node: " << current->getName() << std::endl; - ready.pop(); - - current->accept(visitor); - - const auto& neighbours = topDown ? current->m_children : current->m_parents; - for (auto& neighbour : neighbours) - { - --(neighbour->m_pendingCount); - if (neighbour->m_pendingCount == 0) - { - ready.push(neighbour.get()); - } - } - } -} - - bool MappingGraph::isBuilt() const { return m_isBuilt; } template @@ -426,7 +390,7 @@ void MappingGraph::build(const InputLists& input) { if (node->m_parents.empty()) { - m_roots.push_back(node->m_component.get()); + m_rootStates.push_back(node->m_component.get()); m_positionInGlobalMatrix[node->m_component.get()] = type::Vec2u(m_totalNbMainDofs, m_totalNbMainDofs); m_totalNbMainDofs += node->m_component->getMatrixSize(); } diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 6d79351c811..af0279c8acf 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -402,25 +402,43 @@ class SOFA_SIMULATION_CORE_API MappingGraph // Graph ownership structures: std::vector m_allNodes; ///< All nodes in the graph. - sofa::type::vector m_roots {}; ///< List of initial, unmapped mechanical states (graph roots). + sofa::type::vector m_rootStates {}; ///< List of initial, unmapped mechanical states (graph roots). std::unordered_map m_stateIndex; ///< Quick lookup for a state's node. std::vector, ComponentGroupMappingGraphNode::SPtr>> m_groupIndex; ///< Indexing mechanism for group nodes. // ------------------------------------------------------------------ + + std::queue prepareRootForTraversal() const; + /** * @brief Performs a breadth-first search (BFS) traversal, processing nodes in dependency order. * * This static helper method is used for both top-down and bottom-up traversals. * * @param ready The queue of nodes that are currently ready to be visited/processed. - * @param visitor The visitor implementation used by the process. - * @param topDown If true, processes children (top-down); if false, processes parents (bottom-up). */ - static void processQueue(std::queue& ready, - MappingGraphVisitor& visitor, - bool topDown); + template + static void processQueue(std::queue& ready, const Callable& f) + { + while (!ready.empty()) + { + BaseMappingGraphNode* current = ready.front(); + ready.pop(); + + f(current); + + for (auto& child : current->m_children) + { + --(child->m_pendingCount); + if (child->m_pendingCount == 0) + { + ready.push(child.get()); + } + } + } + } /** * @brief Locates or creates a component group node encompassing the given set of states. From a78d3d0701d5fe595203ab7c5894cdfb5937aec2 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Tue, 28 Apr 2026 17:17:21 +0200 Subject: [PATCH 12/41] know if a node is mapped --- .../Core/src/sofa/simulation/MappingGraph.cpp | 18 +++++++--- .../Core/src/sofa/simulation/MappingGraph.h | 33 +++++++++++++++++-- 2 files changed, 45 insertions(+), 6 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index bf63d51cb79..7113ddba7f3 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -6,6 +6,14 @@ namespace sofa::simulation { +bool BaseMappingGraphNode::isMapped() const +{ + return std::any_of(m_parents.begin(), m_parents.end(), [](const SPtr& node) + { + return node->getType() == NodeType::Mapping || node->isMapped(); + }); +} + MappingGraph::InputLists MappingGraph::InputLists::makeFromNode(core::objectmodel::BaseContext* node) { InputLists inputLists; @@ -241,7 +249,7 @@ std::queue MappingGraph::prepareRootForTraversal() const for (auto& node : m_allNodes) { node->m_pendingCount = static_cast(node->m_parents.size()); - if (node->m_pendingCount == 0) + if (node->m_pendingCount == 0) //node without any parent -> a root { ready.push(node.get()); } @@ -256,10 +264,12 @@ void MappingGraph::traverseBottomUp(MappingGraphVisitor& visitor) const //register the traversed nodes in a list. The bottom-up traversal corresponds to the //reversed list. - std::queue ready = prepareRootForTraversal(); - sofa::type::vector nodes; - processQueue(ready, [&nodes](BaseMappingGraphNode* node){ nodes.push_back(node); }); + nodes.reserve(m_allNodes.size()); + { + std::queue ready = prepareRootForTraversal(); + processQueue(ready, [&nodes](BaseMappingGraphNode* node){ nodes.push_back(node); }); + } for (const auto* node : std::views::reverse(nodes)) { diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index af0279c8acf..892df0409df 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -108,7 +108,21 @@ class BaseMappingGraphNode : public std::enable_shared_from_this m_parents; ///< prerequisite nodes (nodes pointing to this one) sofa::type::vector m_children; ///< dependent nodes (nodes pointed from this one) @@ -161,7 +175,17 @@ class MappingGraphNode : public BaseMappingGraphNode return m_component->getName(); } -private: + NodeType getType() const override + { + if constexpr (std::is_base_of_v) + return NodeType::MechanicalState; + else if constexpr (std::is_base_of_v) + return NodeType::Mapping; + else + return NodeType::Component; + } + + private: typename TComponent::SPtr m_component; ///< The actual SOFA component instance pointer. }; @@ -183,6 +207,11 @@ class ComponentGroupMappingGraphNode : public BaseMappingGraphNode { return "group"; } + + NodeType getType() const override + { + return NodeType::Group; + } }; /** From 9503690519f736a5157cb5d8d11957f1c24bb5da Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 09:41:20 +0200 Subject: [PATCH 13/41] Enhance graph traversal with scoped visitation Adds support for targeted node traversal in `MappingGraph::traverseTopDown` using defined scopes: * `ONLY_MAPPED_NODES`: Visits only components involved in mappings. * `ONLY_MAIN_NODES`: Visits component groups that do not rely on external mapping inputs. This change updates the API to provide fine-grained control over which nodes are processed during traversal, and includes new tests validating these scoped behaviors. --- .../Core/src/sofa/simulation/MappingGraph.cpp | 25 +++++- .../Core/src/sofa/simulation/MappingGraph.h | 14 +++- .../Core/test/MappingGraph2_test.cpp | 81 ++++++++++++++++++- 3 files changed, 111 insertions(+), 9 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 7113ddba7f3..2adc13a68bc 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -237,10 +237,31 @@ sofa::type::vector MappingGraph::getBottomUpMappingsFrom( return {}; } -void MappingGraph::traverseTopDown(MappingGraphVisitor& visitor) const +void MappingGraph::traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope) const { std::queue ready = prepareRootForTraversal(); - processQueue(ready, [&visitor](const BaseMappingGraphNode* node){ node->accept(visitor); }); + processQueue(ready, [&visitor, this, scope](const BaseMappingGraphNode* node){ + bool should_visit = true; + + switch (scope) + { + case VisitorApplication::ONLY_MAPPED_NODES: + should_visit = node->isMapped(); + break; + case VisitorApplication::ONLY_MAIN_NODES: + should_visit = !node->isMapped(); + break; + case VisitorApplication::ALL_NODES: + default: + should_visit = true; // Visit all nodes. + break; + } + + if (should_visit) + { + node->accept(visitor); + } + }); } std::queue MappingGraph::prepareRootForTraversal() const diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 892df0409df..4c112e971b8 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -44,6 +44,13 @@ namespace sofa::simulation { class TaskScheduler; +enum class VisitorApplication +{ + ALL_NODES, + ONLY_MAPPED_NODES, + ONLY_MAIN_NODES +}; + /** * @brief Visitor interface for traversing the mapping graph. * @@ -368,7 +375,7 @@ class SOFA_SIMULATION_CORE_API MappingGraph // Ensures that a BaseMechanicalState is only processed after all mappings // that produce it as output have been processed, and similarly for mappings // and leaf components. This guarantees correct dependency order. - void traverseTopDown(MappingGraphVisitor& visitor) const; + void traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; // ------------------------------------------------------------------ // Bottom-up traversal: leaves → roots. @@ -378,15 +385,14 @@ class SOFA_SIMULATION_CORE_API MappingGraph void traverseBottomUp(MappingGraphVisitor& visitor) const; /** - * @brief Performs a full graph traversal specifically designed to visit and process - * component groups defined in the scene hierarchy. + * @brief Visit and process component groups without any specific order. * @param visitor The concrete visitor implementation. */ void traverseComponentGroups(MappingGraphVisitor& visitor) const; /** * @brief Performs a full graph traversal for component groups, optionally coordinating - * with a TaskScheduler to manage execution order. + * with a TaskScheduler to manage execution parallelism. * @param visitor The concrete visitor implementation. * @param taskScheduler Optional scheduler instance for tasks requiring explicit ordering. */ diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index 638c0700d41..04520893d0d 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -186,27 +186,48 @@ TEST(MappingGraph, SingleMappingWithIntermediateNodeInverseInputOutput) EXPECT_EQ(visitor.names[2], "state2"); } -TEST(MappingGraph, ComplexGraph) +/** + * @brief Sets up the complex graph environment for testing, creating nodes and components. + * + * @return std::tuple A tuple containing the root node pointer and collected input lists. + */ +auto setupComplexGraphEnvironment() -> std::pair { - const sofa::simulation::Node::SPtr root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); - + // Setup common plugins required for both complex graph tests sofa::simpleapi::importPlugin(Sofa.Component.Mapping.Linear); sofa::simpleapi::importPlugin(Sofa.Component.StateContainer); sofa::simpleapi::importPlugin(Sofa.Component.MechanicalLoad); sofa::simpleapi::importPlugin(Sofa.Component.Mass); + const sofa::simulation::Node::SPtr root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + + // Components on the root node (state1) sofa::simpleapi::createObject(root, "MechanicalObject", {{"name", "state1"}}); sofa::simpleapi::createObject(root, "ConstantForceField", {{"name", "ff1"}, {"state", "@state1"}, {"forces", "1 0 0"}}); sofa::simpleapi::createObject(root, "UniformMass", {{"name", "mass1"}}); + + // Components on the child node (state2) const auto node1 = root->createChild("node1"); sofa::simpleapi::createObject(node1, "MechanicalObject", {{"name", "state2"}}); sofa::simpleapi::createObject(node1, "ConstantForceField", {{"name", "ff2"}, {"state", "@state2"}, {"forces", "1 0 0"}}); sofa::simpleapi::createObject(node1, "UniformMass", {{"name", "mass2"}}); + + // Mapping connecting state1 to state2 sofa::simpleapi::createObject(node1, "IdentityMapping", {{"name", "mapping"}, {"input", "@state1"}, {"output", "@state2"}}); + // Initialize the root node structure sofa::simulation::node::initRoot(root.get()); auto inputs = sofa::simulation::MappingGraph::InputLists::makeFromNode(root); + return {root, inputs}; +} + + +TEST(MappingGraph, ComplexGraph) +{ + // Setup environment using helper function + auto [root, inputs] = setupComplexGraphEnvironment(); + ASSERT_EQ(inputs.mappings.size(), 1); ASSERT_EQ(inputs.mechanicalStates.size(), 2); ASSERT_EQ(inputs.forceFields.size(), 4); @@ -215,6 +236,7 @@ TEST(MappingGraph, ComplexGraph) CollectNamesVisitor visitor; + // Top Down Traversal Check mappingGraph.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 9); // 9 and not 7 because a UniformMass is a BaseMass and also a BaseForceField @@ -230,6 +252,7 @@ TEST(MappingGraph, ComplexGraph) EXPECT_EQ(visitor.names[8], "mass2"); visitor.names.clear(); + // Bottom Up Traversal Check mappingGraph.traverseBottomUp(visitor); ASSERT_EQ(visitor.names.size(), 9); @@ -245,4 +268,56 @@ TEST(MappingGraph, ComplexGraph) EXPECT_EQ(visitor.names[8], "state1"); } +/** + * @brief Tests scoped traversal using different VisitorApplication scopes, limiting results to mapped nodes only. + */ +TEST(MappingGraph, ComplexGraph_OnlyMappedNodes) +{ + // Setup environment using helper function + auto [root, inputs] = setupComplexGraphEnvironment(); + + ASSERT_EQ(inputs.mappings.size(), 1); + ASSERT_EQ(inputs.mechanicalStates.size(), 2); + ASSERT_EQ(inputs.forceFields.size(), 4); + sofa::simulation::MappingGraph mappingGraph(inputs); + ASSERT_TRUE(mappingGraph.isBuilt()); + + CollectNamesVisitor visitor; + + // Test ONLY_MAPPED_NODES scope + mappingGraph.traverseTopDown(visitor, sofa::simulation::VisitorApplication::ONLY_MAPPED_NODES); + ASSERT_GT(visitor.names.size(), 3); + EXPECT_EQ(visitor.names[0], "state2"); + EXPECT_EQ(visitor.names[1], "ff2"); + EXPECT_EQ(visitor.names[2], "mass2"); + +} + +/** + * @brief Tests scoped traversal using different VisitorApplication scopes, limiting results to main nodes only. + */ +TEST(MappingGraph, ComplexGraph_OnlyMainNodes) +{ + // Setup environment using helper function + auto [root, inputs] = setupComplexGraphEnvironment(); + + ASSERT_EQ(inputs.mappings.size(), 1); + ASSERT_EQ(inputs.mechanicalStates.size(), 2); + ASSERT_EQ(inputs.forceFields.size(), 4); + sofa::simulation::MappingGraph mappingGraph(inputs); + ASSERT_TRUE(mappingGraph.isBuilt()); + + CollectNamesVisitor visitor; + + // Test ONLY_MAIN_NODES scope + mappingGraph.traverseTopDown(visitor, sofa::simulation::VisitorApplication::ONLY_MAIN_NODES); + ASSERT_EQ(visitor.names.size(), 5); + + EXPECT_EQ(visitor.names[0], "state1"); + EXPECT_EQ(visitor.names[1], "ff1"); + EXPECT_EQ(visitor.names[2], "mass1"); + EXPECT_EQ(visitor.names[3], "mass1"); + EXPECT_EQ(visitor.names[4], "mapping"); +} + } From 7515dada93336cd300028f711bdb2a5de808748c Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 09:54:19 +0200 Subject: [PATCH 14/41] feat(core): Support scoped visitation in bottom-up mapping graph traversal Updates `MappingGraph::traverseBottomUp` to accept a `VisitorApplication` scope, ensuring consistent node filtering logic with top-down traversal. This allows targeted processing of nodes (e.g., only mapped or main nodes) when traversing dependencies from the bottom up. Tests are updated to validate scoped bottom-up behavior. --- .../Core/src/sofa/simulation/MappingGraph.cpp | 48 ++++++++++++------- .../Core/src/sofa/simulation/MappingGraph.h | 2 +- .../Core/test/MappingGraph2_test.cpp | 24 +++++++++- 3 files changed, 54 insertions(+), 20 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 2adc13a68bc..a52c1f8c328 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -237,27 +237,33 @@ sofa::type::vector MappingGraph::getBottomUpMappingsFrom( return {}; } -void MappingGraph::traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope) const +namespace { - std::queue ready = prepareRootForTraversal(); - processQueue(ready, [&visitor, this, scope](const BaseMappingGraphNode* node){ - bool should_visit = true; - +bool shouldVisit(const BaseMappingGraphNode* node, VisitorApplication scope) +{ + if (node) + { switch (scope) { - case VisitorApplication::ONLY_MAPPED_NODES: - should_visit = node->isMapped(); - break; - case VisitorApplication::ONLY_MAIN_NODES: - should_visit = !node->isMapped(); - break; - case VisitorApplication::ALL_NODES: - default: - should_visit = true; // Visit all nodes. - break; + case VisitorApplication::ONLY_MAPPED_NODES: + return node->isMapped(); + case VisitorApplication::ONLY_MAIN_NODES: + return !node->isMapped(); + case VisitorApplication::ALL_NODES: + default: + return true; // Visit all nodes. } + } + return false; +} +} - if (should_visit) +void MappingGraph::traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope) const +{ + std::queue ready = prepareRootForTraversal(); + processQueue(ready, [&visitor, scope](const BaseMappingGraphNode* node) + { + if (shouldVisit(node, scope)) { node->accept(visitor); } @@ -279,7 +285,7 @@ std::queue MappingGraph::prepareRootForTraversal() const return ready; } -void MappingGraph::traverseBottomUp(MappingGraphVisitor& visitor) const +void MappingGraph::traverseBottomUp(MappingGraphVisitor& visitor, VisitorApplication scope) const { //the strategy consists in traversing the graph from top to bottom and //register the traversed nodes in a list. The bottom-up traversal corresponds to the @@ -289,7 +295,13 @@ void MappingGraph::traverseBottomUp(MappingGraphVisitor& visitor) const nodes.reserve(m_allNodes.size()); { std::queue ready = prepareRootForTraversal(); - processQueue(ready, [&nodes](BaseMappingGraphNode* node){ nodes.push_back(node); }); + processQueue(ready, [&nodes, scope](BaseMappingGraphNode* node) + { + if (shouldVisit(node, scope)) + { + nodes.push_back(node); + } + }); } for (const auto* node : std::views::reverse(nodes)) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 4c112e971b8..1b49d0bcd09 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -382,7 +382,7 @@ class SOFA_SIMULATION_CORE_API MappingGraph // // Provides the reverse dependency ordering check, ensuring that prerequisite // states are processed before the components that require them. - void traverseBottomUp(MappingGraphVisitor& visitor) const; + void traverseBottomUp(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; /** * @brief Visit and process component groups without any specific order. diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index 04520893d0d..ba0b6a88058 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -286,10 +286,21 @@ TEST(MappingGraph, ComplexGraph_OnlyMappedNodes) // Test ONLY_MAPPED_NODES scope mappingGraph.traverseTopDown(visitor, sofa::simulation::VisitorApplication::ONLY_MAPPED_NODES); - ASSERT_GT(visitor.names.size(), 3); + ASSERT_EQ(visitor.names.size(), 4); EXPECT_EQ(visitor.names[0], "state2"); EXPECT_EQ(visitor.names[1], "ff2"); EXPECT_EQ(visitor.names[2], "mass2"); + EXPECT_EQ(visitor.names[3], "mass2"); + + visitor.names.clear(); + // Bottom Up Traversal Check + mappingGraph.traverseBottomUp(visitor, sofa::simulation::VisitorApplication::ONLY_MAPPED_NODES); + ASSERT_EQ(visitor.names.size(), 4); + + EXPECT_EQ(visitor.names[0], "mass2"); + EXPECT_EQ(visitor.names[1], "mass2"); + EXPECT_EQ(visitor.names[2], "ff2"); + EXPECT_EQ(visitor.names[3], "state2"); } @@ -318,6 +329,17 @@ TEST(MappingGraph, ComplexGraph_OnlyMainNodes) EXPECT_EQ(visitor.names[2], "mass1"); EXPECT_EQ(visitor.names[3], "mass1"); EXPECT_EQ(visitor.names[4], "mapping"); + + visitor.names.clear(); + // Bottom Up Traversal Check + mappingGraph.traverseBottomUp(visitor, sofa::simulation::VisitorApplication::ONLY_MAIN_NODES); + ASSERT_EQ(visitor.names.size(), 5); + + EXPECT_EQ(visitor.names[0], "mapping"); + EXPECT_EQ(visitor.names[1], "mass1"); + EXPECT_EQ(visitor.names[2], "mass1"); + EXPECT_EQ(visitor.names[3], "ff1"); + EXPECT_EQ(visitor.names[4], "state1"); } } From 307dd68a97e9e876d33b0b2a1c67add6a0648473 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 09:58:40 +0200 Subject: [PATCH 15/41] Cache mapped status using optional type Updates `BaseMappingGraphNode::isMapped()` to use `std::optional` for managing and lazily computing the node's mapped status, improving efficiency of repeated checks. --- .../Core/src/sofa/simulation/MappingGraph.cpp | 10 +++++++++- .../Simulation/Core/src/sofa/simulation/MappingGraph.h | 6 +++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index a52c1f8c328..7e6ca669d67 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -8,10 +8,18 @@ namespace sofa::simulation bool BaseMappingGraphNode::isMapped() const { - return std::any_of(m_parents.begin(), m_parents.end(), [](const SPtr& node) + if (m_isMapped.has_value()) + { + return m_isMapped.value(); + } + + const auto isMapped = std::any_of(m_parents.begin(), m_parents.end(), [](const SPtr& node) { return node->getType() == NodeType::Mapping || node->isMapped(); }); + + m_isMapped = isMapped; + return isMapped; } MappingGraph::InputLists MappingGraph::InputLists::makeFromNode(core::objectmodel::BaseContext* node) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 1b49d0bcd09..76f79b899d0 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -32,12 +32,14 @@ */ #pragma once -#include #include #include #include #include #include +#include + +#include #include namespace sofa::simulation @@ -135,6 +137,8 @@ class BaseMappingGraphNode : public std::enable_shared_from_this m_isMapped; }; /** From d58b19d178544063df2f77dd26ace04cc1705b85 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 10:01:48 +0200 Subject: [PATCH 16/41] Add scoped visitation to component group traversal Updates `MappingGraph::traverseComponentGroups` to accept a scope parameter, allowing controlled filtering and processing of component groups during graph traversal. --- .../Simulation/Core/src/sofa/simulation/MappingGraph.cpp | 9 ++++++--- .../Simulation/Core/src/sofa/simulation/MappingGraph.h | 4 ++-- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 7e6ca669d67..38be483c4ea 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -318,13 +318,16 @@ void MappingGraph::traverseBottomUp(MappingGraphVisitor& visitor, VisitorApplica } } -void MappingGraph::traverseComponentGroups(MappingGraphVisitor& visitor) const +void MappingGraph::traverseComponentGroups(MappingGraphVisitor& visitor, VisitorApplication scope) const { for (auto& [states, node] : m_groupIndex) { - for (auto& child : node->m_children) + if (shouldVisit(node.get(), scope)) { - child->accept(visitor); + for (auto& child : node->m_children) + { + child->accept(visitor); + } } } } diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 76f79b899d0..d6c10cdf10d 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -392,10 +392,10 @@ class SOFA_SIMULATION_CORE_API MappingGraph * @brief Visit and process component groups without any specific order. * @param visitor The concrete visitor implementation. */ - void traverseComponentGroups(MappingGraphVisitor& visitor) const; + void traverseComponentGroups(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; /** - * @brief Performs a full graph traversal for component groups, optionally coordinating + * @brief Visit and process component groups without any specific order, optionally coordinating * with a TaskScheduler to manage execution parallelism. * @param visitor The concrete visitor implementation. * @param taskScheduler Optional scheduler instance for tasks requiring explicit ordering. From dad9588ac6f242088c92036aec1cf0e9d81b46fb Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 10:11:12 +0200 Subject: [PATCH 17/41] Refactor: Prefix names with component type in graph traversal tests Updates `MappingGraph2_test` to prepend descriptive prefixes (e.g., `[STATE]`, `[MAPPING]`) to node names visited during traversals, ensuring clear identification of the component type for accurate testing results. --- .../Core/test/MappingGraph2_test.cpp | 118 +++++++++--------- 1 file changed, 59 insertions(+), 59 deletions(-) diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index ba0b6a88058..b28e0f334f0 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -45,22 +45,22 @@ struct CollectNamesVisitor : public sofa::simulation::MappingGraphVisitor { void visit(core::BaseMapping& mapping) override { - names.push_back(mapping.getName()); + names.push_back("[MAPPING]" + mapping.getName()); } void visit(core::behavior::BaseMechanicalState& state) override { - names.push_back(state.getName()); + names.push_back("[STATE]" + state.getName()); } void visit(core::behavior::BaseForceField& ff) override { - names.push_back(ff.getName()); + names.push_back("[FORCEFIELD]" + ff.getName()); } void visit(core::behavior::BaseMass& mass) override { - names.push_back(mass.getName()); + names.push_back("[MASS]" + mass.getName()); } std::vector names; @@ -82,7 +82,7 @@ TEST(MappingGraph, SingleState) mappingGraph.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 1); - EXPECT_EQ(visitor.names[0], "state"); + EXPECT_EQ(visitor.names[0], "[STATE]state"); } TEST(MappingGraph, SingleMappingInSingleNode) @@ -106,16 +106,16 @@ TEST(MappingGraph, SingleMappingInSingleNode) mappingGraph.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 3); - EXPECT_EQ(visitor.names[0], "state1"); - EXPECT_EQ(visitor.names[1], "mapping"); - EXPECT_EQ(visitor.names[2], "state2"); + EXPECT_EQ(visitor.names[0], "[STATE]state1"); + EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); + EXPECT_EQ(visitor.names[2], "[STATE]state2"); visitor.names.clear(); mappingGraph.traverseBottomUp(visitor); ASSERT_EQ(visitor.names.size(), 3); - EXPECT_EQ(visitor.names[0], "state2"); - EXPECT_EQ(visitor.names[1], "mapping"); - EXPECT_EQ(visitor.names[2], "state1"); + EXPECT_EQ(visitor.names[0], "[STATE]state2"); + EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); + EXPECT_EQ(visitor.names[2], "[STATE]state1"); } TEST(MappingGraph, SingleMappingWithIntermediateNode) @@ -140,16 +140,16 @@ TEST(MappingGraph, SingleMappingWithIntermediateNode) mappingGraph.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 3); - EXPECT_EQ(visitor.names[0], "state1"); - EXPECT_EQ(visitor.names[1], "mapping"); - EXPECT_EQ(visitor.names[2], "state2"); + EXPECT_EQ(visitor.names[0], "[STATE]state1"); + EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); + EXPECT_EQ(visitor.names[2], "[STATE]state2"); visitor.names.clear(); mappingGraph.traverseBottomUp(visitor); ASSERT_EQ(visitor.names.size(), 3); - EXPECT_EQ(visitor.names[0], "state2"); - EXPECT_EQ(visitor.names[1], "mapping"); - EXPECT_EQ(visitor.names[2], "state1"); + EXPECT_EQ(visitor.names[0], "[STATE]state2"); + EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); + EXPECT_EQ(visitor.names[2], "[STATE]state1"); } TEST(MappingGraph, SingleMappingWithIntermediateNodeInverseInputOutput) @@ -174,16 +174,16 @@ TEST(MappingGraph, SingleMappingWithIntermediateNodeInverseInputOutput) mappingGraph.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 3); - EXPECT_EQ(visitor.names[0], "state2"); - EXPECT_EQ(visitor.names[1], "mapping"); - EXPECT_EQ(visitor.names[2], "state1"); + EXPECT_EQ(visitor.names[0], "[STATE]state2"); + EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); + EXPECT_EQ(visitor.names[2], "[STATE]state1"); visitor.names.clear(); mappingGraph.traverseBottomUp(visitor); ASSERT_EQ(visitor.names.size(), 3); - EXPECT_EQ(visitor.names[0], "state1"); - EXPECT_EQ(visitor.names[1], "mapping"); - EXPECT_EQ(visitor.names[2], "state2"); + EXPECT_EQ(visitor.names[0], "[STATE]state1"); + EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); + EXPECT_EQ(visitor.names[2], "[STATE]state2"); } /** @@ -240,32 +240,32 @@ TEST(MappingGraph, ComplexGraph) mappingGraph.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 9); // 9 and not 7 because a UniformMass is a BaseMass and also a BaseForceField - EXPECT_EQ(visitor.names[0], "state1"); - EXPECT_EQ(visitor.names[1], "ff1"); - EXPECT_EQ(visitor.names[2], "mass1"); - EXPECT_EQ(visitor.names[3], "mass1"); + EXPECT_EQ(visitor.names[0], "[STATE]state1"); + EXPECT_EQ(visitor.names[1], "[FORCEFIELD]ff1"); + EXPECT_EQ(visitor.names[2], "[FORCEFIELD]mass1"); + EXPECT_EQ(visitor.names[3], "[MASS]mass1"); - EXPECT_EQ(visitor.names[4], "mapping"); - EXPECT_EQ(visitor.names[5], "state2"); - EXPECT_EQ(visitor.names[6], "ff2"); - EXPECT_EQ(visitor.names[7], "mass2"); - EXPECT_EQ(visitor.names[8], "mass2"); + EXPECT_EQ(visitor.names[4], "[MAPPING]mapping"); + EXPECT_EQ(visitor.names[5], "[STATE]state2"); + EXPECT_EQ(visitor.names[6], "[FORCEFIELD]ff2"); + EXPECT_EQ(visitor.names[7], "[FORCEFIELD]mass2"); + EXPECT_EQ(visitor.names[8], "[MASS]mass2"); visitor.names.clear(); // Bottom Up Traversal Check mappingGraph.traverseBottomUp(visitor); ASSERT_EQ(visitor.names.size(), 9); - EXPECT_EQ(visitor.names[0], "mass2"); - EXPECT_EQ(visitor.names[1], "mass2"); - EXPECT_EQ(visitor.names[2], "ff2"); - EXPECT_EQ(visitor.names[3], "state2"); + EXPECT_EQ(visitor.names[0], "[MASS]mass2"); + EXPECT_EQ(visitor.names[1], "[FORCEFIELD]mass2"); + EXPECT_EQ(visitor.names[2], "[FORCEFIELD]ff2"); + EXPECT_EQ(visitor.names[3], "[STATE]state2"); - EXPECT_EQ(visitor.names[4], "mapping"); - EXPECT_EQ(visitor.names[5], "mass1"); - EXPECT_EQ(visitor.names[6], "mass1"); - EXPECT_EQ(visitor.names[7], "ff1"); - EXPECT_EQ(visitor.names[8], "state1"); + EXPECT_EQ(visitor.names[4], "[MAPPING]mapping"); + EXPECT_EQ(visitor.names[5], "[MASS]mass1"); + EXPECT_EQ(visitor.names[6], "[FORCEFIELD]mass1"); + EXPECT_EQ(visitor.names[7], "[FORCEFIELD]ff1"); + EXPECT_EQ(visitor.names[8], "[STATE]state1"); } /** @@ -287,20 +287,20 @@ TEST(MappingGraph, ComplexGraph_OnlyMappedNodes) // Test ONLY_MAPPED_NODES scope mappingGraph.traverseTopDown(visitor, sofa::simulation::VisitorApplication::ONLY_MAPPED_NODES); ASSERT_EQ(visitor.names.size(), 4); - EXPECT_EQ(visitor.names[0], "state2"); - EXPECT_EQ(visitor.names[1], "ff2"); - EXPECT_EQ(visitor.names[2], "mass2"); - EXPECT_EQ(visitor.names[3], "mass2"); + EXPECT_EQ(visitor.names[0], "[STATE]state2"); + EXPECT_EQ(visitor.names[1], "[FORCEFIELD]ff2"); + EXPECT_EQ(visitor.names[2], "[FORCEFIELD]mass2"); + EXPECT_EQ(visitor.names[3], "[MASS]mass2"); visitor.names.clear(); // Bottom Up Traversal Check mappingGraph.traverseBottomUp(visitor, sofa::simulation::VisitorApplication::ONLY_MAPPED_NODES); ASSERT_EQ(visitor.names.size(), 4); - EXPECT_EQ(visitor.names[0], "mass2"); - EXPECT_EQ(visitor.names[1], "mass2"); - EXPECT_EQ(visitor.names[2], "ff2"); - EXPECT_EQ(visitor.names[3], "state2"); + EXPECT_EQ(visitor.names[0], "[MASS]mass2"); + EXPECT_EQ(visitor.names[1], "[FORCEFIELD]mass2"); + EXPECT_EQ(visitor.names[2], "[FORCEFIELD]ff2"); + EXPECT_EQ(visitor.names[3], "[STATE]state2"); } @@ -324,22 +324,22 @@ TEST(MappingGraph, ComplexGraph_OnlyMainNodes) mappingGraph.traverseTopDown(visitor, sofa::simulation::VisitorApplication::ONLY_MAIN_NODES); ASSERT_EQ(visitor.names.size(), 5); - EXPECT_EQ(visitor.names[0], "state1"); - EXPECT_EQ(visitor.names[1], "ff1"); - EXPECT_EQ(visitor.names[2], "mass1"); - EXPECT_EQ(visitor.names[3], "mass1"); - EXPECT_EQ(visitor.names[4], "mapping"); + EXPECT_EQ(visitor.names[0], "[STATE]state1"); + EXPECT_EQ(visitor.names[1], "[FORCEFIELD]ff1"); + EXPECT_EQ(visitor.names[2], "[FORCEFIELD]mass1"); + EXPECT_EQ(visitor.names[3], "[MASS]mass1"); + EXPECT_EQ(visitor.names[4], "[MAPPING]mapping"); visitor.names.clear(); // Bottom Up Traversal Check mappingGraph.traverseBottomUp(visitor, sofa::simulation::VisitorApplication::ONLY_MAIN_NODES); ASSERT_EQ(visitor.names.size(), 5); - EXPECT_EQ(visitor.names[0], "mapping"); - EXPECT_EQ(visitor.names[1], "mass1"); - EXPECT_EQ(visitor.names[2], "mass1"); - EXPECT_EQ(visitor.names[3], "ff1"); - EXPECT_EQ(visitor.names[4], "state1"); + EXPECT_EQ(visitor.names[0], "[MAPPING]mapping"); + EXPECT_EQ(visitor.names[1], "[MASS]mass1"); + EXPECT_EQ(visitor.names[2], "[FORCEFIELD]mass1"); + EXPECT_EQ(visitor.names[3], "[FORCEFIELD]ff1"); + EXPECT_EQ(visitor.names[4], "[STATE]state1"); } } From 6325a59b284a434761c5237bf3cba8c7e46cbe59 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 10:46:18 +0200 Subject: [PATCH 18/41] Move classes in their own file --- Sofa/framework/Simulation/Core/CMakeLists.txt | 8 + .../Core/src/sofa/simulation/MappingGraph.cpp | 18 +- .../Core/src/sofa/simulation/MappingGraph.h | 180 +----------------- .../mappinggraph/BaseMappingGraphNode.cpp | 43 +++++ .../mappinggraph/BaseMappingGraphNode.h | 83 ++++++++ .../ComponentGroupMappingGraphNode.h | 54 ++++++ .../mappinggraph/MappingGraphNode.h | 88 +++++++++ .../mappinggraph/MappingGraphVisitor.cpp | 27 +++ .../mappinggraph/MappingGraphVisitor.h | 71 +++++++ 9 files changed, 378 insertions(+), 194 deletions(-) create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.cpp create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ComponentGroupMappingGraphNode.h create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphNode.h create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.cpp create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.h diff --git a/Sofa/framework/Simulation/Core/CMakeLists.txt b/Sofa/framework/Simulation/Core/CMakeLists.txt index e16319d1d4e..527bdfa877f 100644 --- a/Sofa/framework/Simulation/Core/CMakeLists.txt +++ b/Sofa/framework/Simulation/Core/CMakeLists.txt @@ -76,6 +76,11 @@ set(HEADER_FILES ${SRC_ROOT}/events/SimulationStopEvent.h ${SRC_ROOT}/events/SolveConstraintSystemEndEvent.h + ${SRC_ROOT}/mappinggraph/BaseMappingGraphNode.h + ${SRC_ROOT}/mappinggraph/ComponentGroupMappingGraphNode.h + ${SRC_ROOT}/mappinggraph/MappingGraphNode.h + ${SRC_ROOT}/mappinggraph/MappingGraphVisitor.h + ${SRC_ROOT}/mechanicalvisitor/MechanicalAccFromFVisitor.h ${SRC_ROOT}/mechanicalvisitor/MechanicalAccumulateJacobian.h ${SRC_ROOT}/mechanicalvisitor/MechanicalAccumulateJacobian.h @@ -214,6 +219,9 @@ set(SOURCE_FILES ${SRC_ROOT}/events/SimulationStopEvent.cpp ${SRC_ROOT}/events/SolveConstraintSystemEndEvent.cpp + ${SRC_ROOT}/mappinggraph/BaseMappingGraphNode.cpp + ${SRC_ROOT}/mappinggraph/MappingGraphVisitor.cpp + ${SRC_ROOT}/mechanicalvisitor/MechanicalAccFromFVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalAccumulateJacobian.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalAccumulateJacobian.cpp diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 38be483c4ea..7606f45dd94 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -1,27 +1,13 @@ #include #include +#include + #include namespace sofa::simulation { -bool BaseMappingGraphNode::isMapped() const -{ - if (m_isMapped.has_value()) - { - return m_isMapped.value(); - } - - const auto isMapped = std::any_of(m_parents.begin(), m_parents.end(), [](const SPtr& node) - { - return node->getType() == NodeType::Mapping || node->isMapped(); - }); - - m_isMapped = isMapped; - return isMapped; -} - MappingGraph::InputLists MappingGraph::InputLists::makeFromNode(core::objectmodel::BaseContext* node) { InputLists inputLists; diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index d6c10cdf10d..e7eb8a8fb85 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -32,14 +32,10 @@ */ #pragma once -#include -#include -#include -#include -#include #include +#include +#include -#include #include namespace sofa::simulation @@ -53,178 +49,6 @@ enum class VisitorApplication ONLY_MAIN_NODES }; -/** - * @brief Visitor interface for traversing the mapping graph. - * - * This visitor pattern is used to allow algorithms to process nodes in a - * structured way without modifying their structure or coupling traversal logic - * with specific component types. Implementations must override visit methods - * corresponding to the behaviors they intend to handle. - */ -class MappingGraphVisitor -{ -public: - virtual ~MappingGraphVisitor() = default; - - /** - * @brief Visits a mechanical state node. - * @param mstate The mechanical state component to visit. - */ - virtual void visit(core::behavior::BaseMechanicalState&) {} - - /** - * @brief Visits a base mapping node. - * @param mapping The mapping component to visit. - */ - virtual void visit(core::BaseMapping&) {} - - /** - * @brief Visits a force field behavior node. - * @param ff The force field component to visit. - */ - virtual void visit(core::behavior::BaseForceField&) {} - - /** - * @brief Visits a mass behavior node. - * @param m The mass component to visit. - */ - virtual void visit(core::behavior::BaseMass&) {} -}; - -/** - * @brief Abstract base class for nodes in the mapping graph. - * - * Provides common functionality for tracking prerequisites (parents) and dependencies - * (children), forming a Directed Acyclic Graph (DAG). It implements the Visitor pattern. - */ -class BaseMappingGraphNode : public std::enable_shared_from_this -{ -public: - using SPtr = std::shared_ptr; - friend class MappingGraph; - - virtual ~BaseMappingGraphNode() = default; - - /** - * @brief Accepts a visitor, allowing the graph to be processed by an external algorithm. - * @param visitor The concrete visitor implementation. - */ - virtual void accept(MappingGraphVisitor& visitor) const = 0; - - /** - * @brief Gets the name of the component represented by this node. - * @return The name string. - */ - virtual std::string getName() const { return {}; } - - enum class NodeType { - MechanicalState, - Mapping, - Component, - Group - }; - - virtual NodeType getType() const = 0; - - /** - * @return True if the node has an ancestor which is a mapping node - */ - bool isMapped() const; - - private: - sofa::type::vector m_parents; ///< prerequisite nodes (nodes pointing to this one) - sofa::type::vector m_children; ///< dependent nodes (nodes pointed from this one) - - // Mutable counter used during traversal (reset before each traversal). - mutable int m_pendingCount = 0; - - mutable std::optional m_isMapped; -}; - -/** - * @brief Template class representing a graph node associated with any SOFA component. - * - * This wrapper allows the `MappingGraph` to manage nodes for various types of - * components (e.g., MechanicalState, ForceField) polymorphically while maintaining - * type safety and providing standard graph node interfaces. - * - * @tparam TComponent The actual SOFA component class pointer type. - */ -template -class MappingGraphNode : public BaseMappingGraphNode -{ -public: - using SPtr = std::shared_ptr; - friend class MappingGraph; - - /** - * @brief Constructs a node wrapper for the given component. - * @param s A shared pointer to the component instance. - */ - explicit MappingGraphNode(typename TComponent::SPtr s) - : m_component(std::move(s)) - {} - - /** - * @brief Implements accept by calling visit on the wrapped component. - * @param visitor The concrete visitor implementation. - */ - void accept(MappingGraphVisitor& visitor) const override - { - if (m_component) - { - visitor.visit(*m_component); - } - } - - /** - * @brief Returns the name of the wrapped component. - * @return The name string from the component. - */ - std::string getName() const override - { - return m_component->getName(); - } - - NodeType getType() const override - { - if constexpr (std::is_base_of_v) - return NodeType::MechanicalState; - else if constexpr (std::is_base_of_v) - return NodeType::Mapping; - else - return NodeType::Component; - } - - private: - typename TComponent::SPtr m_component; ///< The actual SOFA component instance pointer. -}; - -/** - * @brief A node wrapper used for representing groups of components or abstract groupings. - */ -class ComponentGroupMappingGraphNode : public BaseMappingGraphNode -{ -public: - using SPtr = std::shared_ptr; - - void accept(MappingGraphVisitor& visitor) const override { SOFA_UNUSED(visitor); } - - /** - * @brief Returns the fixed name "group" for this type of node. - * @return The string "group". - */ - std::string getName() const override - { - return "group"; - } - - NodeType getType() const override - { - return NodeType::Group; - } -}; - /** * @brief Represents the overall mechanical simulation graph structure (Mapping Graph). * diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.cpp new file mode 100644 index 00000000000..49f5fce593e --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.cpp @@ -0,0 +1,43 @@ +/****************************************************************************** +* 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 * +******************************************************************************/ +#include + +namespace sofa::simulation +{ + +bool BaseMappingGraphNode::isMapped() const +{ + if (m_isMapped.has_value()) + { + return m_isMapped.value(); + } + + const auto isMapped = std::any_of(m_parents.begin(), m_parents.end(), [](const SPtr& node) + { + return node->getType() == NodeType::Mapping || node->isMapped(); + }); + + m_isMapped = isMapped; + return isMapped; +} + +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h new file mode 100644 index 00000000000..16e67d884b3 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h @@ -0,0 +1,83 @@ +/****************************************************************************** +* 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 +#include +#include + +namespace sofa::simulation +{ + + +/** + * @brief Abstract base class for nodes in the mapping graph. + * + * Provides common functionality for tracking prerequisites (parents) and dependencies + * (children), forming a Directed Acyclic Graph (DAG). It implements the Visitor pattern. + */ +class SOFA_SIMULATION_CORE_API BaseMappingGraphNode : public std::enable_shared_from_this +{ +public: + using SPtr = std::shared_ptr; + friend class MappingGraph; + + virtual ~BaseMappingGraphNode() = default; + + /** + * @brief Accepts a visitor, allowing the graph to be processed by an external algorithm. + * @param visitor The concrete visitor implementation. + */ + virtual void accept(MappingGraphVisitor& visitor) const = 0; + + /** + * @brief Gets the name of the component represented by this node. + * @return The name string. + */ + virtual std::string getName() const { return {}; } + + enum class NodeType { + MechanicalState, + Mapping, + Component, + Group + }; + + virtual NodeType getType() const = 0; + + /** + * @return True if the node has an ancestor which is a mapping node + */ + bool isMapped() const; + +private: + sofa::type::vector m_parents; ///< prerequisite nodes (nodes pointing to this one) + sofa::type::vector m_children; ///< dependent nodes (nodes pointed from this one) + + // Mutable counter used during traversal (reset before each traversal). + mutable int m_pendingCount = 0; + + mutable std::optional m_isMapped; +}; + + +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ComponentGroupMappingGraphNode.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ComponentGroupMappingGraphNode.h new file mode 100644 index 00000000000..eb16c413d93 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ComponentGroupMappingGraphNode.h @@ -0,0 +1,54 @@ +/****************************************************************************** +* 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 +{ + +/** + * @brief A node wrapper used for representing groups of components or abstract groupings. + */ +class SOFA_SIMULATION_CORE_API ComponentGroupMappingGraphNode : public BaseMappingGraphNode +{ +public: + using SPtr = std::shared_ptr; + + void accept(MappingGraphVisitor& visitor) const override { SOFA_UNUSED(visitor); } + + /** + * @brief Returns the fixed name "group" for this type of node. + * @return The string "group". + */ + std::string getName() const override + { + return "group"; + } + + NodeType getType() const override + { + return NodeType::Group; + } +}; + +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphNode.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphNode.h new file mode 100644 index 00000000000..dc1d74e4779 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphNode.h @@ -0,0 +1,88 @@ +/****************************************************************************** +* 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 +{ + +/** + * @brief Template class representing a graph node associated with any SOFA component. + * + * This wrapper allows the `MappingGraph` to manage nodes for various types of + * components (e.g., MechanicalState, ForceField) polymorphically while maintaining + * type safety and providing standard graph node interfaces. + * + * @tparam TComponent The actual SOFA component class pointer type. + */ +template +class MappingGraphNode : public BaseMappingGraphNode +{ +public: + using SPtr = std::shared_ptr; + friend class MappingGraph; + + /** + * @brief Constructs a node wrapper for the given component. + * @param s A shared pointer to the component instance. + */ + explicit MappingGraphNode(typename TComponent::SPtr s) + : m_component(std::move(s)) + {} + + /** + * @brief Implements accept by calling visit on the wrapped component. + * @param visitor The concrete visitor implementation. + */ + void accept(MappingGraphVisitor& visitor) const override + { + if (m_component) + { + visitor.visit(*m_component); + } + } + + /** + * @brief Returns the name of the wrapped component. + * @return The name string from the component. + */ + std::string getName() const override + { + return m_component->getName(); + } + + NodeType getType() const override + { + if constexpr (std::is_base_of_v) + return NodeType::MechanicalState; + else if constexpr (std::is_base_of_v) + return NodeType::Mapping; + else + return NodeType::Component; + } + +private: + typename TComponent::SPtr m_component; ///< The actual SOFA component instance pointer. +}; + +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.cpp new file mode 100644 index 00000000000..425b7e5aa4c --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.cpp @@ -0,0 +1,27 @@ +/****************************************************************************** +* 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 * +******************************************************************************/ +#include + +namespace sofa::simulation +{ + +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.h new file mode 100644 index 00000000000..a777a926041 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.h @@ -0,0 +1,71 @@ +/****************************************************************************** +* 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 +#include +#include +#include + +namespace sofa::simulation +{ + +/** + * @brief Visitor interface for traversing the mapping graph. + * + * This visitor pattern is used to allow algorithms to process nodes in a + * structured way without modifying their structure or coupling traversal logic + * with specific component types. Implementations must override visit methods + * corresponding to the behaviors they intend to handle. + */ +class MappingGraphVisitor +{ +public: + virtual ~MappingGraphVisitor() = default; + + /** + * @brief Visits a mechanical state node. + * @param mstate The mechanical state component to visit. + */ + virtual void visit(core::behavior::BaseMechanicalState&) {} + + /** + * @brief Visits a base mapping node. + * @param mapping The mapping component to visit. + */ + virtual void visit(core::BaseMapping&) {} + + /** + * @brief Visits a force field behavior node. + * @param ff The force field component to visit. + */ + virtual void visit(core::behavior::BaseForceField&) {} + + /** + * @brief Visits a mass behavior node. + * @param m The mass component to visit. + */ + virtual void visit(core::behavior::BaseMass&) {} +}; + +} From 9735aca32f624be2d60d8848562b490f00e975cb Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 14:58:22 +0200 Subject: [PATCH 19/41] Add callable visitor support for graph traversal Implements overloads for `traverseTopDown_` and `traverseBottomUp_` in `MappingGraph`, allowing users to pass a callable object or lambda directly. This approach simplifies graph traversal by leveraging the new `CallableVisitor` pattern, which automatically handles invoking the provided logic against visited components. Tests are updated to validate lambda usage during complex graph traversals. --- Sofa/framework/Simulation/Core/CMakeLists.txt | 1 + .../Core/src/sofa/simulation/MappingGraph.h | 17 +++- .../simulation/mappinggraph/CallableVisitor.h | 77 +++++++++++++++++++ .../Core/test/MappingGraph2_test.cpp | 32 ++++++++ 4 files changed, 126 insertions(+), 1 deletion(-) create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h diff --git a/Sofa/framework/Simulation/Core/CMakeLists.txt b/Sofa/framework/Simulation/Core/CMakeLists.txt index 527bdfa877f..3d2ef86b33b 100644 --- a/Sofa/framework/Simulation/Core/CMakeLists.txt +++ b/Sofa/framework/Simulation/Core/CMakeLists.txt @@ -77,6 +77,7 @@ set(HEADER_FILES ${SRC_ROOT}/events/SolveConstraintSystemEndEvent.h ${SRC_ROOT}/mappinggraph/BaseMappingGraphNode.h + ${SRC_ROOT}/mappinggraph/CallableVisitor.h ${SRC_ROOT}/mappinggraph/ComponentGroupMappingGraphNode.h ${SRC_ROOT}/mappinggraph/MappingGraphNode.h ${SRC_ROOT}/mappinggraph/MappingGraphVisitor.h diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index e7eb8a8fb85..dd82965848f 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -32,9 +32,10 @@ */ #pragma once -#include #include +#include #include +#include #include @@ -205,6 +206,13 @@ class SOFA_SIMULATION_CORE_API MappingGraph // and leaf components. This guarantees correct dependency order. void traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + template + void traverseTopDown_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const + { + CallableVisitor visitor{callable}; + traverseTopDown(visitor, scope); + } + // ------------------------------------------------------------------ // Bottom-up traversal: leaves → roots. // @@ -212,6 +220,13 @@ class SOFA_SIMULATION_CORE_API MappingGraph // states are processed before the components that require them. void traverseBottomUp(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + template + void traverseBottomUp_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const + { + CallableVisitor visitor{callable}; + traverseBottomUp(visitor, scope); + } + /** * @brief Visit and process component groups without any specific order. * @param visitor The concrete visitor implementation. diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h new file mode 100644 index 00000000000..ed5e5e0c678 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h @@ -0,0 +1,77 @@ +/****************************************************************************** +* 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 +#include +#include + +namespace sofa::simulation +{ + +template +struct BaseCallableVisitor : public MappingGraphVisitor +{ + explicit BaseCallableVisitor(const Callable& callable) + : m_callable(callable) + {} + + void visit(Component& component) override + { + this->m_callable(component); + } + +protected: + const Callable& m_callable; +}; + +template +struct GetComponentFromCallable; + +template requires std::is_invocable_v +struct GetComponentFromCallable +{ + using type = core::behavior::BaseForceField; +}; + +template requires std::is_invocable_v +struct GetComponentFromCallable +{ + using type = core::behavior::BaseMass; +}; + +template requires std::is_invocable_v +struct GetComponentFromCallable +{ + using type = core::behavior::BaseMechanicalState; +}; + +template requires std::is_invocable_v +struct GetComponentFromCallable +{ + using type = core::BaseMapping; +}; + +template +using CallableVisitor = BaseCallableVisitor::type>; + +} diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index b28e0f334f0..49b4aa14a6d 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -342,4 +342,36 @@ TEST(MappingGraph, ComplexGraph_OnlyMainNodes) EXPECT_EQ(visitor.names[4], "[STATE]state1"); } +TEST(MappingGraph, ComplexGraphUsingLambdas) +{ + // Setup environment using helper function + auto [root, inputs] = setupComplexGraphEnvironment(); + + ASSERT_EQ(inputs.mappings.size(), 1); + ASSERT_EQ(inputs.mechanicalStates.size(), 2); + ASSERT_EQ(inputs.forceFields.size(), 4); + sofa::simulation::MappingGraph mappingGraph(inputs); + ASSERT_TRUE(mappingGraph.isBuilt()); + + sofa::type::vector visited; + + mappingGraph.traverseTopDown_([&visited](const core::behavior::BaseMechanicalState& state) + { + visited.push_back(state.getName()); + }); + ASSERT_EQ(visited.size(), 2); + + EXPECT_EQ(visited[0], "state1"); + EXPECT_EQ(visited[1], "state2"); + + visited.clear(); + mappingGraph.traverseTopDown_([&visited](const core::BaseMapping& mapping) + { + visited.push_back(mapping.getName()); + }); + ASSERT_EQ(visited.size(), 1); + + EXPECT_EQ(visited[0], "mapping"); +} + } From 0d0e68329e2740213f0f00130fef8e9d373b2dab Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 15:02:19 +0200 Subject: [PATCH 20/41] Add clear functionality to MappingGraph Introduces a `clear()` method for `MappingGraph` to reset the entire graph state. This function ensures that all internal members, such as root node pointers, build flags, mapping status tracking, and associated data containers, are properly initialized back to their default empty state. --- .../Core/src/sofa/simulation/MappingGraph.cpp | 15 +++++++++++++++ .../Core/src/sofa/simulation/MappingGraph.h | 2 ++ .../simulation/mappinggraph/CallableVisitor.h | 4 +--- 3 files changed, 18 insertions(+), 3 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 7606f45dd94..70258bb2ad1 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -31,6 +31,19 @@ MappingGraph::MappingGraph(core::objectmodel::BaseContext* node) build(node); } +void MappingGraph::clear() +{ + m_rootNode = nullptr; + m_isBuilt = false; + m_hasAnyMapping = false; + m_totalNbMainDofs = 0; + m_positionInGlobalMatrix.clear(); + m_allNodes.clear(); + m_rootStates.clear(); + m_stateIndex.clear(); + m_groupIndex.clear(); +} + core::objectmodel::BaseContext* MappingGraph::getRootNode() const { return m_rootNode; @@ -347,6 +360,8 @@ typename MappingGraphNode::SPtr makeMappingGraphNode(typename TCompo void MappingGraph::build(const InputLists& input) { + clear(); + m_hasAnyMapping = input.mappings.size() > 0; // 1. Create one wrapper node per object; index state nodes by raw ptr. diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index dd82965848f..d30059a1fd4 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -106,6 +106,8 @@ class SOFA_SIMULATION_CORE_API MappingGraph */ explicit MappingGraph(core::objectmodel::BaseContext* node); + void clear(); + /** * @brief Returns the root node used during the initial construction of the graph. * @return A pointer to the root object model context. diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h index ed5e5e0c678..bc44da2d9d1 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h @@ -20,9 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include -#include -#include + #include namespace sofa::simulation From 73db9ee186c217bd4a46ec63e593a993647d6c2d Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 15:09:30 +0200 Subject: [PATCH 21/41] Add callable visitor support for component group traversal Introduces `traverseComponentGroups_` in `MappingGraph`, allowing users to pass a callable object or lambda directly for graph component group traversal. This standardizes the usage of callable visitors across all major graph traversal methods, improving API consistency and reducing boilerplate code. --- .../Simulation/Core/src/sofa/simulation/MappingGraph.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index d30059a1fd4..4d1f023b06b 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -235,6 +235,13 @@ class SOFA_SIMULATION_CORE_API MappingGraph */ void traverseComponentGroups(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + template + void traverseComponentGroups_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const + { + CallableVisitor visitor{callable}; + traverseComponentGroups(visitor, scope); + } + /** * @brief Visit and process component groups without any specific order, optionally coordinating * with a TaskScheduler to manage execution parallelism. From 4c98a56de2016e9863e06af8a3c7d3555a6f7782 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 15:11:14 +0200 Subject: [PATCH 22/41] use MappingGraph in EulerExplicitSolver --- .../odesolver/forward/EulerExplicitSolver.cpp | 37 +++++-------------- .../odesolver/forward/EulerExplicitSolver.h | 4 ++ 2 files changed, 13 insertions(+), 28 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 649f2c55570..09eee8bd004 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 @@ -342,29 +342,6 @@ 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: @@ -386,8 +363,7 @@ class AllOfMassesAreDiagonalVisitor final : public simulation::BaseMechanicalVis bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams* params) { - sofa::simulation::MappingGraph mappingGraph; - mappingGraph.build(this->getContext()); + m_mappingGraph.build(this->getContext()); // To achieve a diagonal global mass matrix in this system: // 1) Each individual mass matrix must itself be diagonal. @@ -397,10 +373,15 @@ 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.traverseComponentGroups_([&hasMappedMass](const sofa::core::behavior::BaseMass& mass) + { + 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); 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 72173e477a9..bada90900ab 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 @@ -25,6 +25,7 @@ #include #include #include +#include namespace sofa::simulation::common { @@ -125,6 +126,9 @@ class SOFA_COMPONENT_ODESOLVER_FORWARD_API EulerExplicitSolver : public sofa::co void solveSystem(core::MultiVecDerivId solution, core::MultiVecDerivId rhs) const; bool isMassMatrixTriviallyInvertible(const core::ExecParams* params); + + + simulation::MappingGraph m_mappingGraph; }; } // namespace sofa::component::odesolver::forward From c3575d8a39985421192c713d873fbb6440059b12 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 15:14:17 +0200 Subject: [PATCH 23/41] use MappingGraph to know if all masses are diagonal --- .../odesolver/forward/EulerExplicitSolver.cpp | 28 ++++--------------- 1 file changed, 6 insertions(+), 22 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 09eee8bd004..87411258b8f 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 @@ -342,25 +342,6 @@ void EulerExplicitSolver::solveSystem(core::MultiVecDerivId solution, core::Mult l_linearSolver->getLinearSystem()->dispatchSystemSolution(solution); } -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) { m_mappingGraph.build(this->getContext()); @@ -384,9 +365,12 @@ bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams } // 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.traverseComponentGroups_([&areAllMassesDiagonal](const sofa::core::behavior::BaseMass& mass) + { + areAllMassesDiagonal &= mass.isDiagonal(); + }); + return areAllMassesDiagonal; } } // namespace sofa::component::odesolver::forward From 7b7db050958abd930cc047e63401dfc5303d27e3 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 29 Apr 2026 16:16:50 +0200 Subject: [PATCH 24/41] implement computeForce using mapping graph --- .../odesolver/forward/EulerExplicitSolver.cpp | 8 ++--- .../odesolver/forward/EulerExplicitSolver.h | 2 +- .../Core/src/sofa/simulation/MappingGraph.cpp | 11 +++++++ .../Core/src/sofa/simulation/MappingGraph.h | 9 ++++++ .../sofa/simulation/MechanicalOperations.cpp | 30 +++++++++++++++++++ .../sofa/simulation/MechanicalOperations.h | 3 ++ 6 files changed, 58 insertions(+), 5 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 87411258b8f..fad3ea40da6 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 @@ -66,6 +66,8 @@ 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() ); @@ -277,14 +279,14 @@ 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::MechanicalOperations* 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); } void EulerExplicitSolver::computeAcceleration(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc, core::ConstMultiVecDerivId f) @@ -344,8 +346,6 @@ void EulerExplicitSolver::solveSystem(core::MultiVecDerivId solution, core::Mult bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams* params) { - m_mappingGraph.build(this->getContext()); - // To achieve a diagonal global mass matrix in this system: // 1) Each individual mass matrix must itself be diagonal. // 2) The mapped masses must also be mapped through a diagonal Jacobian matrix. 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 bada90900ab..b794b22f5c3 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 @@ -108,7 +108,7 @@ class SOFA_COMPONENT_ODESOLVER_FORWARD_API EulerExplicitSolver : public sofa::co static void addSeparateGravity(sofa::simulation::common::MechanicalOperations* 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::MechanicalOperations* mop, core::MultiVecDerivId f) const; /// Compute the acceleration from the force and the inverse of the mass /// acc = M^-1 * f diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 70258bb2ad1..de81d607840 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -265,6 +265,17 @@ bool shouldVisit(const BaseMappingGraphNode* node, VisitorApplication scope) } } +void MappingGraph::traverse(MappingGraphVisitor& visitor, VisitorApplication scope) const +{ + for (auto& node : m_allNodes) + { + if (shouldVisit(node.get(), scope)) + { + node->accept(visitor); + } + } +} + void MappingGraph::traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope) const { std::queue ready = prepareRootForTraversal(); diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 4d1f023b06b..4d278a9db1c 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -200,6 +200,15 @@ class SOFA_SIMULATION_CORE_API MappingGraph core::behavior::BaseMechanicalState*) const; + void traverse(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + + template + void traverse_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const + { + CallableVisitor visitor{callable}; + traverse(visitor, scope); + } + // ------------------------------------------------------------------ // Top-down traversal: roots (unmapped states) → leaves (components). // diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp index d6c0e0b020d..b1c8bf672a3 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp @@ -259,6 +259,36 @@ void MechanicalOperations::computeForce(core::MultiVecDerivId result, bool clear executeVisitor( MechanicalComputeForceVisitor(&mparams, result, accumulate) ); } +void MechanicalOperations::computeForce(const MappingGraph& mappingGraph, + core::MultiVecDerivId result, bool clear, bool accumulate) +{ + setF(result); + if (clear) + { + mappingGraph.traverse_([&](core::behavior::BaseMechanicalState& state) + { + state.resetForce(&mparams, result.getId(&state)); + }); + } + + mappingGraph.traverse_([&](core::behavior::BaseMechanicalState& state) + { + state.accumulateForce(&mparams, result.getId(&state)); + }); + + mappingGraph.traverse_([&](core::behavior::BaseForceField& forceField) + { + forceField.addForce(&mparams, result); + }); + + if (accumulate) + { + mappingGraph.traverseBottomUp_([&](core::BaseMapping& mapping) + { + mapping.applyJT(&mparams, result, result); + }); + } +} /// Compute the current force delta (given the latest propagated displacement) void MechanicalOperations::computeDf(core::MultiVecDerivId df, bool clear, bool accumulate) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h index 787e67fefd7..7d810da0334 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h @@ -29,6 +29,7 @@ #include #include #include +#include namespace sofa::simulation::common @@ -78,6 +79,8 @@ class SOFA_SIMULATION_CORE_API MechanicalOperations void computeEnergy(SReal &kineticEnergy, SReal &potentialEnergy); /// Compute the current force (given the latest propagated position and velocity) void computeForce(core::MultiVecDerivId result, bool clear = true, bool accumulate = true); + /// Compute the current force (given the latest propagated position and velocity) + void computeForce(const MappingGraph& mappingGraph, core::MultiVecDerivId result, bool clear = true, bool accumulate = true); /// Compute the current force delta (given the latest propagated displacement) void computeDf(core::MultiVecDerivId df, bool clear = true, bool accumulate = true); /// Compute the current force delta (given the latest propagated velocity) From 483ebda1a91c88f81b9fa0cbaa429cd5832c09d8 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 09:18:15 +0200 Subject: [PATCH 25/41] use iterators for reverse traversal because clang on CI does not support views --- .../Simulation/Core/src/sofa/simulation/MappingGraph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index de81d607840..9dae4f30ecf 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -322,9 +322,9 @@ void MappingGraph::traverseBottomUp(MappingGraphVisitor& visitor, VisitorApplica }); } - for (const auto* node : std::views::reverse(nodes)) + for (auto it = nodes.crbegin(); it != nodes.crend(); ++it) { - node->accept(visitor); + (*it)->accept(visitor); } } From 1b0e24511e8218cd915dca9d55516e2478b7418b Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 13:03:53 +0200 Subject: [PATCH 26/41] Encapsulate graph traversal logic into MappingGraphAlgorithms Extracts all core graph traversal methods (Top-Down, Bottom-Up, component group traversals) from `MappingGraph` and moves their implementation to a dedicated `MappingGraphAlgorithms` class. This improves modularity by separating the algorithm details from the main graph structure API. Updates headers and tests to utilize the new `algorithms` member struct for all traversal calls. --- Sofa/framework/Simulation/Core/CMakeLists.txt | 3 + .../Core/src/sofa/simulation/MappingGraph.cpp | 117 ----------- .../Core/src/sofa/simulation/MappingGraph.h | 104 +--------- .../sofa/simulation/MechanicalOperations.cpp | 8 +- .../mappinggraph/BaseMappingGraphNode.h | 1 + .../mappinggraph/MappingGraphAlgorithms.cpp | 181 ++++++++++++++++++ .../mappinggraph/MappingGraphAlgorithms.h | 117 +++++++++++ .../mappinggraph/VisitorApplication.h | 35 ++++ .../Core/test/MappingGraph2_test.cpp | 30 +-- 9 files changed, 360 insertions(+), 236 deletions(-) create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/VisitorApplication.h diff --git a/Sofa/framework/Simulation/Core/CMakeLists.txt b/Sofa/framework/Simulation/Core/CMakeLists.txt index 3d2ef86b33b..c5f59c75dfc 100644 --- a/Sofa/framework/Simulation/Core/CMakeLists.txt +++ b/Sofa/framework/Simulation/Core/CMakeLists.txt @@ -79,8 +79,10 @@ set(HEADER_FILES ${SRC_ROOT}/mappinggraph/BaseMappingGraphNode.h ${SRC_ROOT}/mappinggraph/CallableVisitor.h ${SRC_ROOT}/mappinggraph/ComponentGroupMappingGraphNode.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 @@ -221,6 +223,7 @@ set(SOURCE_FILES ${SRC_ROOT}/events/SolveConstraintSystemEndEvent.cpp ${SRC_ROOT}/mappinggraph/BaseMappingGraphNode.cpp + ${SRC_ROOT}/mappinggraph/MappingGraphAlgorithms.cpp ${SRC_ROOT}/mappinggraph/MappingGraphVisitor.cpp ${SRC_ROOT}/mechanicalvisitor/MechanicalAccFromFVisitor.cpp diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 9dae4f30ecf..862f14fd293 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -244,123 +244,6 @@ sofa::type::vector MappingGraph::getBottomUpMappingsFrom( return {}; } -namespace -{ -bool shouldVisit(const BaseMappingGraphNode* node, VisitorApplication scope) -{ - if (node) - { - switch (scope) - { - case VisitorApplication::ONLY_MAPPED_NODES: - return node->isMapped(); - case VisitorApplication::ONLY_MAIN_NODES: - return !node->isMapped(); - case VisitorApplication::ALL_NODES: - default: - return true; // Visit all nodes. - } - } - return false; -} -} - -void MappingGraph::traverse(MappingGraphVisitor& visitor, VisitorApplication scope) const -{ - for (auto& node : m_allNodes) - { - if (shouldVisit(node.get(), scope)) - { - node->accept(visitor); - } - } -} - -void MappingGraph::traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope) const -{ - std::queue ready = prepareRootForTraversal(); - processQueue(ready, [&visitor, scope](const BaseMappingGraphNode* node) - { - if (shouldVisit(node, scope)) - { - node->accept(visitor); - } - }); -} - -std::queue MappingGraph::prepareRootForTraversal() const -{ - std::queue ready; - for (auto& node : m_allNodes) - { - node->m_pendingCount = static_cast(node->m_parents.size()); - if (node->m_pendingCount == 0) //node without any parent -> a root - { - ready.push(node.get()); - } - } - - return ready; -} - -void MappingGraph::traverseBottomUp(MappingGraphVisitor& visitor, VisitorApplication scope) const -{ - //the strategy consists in traversing the graph from top to bottom and - //register the traversed nodes in a list. The bottom-up traversal corresponds to the - //reversed list. - - sofa::type::vector nodes; - nodes.reserve(m_allNodes.size()); - { - std::queue ready = prepareRootForTraversal(); - processQueue(ready, [&nodes, scope](BaseMappingGraphNode* node) - { - if (shouldVisit(node, scope)) - { - nodes.push_back(node); - } - }); - } - - for (auto it = nodes.crbegin(); it != nodes.crend(); ++it) - { - (*it)->accept(visitor); - } -} - -void MappingGraph::traverseComponentGroups(MappingGraphVisitor& visitor, VisitorApplication scope) const -{ - for (auto& [states, node] : m_groupIndex) - { - if (shouldVisit(node.get(), scope)) - { - for (auto& child : node->m_children) - { - child->accept(visitor); - } - } - } -} -void MappingGraph::traverseComponentGroups(MappingGraphVisitor& visitor, - TaskScheduler* taskScheduler) const -{ - if (taskScheduler) - { - sofa::simulation::parallelForEach(*taskScheduler, m_groupIndex.begin(), m_groupIndex.end(), - [&visitor](const auto& pair) - { - for (auto& child : pair.second->m_children) - { - child->accept(visitor); - } - }); - } - else - { - traverseComponentGroups(visitor); - } -} - bool MappingGraph::isBuilt() const { return m_isBuilt; } template diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 4d278a9db1c..0ac4d704c4c 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -35,21 +35,12 @@ #include #include #include -#include - -#include +#include namespace sofa::simulation { class TaskScheduler; -enum class VisitorApplication -{ - ALL_NODES, - ONLY_MAPPED_NODES, - ONLY_MAIN_NODES -}; - /** * @brief Represents the overall mechanical simulation graph structure (Mapping Graph). * @@ -114,6 +105,9 @@ class SOFA_SIMULATION_CORE_API MappingGraph */ [[nodiscard]] core::objectmodel::BaseContext* getRootNode() const; + friend struct MappingGraphAlgorithms; + MappingGraphAlgorithms algorithms { this }; + /** * @brief Gets the list of all main mechanical states that are not used as outputs * in any mapping (i.e., they are root inputs). @@ -199,66 +193,6 @@ class SOFA_SIMULATION_CORE_API MappingGraph sofa::type::vector getBottomUpMappingsFrom( core::behavior::BaseMechanicalState*) const; - - void traverse(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; - - template - void traverse_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const - { - CallableVisitor visitor{callable}; - traverse(visitor, scope); - } - - // ------------------------------------------------------------------ - // Top-down traversal: roots (unmapped states) → leaves (components). - // - // Ensures that a BaseMechanicalState is only processed after all mappings - // that produce it as output have been processed, and similarly for mappings - // and leaf components. This guarantees correct dependency order. - void traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; - - template - void traverseTopDown_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const - { - CallableVisitor visitor{callable}; - traverseTopDown(visitor, scope); - } - - // ------------------------------------------------------------------ - // Bottom-up traversal: leaves → roots. - // - // Provides the reverse dependency ordering check, ensuring that prerequisite - // states are processed before the components that require them. - void traverseBottomUp(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; - - template - void traverseBottomUp_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const - { - CallableVisitor visitor{callable}; - traverseBottomUp(visitor, scope); - } - - /** - * @brief Visit and process component groups without any specific order. - * @param visitor The concrete visitor implementation. - */ - void traverseComponentGroups(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; - - template - void traverseComponentGroups_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const - { - CallableVisitor visitor{callable}; - traverseComponentGroups(visitor, scope); - } - - /** - * @brief Visit and process component groups without any specific order, optionally coordinating - * with a TaskScheduler to manage execution parallelism. - * @param visitor The concrete visitor implementation. - * @param taskScheduler Optional scheduler instance for tasks requiring explicit ordering. - */ - void traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const; - /** * @brief Checks if the graph has been successfully built and analyzed. * @return True if building is complete, false otherwise. @@ -306,36 +240,6 @@ class SOFA_SIMULATION_CORE_API MappingGraph // ------------------------------------------------------------------ - std::queue prepareRootForTraversal() const; - - /** - * @brief Performs a breadth-first search (BFS) traversal, processing nodes in dependency order. - * - * This static helper method is used for both top-down and bottom-up traversals. - * - * @param ready The queue of nodes that are currently ready to be visited/processed. - */ - template - static void processQueue(std::queue& ready, const Callable& f) - { - while (!ready.empty()) - { - BaseMappingGraphNode* current = ready.front(); - ready.pop(); - - f(current); - - for (auto& child : current->m_children) - { - --(child->m_pendingCount); - if (child->m_pendingCount == 0) - { - ready.push(child.get()); - } - } - } - } - /** * @brief Locates or creates a component group node encompassing the given set of states. * @param states The mechanical states belonging to the group. diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp index b1c8bf672a3..c6694702dc9 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp @@ -265,25 +265,25 @@ void MechanicalOperations::computeForce(const MappingGraph& mappingGraph, setF(result); if (clear) { - mappingGraph.traverse_([&](core::behavior::BaseMechanicalState& state) + mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) { state.resetForce(&mparams, result.getId(&state)); }); } - mappingGraph.traverse_([&](core::behavior::BaseMechanicalState& state) + mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) { state.accumulateForce(&mparams, result.getId(&state)); }); - mappingGraph.traverse_([&](core::behavior::BaseForceField& forceField) + mappingGraph.algorithms.traverse_([&](core::behavior::BaseForceField& forceField) { forceField.addForce(&mparams, result); }); if (accumulate) { - mappingGraph.traverseBottomUp_([&](core::BaseMapping& mapping) + mappingGraph.algorithms.traverseBottomUp_([&](core::BaseMapping& mapping) { mapping.applyJT(&mparams, result, result); }); diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h index 16e67d884b3..bfa289688dc 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h @@ -40,6 +40,7 @@ class SOFA_SIMULATION_CORE_API BaseMappingGraphNode : public std::enable_shared_ public: using SPtr = std::shared_ptr; friend class MappingGraph; + friend struct MappingGraphAlgorithms; virtual ~BaseMappingGraphNode() = default; diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp new file mode 100644 index 00000000000..5ee6d0a6405 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp @@ -0,0 +1,181 @@ +/****************************************************************************** +* 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 * +******************************************************************************/ +#include +#include +#include + +namespace sofa::simulation +{ + +namespace +{ +bool shouldVisit(const BaseMappingGraphNode* node, VisitorApplication scope) +{ + if (node) + { + switch (scope) + { + case VisitorApplication::ONLY_MAPPED_NODES: + return node->isMapped(); + case VisitorApplication::ONLY_MAIN_NODES: + return !node->isMapped(); + case VisitorApplication::ALL_NODES: + default: + return true; // Visit all nodes. + } + } + return false; +} +} + +std::queue MappingGraphAlgorithms::prepareRootForTraversal() const +{ + std::queue ready; + for (auto& node : m_mappingGraph->m_allNodes) + { + node->m_pendingCount = static_cast(node->m_parents.size()); + if (node->m_pendingCount == 0) //node without any parent -> a root + { + ready.push(node.get()); + } + } + + return ready; +} + + +/** + * @brief Performs a breadth-first search (BFS) traversal, processing nodes in dependency order. + * + * This static helper method is used for both top-down and bottom-up traversals. + * + * @param ready The queue of nodes that are currently ready to be visited/processed. + */ +template +void MappingGraphAlgorithms::processQueue(std::queue& ready, const Callable& f) +{ + while (!ready.empty()) + { + BaseMappingGraphNode* current = ready.front(); + ready.pop(); + + f(current); + + for (auto& child : current->m_children) + { + --(child->m_pendingCount); + if (child->m_pendingCount == 0) + { + ready.push(child.get()); + } + } + } +} + + +void MappingGraphAlgorithms::traverse(MappingGraphVisitor& visitor, VisitorApplication scope) const +{ + for (auto& node : m_mappingGraph->m_allNodes) + { + if (shouldVisit(node.get(), scope)) + { + node->accept(visitor); + } + } +} + +void MappingGraphAlgorithms::traverseTopDown(MappingGraphVisitor& visitor, + VisitorApplication scope) const +{ + std::queue ready = prepareRootForTraversal(); + processQueue(ready, [&visitor, scope](const BaseMappingGraphNode* node) + { + if (shouldVisit(node, scope)) + { + node->accept(visitor); + } + }); +} + +void MappingGraphAlgorithms::traverseBottomUp(MappingGraphVisitor& visitor, + VisitorApplication scope) const +{ + //the strategy consists in traversing the graph from top to bottom and + //register the traversed nodes in a list. The bottom-up traversal corresponds to the + //reversed list. + + sofa::type::vector nodes; + nodes.reserve(m_mappingGraph->m_allNodes.size()); + { + std::queue ready = prepareRootForTraversal(); + processQueue(ready, [&nodes, scope](BaseMappingGraphNode* node) + { + if (shouldVisit(node, scope)) + { + nodes.push_back(node); + } + }); + } + + for (auto it = nodes.crbegin(); it != nodes.crend(); ++it) + { + (*it)->accept(visitor); + } +} + +void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visitor, + VisitorApplication scope) const +{ + for (auto& [states, node] : m_mappingGraph->m_groupIndex) + { + if (shouldVisit(node.get(), scope)) + { + for (auto& child : node->m_children) + { + child->accept(visitor); + } + } + } +} + +void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visitor, + TaskScheduler* taskScheduler) const +{ + if (taskScheduler) + { + sofa::simulation::parallelForEach(*taskScheduler, + m_mappingGraph->m_groupIndex.begin(), m_mappingGraph->m_groupIndex.end(), + [&visitor](const auto& pair) + { + for (auto& child : pair.second->m_children) + { + child->accept(visitor); + } + }); + } + else + { + traverseComponentGroups(visitor); + } +} + +} // namespace sofa::simulation diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h new file mode 100644 index 00000000000..0cebe23f5e2 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h @@ -0,0 +1,117 @@ +/****************************************************************************** +* 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 +#include +#include + +#include + +namespace sofa::simulation +{ + +class TaskScheduler; +class BaseMappingGraphNode; +class MappingGraph; + +struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms +{ + explicit MappingGraphAlgorithms(MappingGraph* mappingGraph) + : m_mappingGraph(mappingGraph) + { + } + + // ------------------------------------------------------------------ + // Traverse without any specific order + // ------------------------------------------------------------------ + + void traverse(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + + template + void traverse_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const + { + CallableVisitor visitor{callable}; + traverse(visitor, scope); + } + + // ------------------------------------------------------------------ + // Top-Down traversal + // ------------------------------------------------------------------ + + void traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + + template + void traverseTopDown_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const + { + CallableVisitor visitor{callable}; + traverseTopDown(visitor, scope); + } + + // ------------------------------------------------------------------ + // Bottom-Up traversal + // ------------------------------------------------------------------ + + void traverseBottomUp(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + + template + void traverseBottomUp_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const + { + CallableVisitor visitor{callable}; + traverseBottomUp(visitor, scope); + } + + // ------------------------------------------------------------------ + // Traverse only component groups without any specific order + // ------------------------------------------------------------------ + + /** + * @brief Visit and process component groups without any specific order. + * @param visitor The concrete visitor implementation. + */ + void traverseComponentGroups(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + + template + void traverseComponentGroups_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const + { + CallableVisitor visitor{callable}; + traverseComponentGroups(visitor, scope); + } + + /** + * @brief Visit and process component groups without any specific order, optionally coordinating + * with a TaskScheduler to manage execution parallelism. + * @param visitor The concrete visitor implementation. + * @param taskScheduler Optional scheduler instance for tasks requiring explicit ordering. + */ + void traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const; + +private: + MappingGraph* m_mappingGraph { nullptr }; + + std::queue prepareRootForTraversal() const; + + template + static void processQueue(std::queue& ready, const Callable& f); +}; +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/VisitorApplication.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/VisitorApplication.h new file mode 100644 index 00000000000..6e9c4b085ca --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/VisitorApplication.h @@ -0,0 +1,35 @@ +/****************************************************************************** +* 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 +{ + +enum class SOFA_SIMULATION_CORE_API VisitorApplication +{ + ALL_NODES, + ONLY_MAPPED_NODES, + ONLY_MAIN_NODES +}; + +} diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index 49b4aa14a6d..1afd3e051cb 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -80,7 +80,7 @@ TEST(MappingGraph, SingleState) CollectNamesVisitor visitor; - mappingGraph.traverseTopDown(visitor); + mappingGraph.algorithms.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 1); EXPECT_EQ(visitor.names[0], "[STATE]state"); } @@ -104,14 +104,14 @@ TEST(MappingGraph, SingleMappingInSingleNode) CollectNamesVisitor visitor; - mappingGraph.traverseTopDown(visitor); + mappingGraph.algorithms.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 3); EXPECT_EQ(visitor.names[0], "[STATE]state1"); EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); EXPECT_EQ(visitor.names[2], "[STATE]state2"); visitor.names.clear(); - mappingGraph.traverseBottomUp(visitor); + mappingGraph.algorithms.traverseBottomUp(visitor); ASSERT_EQ(visitor.names.size(), 3); EXPECT_EQ(visitor.names[0], "[STATE]state2"); EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); @@ -138,14 +138,14 @@ TEST(MappingGraph, SingleMappingWithIntermediateNode) CollectNamesVisitor visitor; - mappingGraph.traverseTopDown(visitor); + mappingGraph.algorithms.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 3); EXPECT_EQ(visitor.names[0], "[STATE]state1"); EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); EXPECT_EQ(visitor.names[2], "[STATE]state2"); visitor.names.clear(); - mappingGraph.traverseBottomUp(visitor); + mappingGraph.algorithms.traverseBottomUp(visitor); ASSERT_EQ(visitor.names.size(), 3); EXPECT_EQ(visitor.names[0], "[STATE]state2"); EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); @@ -172,14 +172,14 @@ TEST(MappingGraph, SingleMappingWithIntermediateNodeInverseInputOutput) CollectNamesVisitor visitor; - mappingGraph.traverseTopDown(visitor); + mappingGraph.algorithms.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 3); EXPECT_EQ(visitor.names[0], "[STATE]state2"); EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); EXPECT_EQ(visitor.names[2], "[STATE]state1"); visitor.names.clear(); - mappingGraph.traverseBottomUp(visitor); + mappingGraph.algorithms.traverseBottomUp(visitor); ASSERT_EQ(visitor.names.size(), 3); EXPECT_EQ(visitor.names[0], "[STATE]state1"); EXPECT_EQ(visitor.names[1], "[MAPPING]mapping"); @@ -237,7 +237,7 @@ TEST(MappingGraph, ComplexGraph) CollectNamesVisitor visitor; // Top Down Traversal Check - mappingGraph.traverseTopDown(visitor); + mappingGraph.algorithms.traverseTopDown(visitor); ASSERT_EQ(visitor.names.size(), 9); // 9 and not 7 because a UniformMass is a BaseMass and also a BaseForceField EXPECT_EQ(visitor.names[0], "[STATE]state1"); @@ -253,7 +253,7 @@ TEST(MappingGraph, ComplexGraph) visitor.names.clear(); // Bottom Up Traversal Check - mappingGraph.traverseBottomUp(visitor); + mappingGraph.algorithms.traverseBottomUp(visitor); ASSERT_EQ(visitor.names.size(), 9); EXPECT_EQ(visitor.names[0], "[MASS]mass2"); @@ -285,7 +285,7 @@ TEST(MappingGraph, ComplexGraph_OnlyMappedNodes) CollectNamesVisitor visitor; // Test ONLY_MAPPED_NODES scope - mappingGraph.traverseTopDown(visitor, sofa::simulation::VisitorApplication::ONLY_MAPPED_NODES); + mappingGraph.algorithms.traverseTopDown(visitor, sofa::simulation::VisitorApplication::ONLY_MAPPED_NODES); ASSERT_EQ(visitor.names.size(), 4); EXPECT_EQ(visitor.names[0], "[STATE]state2"); EXPECT_EQ(visitor.names[1], "[FORCEFIELD]ff2"); @@ -294,7 +294,7 @@ TEST(MappingGraph, ComplexGraph_OnlyMappedNodes) visitor.names.clear(); // Bottom Up Traversal Check - mappingGraph.traverseBottomUp(visitor, sofa::simulation::VisitorApplication::ONLY_MAPPED_NODES); + mappingGraph.algorithms.traverseBottomUp(visitor, sofa::simulation::VisitorApplication::ONLY_MAPPED_NODES); ASSERT_EQ(visitor.names.size(), 4); EXPECT_EQ(visitor.names[0], "[MASS]mass2"); @@ -321,7 +321,7 @@ TEST(MappingGraph, ComplexGraph_OnlyMainNodes) CollectNamesVisitor visitor; // Test ONLY_MAIN_NODES scope - mappingGraph.traverseTopDown(visitor, sofa::simulation::VisitorApplication::ONLY_MAIN_NODES); + mappingGraph.algorithms.traverseTopDown(visitor, sofa::simulation::VisitorApplication::ONLY_MAIN_NODES); ASSERT_EQ(visitor.names.size(), 5); EXPECT_EQ(visitor.names[0], "[STATE]state1"); @@ -332,7 +332,7 @@ TEST(MappingGraph, ComplexGraph_OnlyMainNodes) visitor.names.clear(); // Bottom Up Traversal Check - mappingGraph.traverseBottomUp(visitor, sofa::simulation::VisitorApplication::ONLY_MAIN_NODES); + mappingGraph.algorithms.traverseBottomUp(visitor, sofa::simulation::VisitorApplication::ONLY_MAIN_NODES); ASSERT_EQ(visitor.names.size(), 5); EXPECT_EQ(visitor.names[0], "[MAPPING]mapping"); @@ -355,7 +355,7 @@ TEST(MappingGraph, ComplexGraphUsingLambdas) sofa::type::vector visited; - mappingGraph.traverseTopDown_([&visited](const core::behavior::BaseMechanicalState& state) + mappingGraph.algorithms.traverseTopDown_([&visited](const core::behavior::BaseMechanicalState& state) { visited.push_back(state.getName()); }); @@ -365,7 +365,7 @@ TEST(MappingGraph, ComplexGraphUsingLambdas) EXPECT_EQ(visited[1], "state2"); visited.clear(); - mappingGraph.traverseTopDown_([&visited](const core::BaseMapping& mapping) + mappingGraph.algorithms.traverseTopDown_([&visited](const core::BaseMapping& mapping) { visited.push_back(mapping.getName()); }); From 53cfda6e2622c13d6d03ee370a266b0d76170360 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 13:09:18 +0200 Subject: [PATCH 27/41] documentation --- .../mappinggraph/MappingGraphAlgorithms.h | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h index 0cebe23f5e2..91b95bd4e07 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h @@ -35,8 +35,20 @@ class TaskScheduler; class BaseMappingGraphNode; class MappingGraph; +/** + * @brief Provides graph traversal algorithms for the mapping graph. + * + * This class contains static and instance methods that implement various ways + * to traverse the nodes and component groups defined in a MappingGraph, + * such as top-down, bottom-up, or arbitrary order. It uses the Visitor pattern + * to process components during traversal. + */ struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms { + /** + * @brief Constructor for MappingGraphAlgorithms. + * @param mappingGraph Pointer to the mapping graph to be traversed. + */ explicit MappingGraphAlgorithms(MappingGraph* mappingGraph) : m_mappingGraph(mappingGraph) { @@ -46,8 +58,19 @@ struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms // Traverse without any specific order // ------------------------------------------------------------------ + /** + * @brief Traverses the entire mapping graph nodes in an arbitrary order. + * @param visitor The concrete visitor implementation to process each node. + * @param scope Specifies which types of nodes should be visited (e.g., all, or only component groups). + */ void traverse(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + /** + * @brief Traverses the entire mapping graph nodes using a callable function in an arbitrary order. + * @tparam Callable The type of callable object used for visitation. + * @param callable The callable object containing the logic to execute during traversal. + * @param scope Specifies which types of nodes should be visited. + */ template void traverse_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const { @@ -59,8 +82,19 @@ struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms // Top-Down traversal // ------------------------------------------------------------------ + /** + * @brief Traverses the mapping graph in a top-down order. + * @param visitor The concrete visitor implementation to process each node. + * @param scope Specifies which types of nodes should be visited. + */ void traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + /** + * @brief Traverses the mapping graph in a top-down order using a callable function. + * @tparam Callable The type of callable object used for visitation. + * @param callable The callable object containing the logic to execute during traversal. + * @param scope Specifies which types of nodes should be visited. + */ template void traverseTopDown_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const { @@ -72,8 +106,19 @@ struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms // Bottom-Up traversal // ------------------------------------------------------------------ + /** + * @brief Traverses the mapping graph in a bottom-up order. + * @param visitor The concrete visitor implementation to process each node. + * @param scope Specifies which types of nodes should be visited. + */ void traverseBottomUp(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + /** + * @brief Traverses the mapping graph in a bottom-up order using a callable function. + * @tparam Callable The type of callable object used for visitation. + * @param callable The callable object containing the logic to execute during traversal. + * @param scope Specifies which types of nodes should be visited. + */ template void traverseBottomUp_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const { @@ -88,9 +133,16 @@ struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms /** * @brief Visit and process component groups without any specific order. * @param visitor The concrete visitor implementation. + * @param scope Specifies which types of nodes should be visited. Defaults to ALL_NODES. */ void traverseComponentGroups(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + /** + * @brief Visit and process component groups without any specific order using a callable function. + * @tparam Callable The type of callable object used for visitation. + * @param callable The callable object containing the logic to execute during traversal. + * @param scope Specifies which types of nodes should be visited. Defaults to ALL_NODES. + */ template void traverseComponentGroups_(const Callable& callable, VisitorApplication scope = VisitorApplication::ALL_NODES) const { From 7120160634b1b197fefd1b03f4af83dac029e82c Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 13:33:05 +0200 Subject: [PATCH 28/41] Enhance graph traversal with parallel execution support Adds an overloaded `traverse` method to `MappingGraphAlgorithms`. This overload allows the graph traversal process to utilize a `TaskScheduler`, executing node visits concurrently for improved performance when processing large graphs on multi-threaded environments. It falls back to synchronous traversal if no scheduler is provided. --- .../mappinggraph/MappingGraphAlgorithms.cpp | 27 +++++++++++++++++++ .../mappinggraph/MappingGraphAlgorithms.h | 2 ++ 2 files changed, 29 insertions(+) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp index 5ee6d0a6405..f9bb54cae75 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp @@ -103,6 +103,33 @@ void MappingGraphAlgorithms::traverse(MappingGraphVisitor& visitor, VisitorAppli } } +void MappingGraphAlgorithms::traverse(MappingGraphVisitor& visitor, + VisitorApplication scope, TaskScheduler* taskScheduler) const +{ + if (taskScheduler) + { + sofa::type::vector visitableNodes; + for (const auto& node : m_mappingGraph->m_allNodes) + { + if (shouldVisit(node.get(), scope)) + { + visitableNodes.push_back(node); + } + } + + sofa::simulation::parallelForEach(*taskScheduler, + visitableNodes.begin(), visitableNodes.end(), + [&visitor](const auto& node) + { + node->accept(visitor); + }); + } + else + { + traverse(visitor); + } +} + void MappingGraphAlgorithms::traverseTopDown(MappingGraphVisitor& visitor, VisitorApplication scope) const { diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h index 91b95bd4e07..7502cf3a9f4 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h @@ -65,6 +65,8 @@ struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms */ void traverse(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + void traverse(MappingGraphVisitor& visitor, VisitorApplication scope, TaskScheduler* taskScheduler) const; + /** * @brief Traverses the entire mapping graph nodes using a callable function in an arbitrary order. * @tparam Callable The type of callable object used for visitation. From a88c0049c3783ee79587d315df106cac45cf7a53 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 16:10:39 +0200 Subject: [PATCH 29/41] start test with interaction force field --- .../Core/test/MappingGraph2_test.cpp | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index 1afd3e051cb..05a701d6009 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -374,4 +374,48 @@ TEST(MappingGraph, ComplexGraphUsingLambdas) EXPECT_EQ(visited[0], "mapping"); } + +TEST(MappingGraph, ComplexGraphInteractionForceField) +{ + // Setup common plugins required for both complex graph tests + sofa::simpleapi::importPlugin(Sofa.Component.Mapping.Linear); + sofa::simpleapi::importPlugin(Sofa.Component.StateContainer); + sofa::simpleapi::importPlugin(Sofa.Component.MechanicalLoad); + sofa::simpleapi::importPlugin(Sofa.Component.Mass); + sofa::simpleapi::importPlugin(Sofa.Component.SolidMechanics.Spring); + + const sofa::simulation::Node::SPtr root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + + const auto node1 = root->createChild("node1"); + sofa::simpleapi::createObject(node1, "MechanicalObject", {{"name", "state1"}}); + sofa::simpleapi::createObject(node1, "ConstantForceField", {{"name", "ff1"}, {"state", "@state1"}, {"forces", "1 0 0"}}); + sofa::simpleapi::createObject(node1, "UniformMass", {{"name", "mass1"}}); + + const auto node2 = root->createChild("node2"); + sofa::simpleapi::createObject(node2, "MechanicalObject", {{"name", "state2"}}); + sofa::simpleapi::createObject(node2, "ConstantForceField", {{"name", "ff2"}, {"state", "@state2"}, {"forces", "1 0 0"}}); + sofa::simpleapi::createObject(node2, "UniformMass", {{"name", "mass2"}}); + + // const auto node3 = root->createChild("node3"); + // sofa::simpleapi::createObject(node3, "MechanicalObject", {{"name", "state3"}}); + + sofa::simpleapi::createObject(root, "SpringForceField", {{"name", "spring"}, {"object1", "@node1/state1"}, {"object2", "@node2/state2"}}); + + // Initialize the root node structure + sofa::simulation::node::initRoot(root.get()); + + sofa::simulation::MappingGraph mappingGraph(root.get()); + ASSERT_TRUE(mappingGraph.isBuilt()); + + CollectNamesVisitor visitor; + + // Top Down Traversal Check + mappingGraph.algorithms.traverseTopDown(visitor); + // ASSERT_EQ(visitor.names.size(), 9); // 9 and not 7 because a UniformMass is a BaseMass and also a BaseForceField + + EXPECT_EQ(visitor.names[0], "[STATE]state1"); +} + + + } From 5fc0c8264cf3accc63d96240ca98dd0e85f4ab99 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 16:10:48 +0200 Subject: [PATCH 30/41] more details --- .../sofa/simulation/MechanicalOperations.cpp | 31 ++++++++++++++++--- .../sofa/simulation/MechanicalOperations.h | 2 +- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp index c6694702dc9..5902da1171e 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp @@ -260,29 +260,50 @@ void MechanicalOperations::computeForce(core::MultiVecDerivId result, bool clear } void MechanicalOperations::computeForce(const MappingGraph& mappingGraph, - core::MultiVecDerivId result, bool clear, bool accumulate) + core::MultiVecDerivId result, bool clear, bool accumulate, + TaskScheduler* taskScheduler) { + //assumes the mapping graph is valid and properly initialized + setF(result); if (clear) { + /** + * Reset forces on all mechanical states in the mapping graph. This operation can be performed + * in any order on all states in the graph. + */ mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) { - state.resetForce(&mparams, result.getId(&state)); + const VecDerivId& stateForce = result.getId(&state); + state.resetForce(&mparams, stateForce); }); } + /** + * Compute f += externalForce on all mechanical states in the mapping graph. This operation can + * be performed in any order on all states in the graph. + */ mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) { - state.accumulateForce(&mparams, result.getId(&state)); + const VecDerivId& stateForce = result.getId(&state); + state.accumulateForce(&mparams, stateForce); }); - mappingGraph.algorithms.traverse_([&](core::behavior::BaseForceField& forceField) + /** + * Compute f += f(x) on all force fields in the mapping graph. This operation can be performed + * in any order on all states in the graph, and can be parallelized among groups. + */ + mappingGraph.algorithms.traverseComponentGroups_([&](core::behavior::BaseForceField& forceField) { forceField.addForce(&mparams, result); - }); + }, sofa::simulation::VisitorApplication::ALL_NODES, taskScheduler); if (accumulate) { + /** + * Compute f_in += J^T * f_out using mappings in the mapping graph. This operation must be + * performed in a bottom-up order to ensure correct force accumulation. + */ mappingGraph.algorithms.traverseBottomUp_([&](core::BaseMapping& mapping) { mapping.applyJT(&mparams, result, result); diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h index 7d810da0334..8b53bfb689f 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h @@ -80,7 +80,7 @@ class SOFA_SIMULATION_CORE_API MechanicalOperations /// Compute the current force (given the latest propagated position and velocity) void computeForce(core::MultiVecDerivId result, bool clear = true, bool accumulate = true); /// Compute the current force (given the latest propagated position and velocity) - void computeForce(const MappingGraph& mappingGraph, core::MultiVecDerivId result, bool clear = true, bool accumulate = true); + void computeForce(const MappingGraph& mappingGraph, core::MultiVecDerivId result, bool clearForceBefore, bool accumulateForcesFromMappedStates, TaskScheduler* taskScheduler); /// Compute the current force delta (given the latest propagated displacement) void computeDf(core::MultiVecDerivId df, bool clear = true, bool accumulate = true); /// Compute the current force delta (given the latest propagated velocity) From 0b97fe673d230c363aba5188d46cb9519cd2dae5 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 16:10:59 +0200 Subject: [PATCH 31/41] more helper functions --- .../mappinggraph/MappingGraphAlgorithms.cpp | 35 +++++++++++++++++-- .../mappinggraph/MappingGraphAlgorithms.h | 16 +++++++++ 2 files changed, 49 insertions(+), 2 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp index f9bb54cae75..3a9dde975cd 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp @@ -169,8 +169,8 @@ void MappingGraphAlgorithms::traverseBottomUp(MappingGraphVisitor& visitor, } } -void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visitor, - VisitorApplication scope) const +void MappingGraphAlgorithms::traverseComponentGroups( + MappingGraphVisitor& visitor, VisitorApplication scope) const { for (auto& [states, node] : m_mappingGraph->m_groupIndex) { @@ -184,6 +184,37 @@ void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visito } } +void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visitor, + VisitorApplication scope, + TaskScheduler* taskScheduler) const +{ + if (taskScheduler) + { + sofa::type::vector visitableNodes; + for (const auto& node : m_mappingGraph->m_groupIndex) + { + if (shouldVisit(node.second.get(), scope)) + { + visitableNodes.push_back(node.second); + } + } + + sofa::simulation::parallelForEach(*taskScheduler, + visitableNodes.begin(), visitableNodes.end(), + [&visitor, &scope](const auto& node) + { + for (auto& child : node->m_children) + { + child->accept(visitor); + } + }); + } + else + { + traverseComponentGroups(visitor); + } +} + void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const { diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h index 7502cf3a9f4..a9ff73547eb 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h @@ -80,6 +80,13 @@ struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms traverse(visitor, scope); } + template + void traverse_(const Callable& callable, VisitorApplication scope, TaskScheduler* taskScheduler) const + { + CallableVisitor visitor{callable}; + traverse(visitor, scope, taskScheduler); + } + // ------------------------------------------------------------------ // Top-Down traversal // ------------------------------------------------------------------ @@ -139,6 +146,8 @@ struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms */ void traverseComponentGroups(MappingGraphVisitor& visitor, VisitorApplication scope = VisitorApplication::ALL_NODES) const; + void traverseComponentGroups(MappingGraphVisitor& visitor, VisitorApplication scope, TaskScheduler* taskScheduler) const; + /** * @brief Visit and process component groups without any specific order using a callable function. * @tparam Callable The type of callable object used for visitation. @@ -160,6 +169,13 @@ struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms */ void traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const; + template + void traverseComponentGroups_(const Callable& callable, VisitorApplication scope, TaskScheduler* taskScheduler) const + { + CallableVisitor visitor{callable}; + traverseComponentGroups(visitor, scope, taskScheduler); + } + private: MappingGraph* m_mappingGraph { nullptr }; From c63a3ea37777df0426ce1c6e3e33feeeaf33152f Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 16:45:39 +0200 Subject: [PATCH 32/41] export mapping graph as DOT format --- Sofa/framework/Simulation/Core/CMakeLists.txt | 2 + .../Core/src/sofa/simulation/MappingGraph.h | 2 + .../mappinggraph/BaseMappingGraphNode.h | 1 + .../simulation/mappinggraph/ExportDot.cpp | 110 ++++++++++++++++++ .../sofa/simulation/mappinggraph/ExportDot.h | 33 ++++++ .../Core/test/MappingGraph2_test.cpp | 3 + 6 files changed, 151 insertions(+) create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.cpp create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.h diff --git a/Sofa/framework/Simulation/Core/CMakeLists.txt b/Sofa/framework/Simulation/Core/CMakeLists.txt index c5f59c75dfc..6f3eded17b1 100644 --- a/Sofa/framework/Simulation/Core/CMakeLists.txt +++ b/Sofa/framework/Simulation/Core/CMakeLists.txt @@ -79,6 +79,7 @@ set(HEADER_FILES ${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 @@ -223,6 +224,7 @@ set(SOURCE_FILES ${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 diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 0ac4d704c4c..60228a0f735 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -216,6 +216,8 @@ class SOFA_SIMULATION_CORE_API MappingGraph */ void build(core::objectmodel::BaseContext* rootNode); + friend std::string SOFA_SIMULATION_CORE_API exportToDotFormat(const MappingGraph& graph); + private: ///< Root node used to start graph exploration during construction. core::objectmodel::BaseContext* m_rootNode { nullptr }; diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h index bfa289688dc..98ea42180b3 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h @@ -41,6 +41,7 @@ class SOFA_SIMULATION_CORE_API BaseMappingGraphNode : public std::enable_shared_ using SPtr = std::shared_ptr; friend class MappingGraph; friend struct MappingGraphAlgorithms; + friend std::string SOFA_SIMULATION_CORE_API exportToDotFormat(const MappingGraph& graph); virtual ~BaseMappingGraphNode() = default; diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.cpp new file mode 100644 index 00000000000..0c81b1e9103 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.cpp @@ -0,0 +1,110 @@ +/****************************************************************************** +* 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 * +******************************************************************************/ +#include +#include + +#include + +namespace sofa::simulation +{ + +std::string getType(BaseMappingGraphNode* node) +{ + const auto type = node->getType(); + if (type == BaseMappingGraphNode::NodeType::Mapping) + return "Mapping"; + else if (type == BaseMappingGraphNode::NodeType::Component) + return "Component"; + else if (type == BaseMappingGraphNode::NodeType::MechanicalState) + return "State"; + else if (type == BaseMappingGraphNode::NodeType::Group) + return "Group"; + else + return "Unknown"; +} + +std::string getColor(BaseMappingGraphNode* node) +{ + const auto type = node->getType(); + if (type == BaseMappingGraphNode::NodeType::Mapping) + return "red1"; + else if (type == BaseMappingGraphNode::NodeType::Component) + return "royalblue1"; + else if (type == BaseMappingGraphNode::NodeType::MechanicalState) + return "seagreen1"; + else if (type == BaseMappingGraphNode::NodeType::Group) + return "slateblue1"; + else + return "webgray"; +} + +std::string getShape(BaseMappingGraphNode* node) +{ + const auto type = node->getType(); + if (type == BaseMappingGraphNode::NodeType::Mapping) + return "diamond"; + else if (type == BaseMappingGraphNode::NodeType::Component) + return "ellipse"; + else if (type == BaseMappingGraphNode::NodeType::MechanicalState) + return "box"; + else if (type == BaseMappingGraphNode::NodeType::Group) + return "component"; + else + return "note"; +} + +std::string exportToDotFormat(const MappingGraph& graph) +{ + // Use a string stream to build the DOT content + std::stringstream ss; + ss << "digraph MappingGraph {\n"; + ss << " rankdir=TB;\n"; // Top to Bottom layout is common for dependency graphs + + // 1. Add all nodes (vertices) + for (const auto& node : graph.m_allNodes) + { + const std::string label = "[" + getType(node.get()) + "]" + node->getName(); + + // Assuming a unique name or ID can be generated for the node in DOT format + ss << " \"Node_" << std::to_string(reinterpret_cast(node.get())) + << "\" [label=\""<< label << "\", color=\"" << getColor(node.get()) << "\", shape=\"" + << getShape(node.get()) << "\"];\n"; + } + + // 2. Add all edges (dependencies) + for (const auto& node : graph.m_allNodes) + { + const std::string sourceName = "Node_" + std::to_string(reinterpret_cast(node.get())); + for (auto child : node->m_children) + { + const std::string targetName = "Node_" + std::to_string(reinterpret_cast(child.get())); + + // Assuming edges represent dependencies (Source -> Target) + ss << " " << sourceName << " -> " << targetName << ";\n"; + } + } + + ss << "}\n"; + return ss.str(); +} + +} // namespace sofa::simulation diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.h new file mode 100644 index 00000000000..da25f198d16 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.h @@ -0,0 +1,33 @@ +/****************************************************************************** +* 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 +{ + +class MappingGraph; + +std::string SOFA_SIMULATION_CORE_API exportToDotFormat(const MappingGraph& graph); + +} // namespace sofa::simulation diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index 05a701d6009..4eb7c790ece 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace sofa { @@ -407,6 +408,8 @@ TEST(MappingGraph, ComplexGraphInteractionForceField) sofa::simulation::MappingGraph mappingGraph(root.get()); ASSERT_TRUE(mappingGraph.isBuilt()); + std::cout << exportToDotFormat(mappingGraph) << std::endl; + CollectNamesVisitor visitor; // Top Down Traversal Check From 3634ca89da6c0439a159785650b7063bfe520e32 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 16:53:31 +0200 Subject: [PATCH 33/41] prevent data races in component groups --- .../mappinggraph/MappingGraphAlgorithms.cpp | 44 +++++++++---------- .../mappinggraph/MappingGraphAlgorithms.h | 8 ---- 2 files changed, 20 insertions(+), 32 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp index 3a9dde975cd..e35b43d7f48 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp @@ -190,17 +190,25 @@ void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visito { if (taskScheduler) { - sofa::type::vector visitableNodes; - for (const auto& node : m_mappingGraph->m_groupIndex) + sofa::type::vector parallelNodes, sequentialNodes; + for (const auto& [states, node] : m_mappingGraph->m_groupIndex) { - if (shouldVisit(node.second.get(), scope)) + //with a size of 1, we are sure that they are all different, preventing data races + if (shouldVisit(node.get(), scope)) { - visitableNodes.push_back(node.second); + if ( states.size() == 1) + { + parallelNodes.push_back(node); + } + else + { + sequentialNodes.push_back(node); + } } } sofa::simulation::parallelForEach(*taskScheduler, - visitableNodes.begin(), visitableNodes.end(), + parallelNodes.begin(), parallelNodes.end(), [&visitor, &scope](const auto& node) { for (auto& child : node->m_children) @@ -208,27 +216,14 @@ void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visito child->accept(visitor); } }); - } - else - { - traverseComponentGroups(visitor); - } -} -void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visitor, - TaskScheduler* taskScheduler) const -{ - if (taskScheduler) - { - sofa::simulation::parallelForEach(*taskScheduler, - m_mappingGraph->m_groupIndex.begin(), m_mappingGraph->m_groupIndex.end(), - [&visitor](const auto& pair) + for (const auto node : sequentialNodes) + { + for (auto& child : node->m_children) { - for (auto& child : pair.second->m_children) - { - child->accept(visitor); - } - }); + child->accept(visitor); + } + } } else { @@ -236,4 +231,5 @@ void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visito } } + } // namespace sofa::simulation diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h index a9ff73547eb..ba8c70473bd 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.h @@ -161,14 +161,6 @@ struct SOFA_SIMULATION_CORE_API MappingGraphAlgorithms traverseComponentGroups(visitor, scope); } - /** - * @brief Visit and process component groups without any specific order, optionally coordinating - * with a TaskScheduler to manage execution parallelism. - * @param visitor The concrete visitor implementation. - * @param taskScheduler Optional scheduler instance for tasks requiring explicit ordering. - */ - void traverseComponentGroups(MappingGraphVisitor& visitor, TaskScheduler* taskScheduler) const; - template void traverseComponentGroups_(const Callable& callable, VisitorApplication scope, TaskScheduler* taskScheduler) const { From 2038c92a60b0af5ca2c6a141d3492a092084c64b Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 16:57:57 +0200 Subject: [PATCH 34/41] fix --- .../odesolver/forward/EulerExplicitSolver.cpp | 10 ++++++---- .../component/odesolver/forward/EulerExplicitSolver.h | 2 +- .../simulation/mappinggraph/MappingGraphAlgorithms.cpp | 4 ++-- 3 files changed, 9 insertions(+), 7 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 fad3ea40da6..961b743ed96 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 @@ -286,7 +286,7 @@ void EulerExplicitSolver::computeForce(sofa::simulation::common::MechanicalOpera // 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(m_mappingGraph, f); + mop->computeForce(m_mappingGraph, f, true, true, nullptr); } void EulerExplicitSolver::computeAcceleration(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId acc, core::ConstMultiVecDerivId f) @@ -344,8 +344,10 @@ void EulerExplicitSolver::solveSystem(core::MultiVecDerivId solution, core::Mult l_linearSolver->getLinearSystem()->dispatchSystemSolution(solution); } -bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams* params) +bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams* params) const { + SOFA_UNUSED(params); + // To achieve a diagonal global mass matrix in this system: // 1) Each individual mass matrix must itself be diagonal. // 2) The mapped masses must also be mapped through a diagonal Jacobian matrix. @@ -355,7 +357,7 @@ bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams // 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. bool hasMappedMass = false; - m_mappingGraph.traverseComponentGroups_([&hasMappedMass](const sofa::core::behavior::BaseMass& mass) + m_mappingGraph.algorithms.traverseComponentGroups_([&hasMappedMass](const sofa::core::behavior::BaseMass&) { hasMappedMass = true; }, simulation::VisitorApplication::ONLY_MAPPED_NODES); @@ -366,7 +368,7 @@ bool EulerExplicitSolver::isMassMatrixTriviallyInvertible(const core::ExecParams // At this stage, we know that we don't have any mapped mass. We can check if they are all diagonal. bool areAllMassesDiagonal = true; - m_mappingGraph.traverseComponentGroups_([&areAllMassesDiagonal](const sofa::core::behavior::BaseMass& mass) + m_mappingGraph.algorithms.traverseComponentGroups_([&areAllMassesDiagonal](const sofa::core::behavior::BaseMass& mass) { areAllMassesDiagonal &= mass.isDiagonal(); }); 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 b794b22f5c3..e08fef94723 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 @@ -125,7 +125,7 @@ class SOFA_COMPONENT_ODESOLVER_FORWARD_API EulerExplicitSolver : public sofa::co 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; diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp index e35b43d7f48..d436aa2abce 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphAlgorithms.cpp @@ -193,9 +193,9 @@ void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visito sofa::type::vector parallelNodes, sequentialNodes; for (const auto& [states, node] : m_mappingGraph->m_groupIndex) { - //with a size of 1, we are sure that they are all different, preventing data races if (shouldVisit(node.get(), scope)) { + //with a size of 1, we are sure that they are all different, preventing data races if ( states.size() == 1) { parallelNodes.push_back(node); @@ -217,7 +217,7 @@ void MappingGraphAlgorithms::traverseComponentGroups(MappingGraphVisitor& visito } }); - for (const auto node : sequentialNodes) + for (const auto& node : sequentialNodes) { for (auto& child : node->m_children) { From 924952ccad1d34bf31e5e5f0f2e84d910ff7afb0 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 30 Apr 2026 17:10:07 +0200 Subject: [PATCH 35/41] compute force using mapping graph --- .../sofa/component/odesolver/backward/EulerImplicitSolver.cpp | 4 +++- .../sofa/component/odesolver/backward/EulerImplicitSolver.h | 4 +++- .../Core/src/sofa/simulation/MechanicalOperations.cpp | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp index 796ff4e4e7d..77a88247263 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp @@ -82,6 +82,8 @@ 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 @@ -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; } diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.h b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.h index aba8e9fe33d..a42f7685b79 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.h +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.h @@ -22,8 +22,8 @@ #pragma once #include #include - #include +#include 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 diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp index 5902da1171e..08944995b1a 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp @@ -291,7 +291,7 @@ void MechanicalOperations::computeForce(const MappingGraph& mappingGraph, /** * Compute f += f(x) on all force fields in the mapping graph. This operation can be performed - * in any order on all states in the graph, and can be parallelized among groups. + * in any order on all states in the graph, and can be parallelized among component groups. */ mappingGraph.algorithms.traverseComponentGroups_([&](core::behavior::BaseForceField& forceField) { From 010326ed43e4921a40b8d65f9d206628081c1b15 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Mon, 4 May 2026 09:27:03 +0200 Subject: [PATCH 36/41] implement addMBKv --- .../backward/EulerImplicitSolver.cpp | 2 +- .../sofa/simulation/MechanicalOperations.cpp | 42 +++++++++++++++++++ .../sofa/simulation/MechanicalOperations.h | 2 + 3 files changed, 45 insertions(+), 1 deletion(-) diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp index 77a88247263..630994b77a8 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp @@ -149,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 diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp index 08944995b1a..68ab54c02e7 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp @@ -380,7 +380,49 @@ void MechanicalOperations::addMBKv(core::MultiVecDerivId df, mparams.setDx(dx); } +void MechanicalOperations::addMBKv(const MappingGraph& mappingGraph, core::MultiVecDerivId df, + core::MatricesFactors::M m, core::MatricesFactors::B b, + core::MatricesFactors::K k, bool clear, bool accumulate) +{ + const core::ConstMultiVecDerivId dx = mparams.dx(); + mparams.setDx(mparams.v()); + setDf(df); + if (clear) + { + /** + * Reset forces on all mapped mechanical states in the mapping graph. This operation can be performed + * in any order on all mapped states in the graph. + */ + mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) + { + const VecDerivId& stateForce = df.getId(&state); + state.resetForce(&mparams, stateForce); + }, VisitorApplication::ONLY_MAPPED_NODES); + } + mparams.setBFactor(b.get()); + mparams.setKFactor(k.get()); + mparams.setMFactor(m.get()); + /* useV = true */ + mappingGraph.algorithms.traverseComponentGroups_([&](core::behavior::BaseForceField& forceField) + { + forceField.addMBKdx(&mparams, df); + }); + + if (accumulate) + { + mappingGraph.algorithms.traverseBottomUp_([&](core::BaseMapping& mapping) + { + mapping.applyJT(&mparams, df, df); + if( mparams.kFactor() == 0 ) + { + mapping.applyDJT(&mparams, df, df); + } + }); + } + + mparams.setDx(dx); +} /// Add dt*Gravity to the velocity void MechanicalOperations::addSeparateGravity(SReal dt, core::MultiVecDerivId result) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h index 8b53bfb689f..45bc02799ef 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h @@ -89,6 +89,8 @@ class SOFA_SIMULATION_CORE_API MechanicalOperations void addMBKdx(core::MultiVecDerivId df, core::MatricesFactors::M m, core::MatricesFactors::B b, core::MatricesFactors::K k, bool clear = true, bool accumulate = true); /// accumulate $ df += (m M + b B + k K) velocity $ void addMBKv(core::MultiVecDerivId df, core::MatricesFactors::M m, core::MatricesFactors::B b, core::MatricesFactors::K k, bool clear = true, bool accumulate = true); + /// accumulate $ df += (m M + b B + k K) velocity $ + void addMBKv(const MappingGraph& mappingGraph, core::MultiVecDerivId df, core::MatricesFactors::M m, core::MatricesFactors::B b, core::MatricesFactors::K k, bool clear = true, bool accumulate = true); /// Add dt*Gravity to the velocity void addSeparateGravity(SReal dt, core::MultiVecDerivId result = core::vec_id::write_access::velocity ); From 715b77c2f0de9873b96922c91cdce484816af6f6 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 8 May 2026 10:24:34 +0200 Subject: [PATCH 37/41] Refactor graph accessors using getters Replaces direct member variable access for internal graph structures (e.g., `m_allNodes`, `m_children`) with public accessor methods like `getAllNodes()` and `getChildren()`. --- .../Simulation/Core/src/sofa/simulation/MappingGraph.h | 4 ++-- .../src/sofa/simulation/mappinggraph/BaseMappingGraphNode.h | 4 +++- .../Core/src/sofa/simulation/mappinggraph/ExportDot.cpp | 6 +++--- Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp | 2 -- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 60228a0f735..38a2eae79a7 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -216,7 +216,7 @@ class SOFA_SIMULATION_CORE_API MappingGraph */ void build(core::objectmodel::BaseContext* rootNode); - friend std::string SOFA_SIMULATION_CORE_API exportToDotFormat(const MappingGraph& graph); + const sofa::type::vector& getAllNodes() const { return m_allNodes; } private: ///< Root node used to start graph exploration during construction. @@ -233,7 +233,7 @@ class SOFA_SIMULATION_CORE_API MappingGraph std::map m_positionInGlobalMatrix; // Graph ownership structures: - std::vector m_allNodes; ///< All nodes in the graph. + sofa::type::vector m_allNodes; ///< All nodes in the graph. sofa::type::vector m_rootStates {}; ///< List of initial, unmapped mechanical states (graph roots). std::unordered_map m_stateIndex; ///< Quick lookup for a state's node. std::vector; friend class MappingGraph; friend struct MappingGraphAlgorithms; - friend std::string SOFA_SIMULATION_CORE_API exportToDotFormat(const MappingGraph& graph); virtual ~BaseMappingGraphNode() = default; @@ -71,6 +70,9 @@ class SOFA_SIMULATION_CORE_API BaseMappingGraphNode : public std::enable_shared_ */ bool isMapped() const; + const sofa::type::vector& getParents() const { return m_parents; } + const sofa::type::vector& getChildren() const { return m_children; } + private: sofa::type::vector m_parents; ///< prerequisite nodes (nodes pointing to this one) sofa::type::vector m_children; ///< dependent nodes (nodes pointed from this one) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.cpp index 0c81b1e9103..addd0d95ef6 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/ExportDot.cpp @@ -80,7 +80,7 @@ std::string exportToDotFormat(const MappingGraph& graph) ss << " rankdir=TB;\n"; // Top to Bottom layout is common for dependency graphs // 1. Add all nodes (vertices) - for (const auto& node : graph.m_allNodes) + for (const auto& node : graph.getAllNodes()) { const std::string label = "[" + getType(node.get()) + "]" + node->getName(); @@ -91,10 +91,10 @@ std::string exportToDotFormat(const MappingGraph& graph) } // 2. Add all edges (dependencies) - for (const auto& node : graph.m_allNodes) + for (const auto& node : graph.getAllNodes()) { const std::string sourceName = "Node_" + std::to_string(reinterpret_cast(node.get())); - for (auto child : node->m_children) + for (const auto& child : node->getChildren()) { const std::string targetName = "Node_" + std::to_string(reinterpret_cast(child.get())); diff --git a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp index 4eb7c790ece..243187ce5b2 100644 --- a/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp +++ b/Sofa/framework/Simulation/Core/test/MappingGraph2_test.cpp @@ -408,8 +408,6 @@ TEST(MappingGraph, ComplexGraphInteractionForceField) sofa::simulation::MappingGraph mappingGraph(root.get()); ASSERT_TRUE(mappingGraph.isBuilt()); - std::cout << exportToDotFormat(mappingGraph) << std::endl; - CollectNamesVisitor visitor; // Top Down Traversal Check From 7bdd6804496a966c3f22c083932ef672a2b54bd9 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 8 May 2026 10:49:54 +0200 Subject: [PATCH 38/41] Refactors ODESolvers and core simulation logic to utilize `MappingGraphMechanicalOperations` --- .../backward/EulerImplicitSolver.cpp | 4 +- .../odesolver/forward/EulerExplicitSolver.cpp | 20 ++- .../odesolver/forward/EulerExplicitSolver.h | 16 +-- Sofa/framework/Simulation/Core/CMakeLists.txt | 2 + .../MappingGraphMechanicalOperations.cpp | 124 ++++++++++++++++++ .../MappingGraphMechanicalOperations.h | 60 +++++++++ .../sofa/simulation/MechanicalOperations.cpp | 93 ------------- .../sofa/simulation/MechanicalOperations.h | 5 - 8 files changed, 205 insertions(+), 119 deletions(-) create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp create mode 100644 Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.h diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp index 630994b77a8..c5de5adf723 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include @@ -88,7 +88,7 @@ void EulerImplicitSolver::solve(const core::ExecParams* params, SReal dt, sofa:: 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 ); 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 961b743ed96..09b445d2839 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 @@ -27,10 +27,8 @@ #include #include #include -#include +#include #include -#include -using sofa::simulation::mechanicalvisitor::MechanicalGetNonDiagonalMassesCountVisitor; //#define SOFA_NO_VMULTIOP @@ -71,7 +69,7 @@ void EulerExplicitSolver::solve(const core::ExecParams* params, // 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 @@ -123,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, @@ -269,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"); @@ -279,7 +277,7 @@ void EulerExplicitSolver::addSeparateGravity(sofa::simulation::common::Mechanica mop->addSeparateGravity(dt, v); } -void EulerExplicitSolver::computeForce(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId f) const +void EulerExplicitSolver::computeForce(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId f) const { SCOPED_TIMER("ComputeForce"); @@ -289,7 +287,7 @@ void EulerExplicitSolver::computeForce(sofa::simulation::common::MechanicalOpera 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"); @@ -301,7 +299,7 @@ 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) { SCOPED_TIMER("projectResponse"); @@ -311,7 +309,7 @@ void EulerExplicitSolver::projectResponse(sofa::simulation::common::MechanicalOp mop->projectResponse(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"); @@ -319,7 +317,7 @@ void EulerExplicitSolver::solveConstraints(sofa::simulation::common::MechanicalO 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"); 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 e08fef94723..048bdaa8023 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 @@ -29,7 +29,7 @@ namespace sofa::simulation::common { -class MechanicalOperations; +class MappingGraphMechanicalOperations; class VectorOperations; } @@ -97,7 +97,7 @@ 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, @@ -105,23 +105,23 @@ class SOFA_COMPONENT_ODESOLVER_FORWARD_API EulerExplicitSolver : public sofa::co /// 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) - void computeForce(sofa::simulation::common::MechanicalOperations* mop, core::MultiVecDerivId f) const; + 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); + static void projectResponse(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId vecId); - 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; diff --git a/Sofa/framework/Simulation/Core/CMakeLists.txt b/Sofa/framework/Simulation/Core/CMakeLists.txt index 6f3eded17b1..006cecd588c 100644 --- a/Sofa/framework/Simulation/Core/CMakeLists.txt +++ b/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 @@ -179,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 diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp new file mode 100644 index 00000000000..0d464a07d4f --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp @@ -0,0 +1,124 @@ +/****************************************************************************** +* 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 * +******************************************************************************/ +#include + +namespace sofa::simulation::common +{ +void MappingGraphMechanicalOperations::computeForce(const MappingGraph& mappingGraph, + core::MultiVecDerivId result, + bool clearForceBefore, + bool accumulateForcesFromMappedStates, + TaskScheduler* taskScheduler) +{ + //assumes the mapping graph is valid and properly initialized + + setF(result); + if (clearForceBefore) + { + /** + * Reset forces on all mechanical states in the mapping graph. This operation can be performed + * in any order on all states in the graph. + */ + mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) + { + const core::VecDerivId& stateForce = result.getId(&state); + state.resetForce(&mparams, stateForce); + }); + } + + /** + * Compute f += externalForce on all mechanical states in the mapping graph. This operation can + * be performed in any order on all states in the graph. + */ + mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) + { + const core::VecDerivId& stateForce = result.getId(&state); + state.accumulateForce(&mparams, stateForce); + }); + + /** + * Compute f += f(x) on all force fields in the mapping graph. This operation can be performed + * in any order on all states in the graph, and can be parallelized among component groups. + */ + mappingGraph.algorithms.traverseComponentGroups_([&](core::behavior::BaseForceField& forceField) + { + forceField.addForce(&mparams, result); + }, sofa::simulation::VisitorApplication::ALL_NODES, taskScheduler); + + if (accumulateForcesFromMappedStates) + { + /** + * Compute f_in += J^T * f_out using mappings in the mapping graph. This operation must be + * performed in a bottom-up order to ensure correct force accumulation. + */ + mappingGraph.algorithms.traverseBottomUp_([&](core::BaseMapping& mapping) + { + mapping.applyJT(&mparams, result, result); + }); + } +} +void MappingGraphMechanicalOperations::addMBKv(const MappingGraph& mappingGraph, + core::MultiVecDerivId df, core::MatricesFactors::M m, + core::MatricesFactors::B b, + core::MatricesFactors::K k, bool clear, + bool accumulate) +{ + const core::ConstMultiVecDerivId dx = mparams.dx(); + mparams.setDx(mparams.v()); + setDf(df); + if (clear) + { + /** + * Reset forces on all mapped mechanical states in the mapping graph. This operation can be performed + * in any order on all mapped states in the graph. + */ + mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) + { + const core::VecDerivId& stateForce = df.getId(&state); + state.resetForce(&mparams, stateForce); + }, VisitorApplication::ONLY_MAPPED_NODES); + } + mparams.setBFactor(b.get()); + mparams.setKFactor(k.get()); + mparams.setMFactor(m.get()); + /* useV = true */ + + mappingGraph.algorithms.traverseComponentGroups_([&](core::behavior::BaseForceField& forceField) + { + forceField.addMBKdx(&mparams, df); + }); + + if (accumulate) + { + mappingGraph.algorithms.traverseBottomUp_([&](core::BaseMapping& mapping) + { + mapping.applyJT(&mparams, df, df); + if( mparams.kFactor() == 0 ) + { + mapping.applyDJT(&mparams, df, df); + } + }); + } + + mparams.setDx(dx); +} +} // namespace sofa::simulation::common diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.h new file mode 100644 index 00000000000..654d0da24b3 --- /dev/null +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.h @@ -0,0 +1,60 @@ +/****************************************************************************** +* 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::common +{ + +/** + * @brief Provides mechanical operations functionality using a MappingGraph. + * + * This class extends {@link MechanicalOperations}, providing methods that adapt core + * mechanical computations to use the topological structure provided by a {@link MappingGraph}. + * + * The standard {@link MechanicalOperations} typically relies on scene graph visitors, which may + * fail or yield incorrect results for complex mappings where the required execution order + * follows the specific dependencies defined in a mapping graph. Therefore, mechanical operations, + * especially those involving kinematic mappings, must be performed by traversing and respecting + * the structure of the MappingGraph to ensure computational consistency and correctness. + * + * Note: The use of {@link MappingGraphMechanicalOperations} is strongly recommended. In future + * versions, reliance on the scene graph will be deprecated, and this class will become the + * primary and exclusive mechanism for performing such operations. + */ +class SOFA_SIMULATION_CORE_API MappingGraphMechanicalOperations : public MechanicalOperations +{ +public: + using MechanicalOperations::MechanicalOperations; + + /// Compute the current force (given the latest propagated position and velocity) + void computeForce(const MappingGraph& mappingGraph, core::MultiVecDerivId result, bool clearForceBefore, bool accumulateForcesFromMappedStates, TaskScheduler* taskScheduler); + using MechanicalOperations::computeForce; + + /// accumulate $ df += (m M + b B + k K) velocity $ + void addMBKv(const MappingGraph& mappingGraph, core::MultiVecDerivId df, core::MatricesFactors::M m, core::MatricesFactors::B b, core::MatricesFactors::K k, bool clear = true, bool accumulate = true); + using MechanicalOperations::addMBKv; +}; + +} diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp index 68ab54c02e7..d6c0e0b020d 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.cpp @@ -259,57 +259,6 @@ void MechanicalOperations::computeForce(core::MultiVecDerivId result, bool clear executeVisitor( MechanicalComputeForceVisitor(&mparams, result, accumulate) ); } -void MechanicalOperations::computeForce(const MappingGraph& mappingGraph, - core::MultiVecDerivId result, bool clear, bool accumulate, - TaskScheduler* taskScheduler) -{ - //assumes the mapping graph is valid and properly initialized - - setF(result); - if (clear) - { - /** - * Reset forces on all mechanical states in the mapping graph. This operation can be performed - * in any order on all states in the graph. - */ - mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) - { - const VecDerivId& stateForce = result.getId(&state); - state.resetForce(&mparams, stateForce); - }); - } - - /** - * Compute f += externalForce on all mechanical states in the mapping graph. This operation can - * be performed in any order on all states in the graph. - */ - mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) - { - const VecDerivId& stateForce = result.getId(&state); - state.accumulateForce(&mparams, stateForce); - }); - - /** - * Compute f += f(x) on all force fields in the mapping graph. This operation can be performed - * in any order on all states in the graph, and can be parallelized among component groups. - */ - mappingGraph.algorithms.traverseComponentGroups_([&](core::behavior::BaseForceField& forceField) - { - forceField.addForce(&mparams, result); - }, sofa::simulation::VisitorApplication::ALL_NODES, taskScheduler); - - if (accumulate) - { - /** - * Compute f_in += J^T * f_out using mappings in the mapping graph. This operation must be - * performed in a bottom-up order to ensure correct force accumulation. - */ - mappingGraph.algorithms.traverseBottomUp_([&](core::BaseMapping& mapping) - { - mapping.applyJT(&mparams, result, result); - }); - } -} /// Compute the current force delta (given the latest propagated displacement) void MechanicalOperations::computeDf(core::MultiVecDerivId df, bool clear, bool accumulate) @@ -380,49 +329,7 @@ void MechanicalOperations::addMBKv(core::MultiVecDerivId df, mparams.setDx(dx); } -void MechanicalOperations::addMBKv(const MappingGraph& mappingGraph, core::MultiVecDerivId df, - core::MatricesFactors::M m, core::MatricesFactors::B b, - core::MatricesFactors::K k, bool clear, bool accumulate) -{ - const core::ConstMultiVecDerivId dx = mparams.dx(); - mparams.setDx(mparams.v()); - setDf(df); - if (clear) - { - /** - * Reset forces on all mapped mechanical states in the mapping graph. This operation can be performed - * in any order on all mapped states in the graph. - */ - mappingGraph.algorithms.traverse_([&](core::behavior::BaseMechanicalState& state) - { - const VecDerivId& stateForce = df.getId(&state); - state.resetForce(&mparams, stateForce); - }, VisitorApplication::ONLY_MAPPED_NODES); - } - mparams.setBFactor(b.get()); - mparams.setKFactor(k.get()); - mparams.setMFactor(m.get()); - /* useV = true */ - - mappingGraph.algorithms.traverseComponentGroups_([&](core::behavior::BaseForceField& forceField) - { - forceField.addMBKdx(&mparams, df); - }); - if (accumulate) - { - mappingGraph.algorithms.traverseBottomUp_([&](core::BaseMapping& mapping) - { - mapping.applyJT(&mparams, df, df); - if( mparams.kFactor() == 0 ) - { - mapping.applyDJT(&mparams, df, df); - } - }); - } - - mparams.setDx(dx); -} /// Add dt*Gravity to the velocity void MechanicalOperations::addSeparateGravity(SReal dt, core::MultiVecDerivId result) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h index 45bc02799ef..787e67fefd7 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MechanicalOperations.h @@ -29,7 +29,6 @@ #include #include #include -#include namespace sofa::simulation::common @@ -79,8 +78,6 @@ class SOFA_SIMULATION_CORE_API MechanicalOperations void computeEnergy(SReal &kineticEnergy, SReal &potentialEnergy); /// Compute the current force (given the latest propagated position and velocity) void computeForce(core::MultiVecDerivId result, bool clear = true, bool accumulate = true); - /// Compute the current force (given the latest propagated position and velocity) - void computeForce(const MappingGraph& mappingGraph, core::MultiVecDerivId result, bool clearForceBefore, bool accumulateForcesFromMappedStates, TaskScheduler* taskScheduler); /// Compute the current force delta (given the latest propagated displacement) void computeDf(core::MultiVecDerivId df, bool clear = true, bool accumulate = true); /// Compute the current force delta (given the latest propagated velocity) @@ -89,8 +86,6 @@ class SOFA_SIMULATION_CORE_API MechanicalOperations void addMBKdx(core::MultiVecDerivId df, core::MatricesFactors::M m, core::MatricesFactors::B b, core::MatricesFactors::K k, bool clear = true, bool accumulate = true); /// accumulate $ df += (m M + b B + k K) velocity $ void addMBKv(core::MultiVecDerivId df, core::MatricesFactors::M m, core::MatricesFactors::B b, core::MatricesFactors::K k, bool clear = true, bool accumulate = true); - /// accumulate $ df += (m M + b B + k K) velocity $ - void addMBKv(const MappingGraph& mappingGraph, core::MultiVecDerivId df, core::MatricesFactors::M m, core::MatricesFactors::B b, core::MatricesFactors::K k, bool clear = true, bool accumulate = true); /// Add dt*Gravity to the velocity void addSeparateGravity(SReal dt, core::MultiVecDerivId result = core::vec_id::write_access::velocity ); From 731111ed9b2cb119bd34b2524ecced8a78a8a19e Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 8 May 2026 14:56:19 +0200 Subject: [PATCH 39/41] fix wrong comparison --- .../src/sofa/simulation/MappingGraphMechanicalOperations.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp index 0d464a07d4f..452f9bd00a3 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp @@ -112,7 +112,7 @@ void MappingGraphMechanicalOperations::addMBKv(const MappingGraph& mappingGraph, mappingGraph.algorithms.traverseBottomUp_([&](core::BaseMapping& mapping) { mapping.applyJT(&mparams, df, df); - if( mparams.kFactor() == 0 ) + if( mparams.kFactor() != 0 ) { mapping.applyDJT(&mparams, df, df); } From 2f79376aa30c623b5dc1cb7820fb5eec89193c73 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 8 May 2026 15:14:47 +0200 Subject: [PATCH 40/41] support visits of projective constraint nodes --- .../Simulation/Core/src/sofa/simulation/MappingGraph.cpp | 2 ++ .../Simulation/Core/src/sofa/simulation/MappingGraph.h | 9 +++++---- .../src/sofa/simulation/mappinggraph/CallableVisitor.h | 6 ++++++ .../sofa/simulation/mappinggraph/MappingGraphVisitor.h | 7 +++++++ 4 files changed, 20 insertions(+), 4 deletions(-) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp index 862f14fd293..e990dc40a33 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.cpp @@ -17,6 +17,7 @@ MappingGraph::InputLists MappingGraph::InputLists::makeFromNode(core::objectmode node->getObjects(inputLists.mappings, core::objectmodel::BaseContext::SearchDirection::SearchDown); node->getObjects(inputLists.forceFields, core::objectmodel::BaseContext::SearchDirection::SearchDown); node->getObjects(inputLists.masses, core::objectmodel::BaseContext::SearchDirection::SearchDown); + node->getObjects(inputLists.projectedConstraints, core::objectmodel::BaseContext::SearchDirection::SearchDown); } return inputLists; } @@ -289,6 +290,7 @@ void MappingGraph::build(const InputLists& input) processComponents(input.forceFields); processComponents(input.masses); + processComponents(input.projectedConstraints); // 2. Wire leaf component edges: connectedState → leafComponent for (auto& [leafNode, states] : leafConnections) diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h index 38a2eae79a7..b0c6e67f383 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraph.h @@ -60,10 +60,11 @@ class SOFA_SIMULATION_CORE_API MappingGraph */ struct SOFA_SIMULATION_CORE_API InputLists { - sofa::type::vector mechanicalStates; ///< All Mechanical State inputs. - sofa::type::vector mappings; ///< All Mapping components. - sofa::type::vector forceFields; ///< All Force Field components. - sofa::type::vector masses; ///< All Mass components. + sofa::type::vector mechanicalStates; + sofa::type::vector mappings; + sofa::type::vector forceFields; + sofa::type::vector masses; + sofa::type::vector projectedConstraints; /** * @brief Creates InputLists from a context pointer. diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h index bc44da2d9d1..bb9656a648f 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/CallableVisitor.h @@ -69,6 +69,12 @@ struct GetComponentFromCallable using type = core::BaseMapping; }; +template requires std::is_invocable_v +struct GetComponentFromCallable +{ + using type = core::behavior::BaseProjectiveConstraintSet; +}; + template using CallableVisitor = BaseCallableVisitor::type>; diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.h index a777a926041..c627279ca1c 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/mappinggraph/MappingGraphVisitor.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace sofa::simulation { @@ -66,6 +67,12 @@ class MappingGraphVisitor * @param m The mass component to visit. */ virtual void visit(core::behavior::BaseMass&) {} + + /** + * @brief Visits a projective constraint node. + * @param pcs The projective constraint component to visit. + */ + virtual void visit(sofa::core::behavior::BaseProjectiveConstraintSet&) {} }; } From 40a735d880b65b919ed8135b89f0a4e1d5b8994f Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Fri, 8 May 2026 15:15:09 +0200 Subject: [PATCH 41/41] implement projectResponse in MappingGraphMechanicalOperations --- .../odesolver/backward/EulerImplicitSolver.cpp | 2 +- .../odesolver/forward/EulerExplicitSolver.cpp | 4 ++-- .../odesolver/forward/EulerExplicitSolver.h | 2 +- .../MappingGraphMechanicalOperations.cpp | 15 +++++++++++++++ .../simulation/MappingGraphMechanicalOperations.h | 4 ++++ 5 files changed, 23 insertions(+), 4 deletions(-) diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp index c5de5adf723..d58a5a71f72 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/EulerImplicitSolver.cpp @@ -159,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; } 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 09b445d2839..3e2b4179048 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 @@ -299,14 +299,14 @@ void EulerExplicitSolver::computeAcceleration(sofa::simulation::common::MappingG mop->accFromF(acc, f); } -void EulerExplicitSolver::projectResponse(sofa::simulation::common::MappingGraphMechanicalOperations* 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::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId acc) 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 048bdaa8023..66cd610d1e7 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 @@ -117,7 +117,7 @@ class SOFA_COMPONENT_ODESOLVER_FORWARD_API EulerExplicitSolver : public sofa::co core::ConstMultiVecDerivId f); /// Apply projective constraints, such as FixedProjectiveConstraint - static void projectResponse(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId vecId); + void projectResponse(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId vecId) const; static void solveConstraints(sofa::simulation::common::MappingGraphMechanicalOperations* mop, core::MultiVecDerivId acc); diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp index 452f9bd00a3..51ca30266b2 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.cpp @@ -23,6 +23,21 @@ namespace sofa::simulation::common { + +void MappingGraphMechanicalOperations::projectResponse(const MappingGraph& mappingGraph, + core::MultiVecDerivId dx, double** W) +{ + setDx(dx); + mappingGraph.algorithms.traverse_([&](core::behavior::BaseProjectiveConstraintSet& constraint) + { + constraint.projectResponse(&mparams, dx); + if (W != nullptr) + { + constraint.projectResponse(&mparams, W); + } + }); +} + void MappingGraphMechanicalOperations::computeForce(const MappingGraph& mappingGraph, core::MultiVecDerivId result, bool clearForceBefore, diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.h b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.h index 654d0da24b3..80a12a60945 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.h +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/MappingGraphMechanicalOperations.h @@ -48,6 +48,10 @@ class SOFA_SIMULATION_CORE_API MappingGraphMechanicalOperations : public Mechani public: using MechanicalOperations::MechanicalOperations; + /// Apply projective constraints to the given vector + void projectResponse(const MappingGraph& mappingGraph, core::MultiVecDerivId dx, double** W = nullptr); + using MechanicalOperations::projectResponse; + /// Compute the current force (given the latest propagated position and velocity) void computeForce(const MappingGraph& mappingGraph, core::MultiVecDerivId result, bool clearForceBefore, bool accumulateForcesFromMappedStates, TaskScheduler* taskScheduler); using MechanicalOperations::computeForce;