[LinearSystem] Contributions to the global matrix in the order of the scene graph traversal

Sofa/Component/LinearSystem/src/sofa/component/linearsystem/MatrixLinearSystem.h

@@ -112,19 +112,23 @@ class MatrixLinearSystem : public TypedMatrixLinearSystem<TMatrix, TVector>
     std::map<BaseMapping*, core::GeometricStiffnessMatrix> m_geometricStiffness;
     std::map<BaseMass*, BaseAssemblingMatrixAccumulator<Contribution::MASS>*> m_mass;
 
+    /**
+     * Components contributing to the same matrix.
+     * They are stored in the order of the scene graph traversal, which is the order of their contributions
+     */
     struct IndependentContributors
     {
-        std::map<BaseForceField*, core::behavior::StiffnessMatrix> m_stiffness;
-        std::map<BaseForceField*, core::behavior::DampingMatrix> m_damping;
-        std::map<BaseMapping*, core::GeometricStiffnessMatrix> m_geometricStiffness;
-        std::map<BaseMass*, BaseAssemblingMatrixAccumulator<Contribution::MASS>*> m_mass;
+        sofa::type::vector<std::pair<BaseForceField*, core::behavior::StiffnessMatrix> > m_stiffness;
+        sofa::type::vector<std::pair<BaseForceField*, core::behavior::DampingMatrix> > m_damping;
+        sofa::type::vector<std::pair<BaseMapping*, core::GeometricStiffnessMatrix> > m_geometricStiffness;
+        sofa::type::vector<std::pair<BaseMass*, BaseAssemblingMatrixAccumulator<Contribution::MASS>*> > m_mass;
         int id {};
     };
 
     sofa::type::vector<IndependentContributors> m_independentContributors;
 
 
-    /// List of shared local matrices under mappings
+    /// List of shared local matrices under mappings, in the order of the scene graph traversal
     sofa::type::vector< std::pair<
         PairMechanicalStates,
         std::shared_ptr<LocalMappedMatrixType<Real> >
@@ -315,6 +319,9 @@ struct GroupOfComponentsAssociatedToAPairOfMechanicalStates
     std::set<BaseMass*> masses;
     std::set<BaseMapping*> mappings;
 
+    /// Rank of the pair of mechanical states when it is first encountered in the scene graph traversal
+    std::size_t order {};
+
     friend std::ostream& operator<<(std::ostream& os,
         const GroupOfComponentsAssociatedToAPairOfMechanicalStates& group);
 };

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

@@ -222,23 +222,27 @@ inline sofa::type::vector<core::behavior::BaseMechanicalState*> retrieveAssociat
     mstates.reserve(mstatesLinks.size());
     for (auto m : mstatesLinks)
     {
-        mstates.push_back(m);
+        //skip duplicates: it may happen for InteractionForceFields
+        if (std::find(mstates.begin(), mstates.end(), m) == mstates.end())
+        {
+            mstates.push_back(m);
+        }
     }
 
-    //remove duplicates: it may happen for InteractionForceFields
-    std::sort( mstates.begin(), mstates.end() );
-    mstates.erase( std::unique( mstates.begin(), mstates.end() ), mstates.end() );
-
     return mstates;
 }
 
 inline sofa::type::vector<core::behavior::BaseMechanicalState*> retrieveAssociatedMechanicalState(BaseMapping* component)
 {
-    type::vector<core::behavior::BaseMechanicalState*> mstates = component->getMechFrom();
-
-    //remove duplicates: it may happen for MultiMappings
-    std::sort( mstates.begin(), mstates.end() );
-    mstates.erase( std::unique( mstates.begin(), mstates.end() ), mstates.end() );
+    type::vector<core::behavior::BaseMechanicalState*> mstates;
+    for (auto* m : component->getMechFrom())
+    {
+        //skip duplicates: it may happen for MultiMappings
+        if (std::find(mstates.begin(), mstates.end(), m) == mstates.end())
+        {
+            mstates.push_back(m);
+        }
+    }
 
     return mstates;
 }
@@ -333,12 +337,22 @@ template <class TMatrix, class TVector>
 void MatrixLinearSystem<TMatrix, TVector>::buildGroupsOfComponentAssociatedToMechanicalStates(
     std::map<PairMechanicalStates, GroupOfComponentsAssociatedToAPairOfMechanicalStates>& groups)
 {
+    const auto groupOf = [&groups](const PairMechanicalStates& pair) -> GroupOfComponentsAssociatedToAPairOfMechanicalStates&
+    {
+        const auto [it, inserted] = groups.try_emplace(pair);
+        if (inserted)
+        {
+            it->second.order = groups.size() - 1;
+        }
+        return it->second;
+    };
+
     for (auto* ff : this->m_forceFields)
     {
         const auto mstates = retrieveAssociatedMechanicalState(ff);
         for (auto& pair : generatePairs(mstates))
         {
-            groups[pair].forcefieds.insert(ff);
+            groupOf(pair).forcefieds.insert(ff);
         }
     }
 
@@ -347,7 +361,7 @@ void MatrixLinearSystem<TMatrix, TVector>::buildGroupsOfComponentAssociatedToMec
         const auto mstates = retrieveAssociatedMechanicalState(mass);
         for (auto& pair : generatePairs(mstates))
         {
-            groups[pair].masses.insert(mass);
+            groupOf(pair).masses.insert(mass);
         }
     }
 
@@ -356,7 +370,7 @@ void MatrixLinearSystem<TMatrix, TVector>::buildGroupsOfComponentAssociatedToMec
         const auto mstates = retrieveAssociatedMechanicalState(mapping);
         for (auto& pair : generatePairs(mstates))
         {
-            groups[pair].mappings.insert(mapping);
+            groupOf(pair).mappings.insert(mapping);
         }
     }
 }
@@ -376,8 +390,20 @@ void MatrixLinearSystem<TMatrix, TVector>::makeLocalMatrixGroups(const core::Mec
     std::map<PairMechanicalStates, GroupOfComponentsAssociatedToAPairOfMechanicalStates> groups;
     buildGroupsOfComponentAssociatedToMechanicalStates(groups);
 
-    for (const auto& [pair, group] : groups)
+    // The mapped matrices are projected into the global matrix in the order of m_localMappedMatrices: the pairs are
+    // processed in the order of the scene graph traversal, rather than in the order of the addresses of the map keys
+    sofa::type::vector<const std::pair<const PairMechanicalStates, GroupOfComponentsAssociatedToAPairOfMechanicalStates>*> orderedGroups;
+    orderedGroups.reserve(groups.size());
+    for (const auto& entry : groups)
+    {
+        orderedGroups.push_back(&entry);
+    }
+    std::sort(orderedGroups.begin(), orderedGroups.end(),
+        [](const auto* a, const auto* b) { return a->second.order < b->second.order; });
+
+    for (const auto* entry : orderedGroups)
     {
+        const auto& [pair, group] = *entry;
         const bool isMapped1 = m_mappingGraph.hasAnyMappingInput(pair[0]);
         const bool isMapped2 = m_mappingGraph.hasAnyMappingInput(pair[1]);
 
@@ -534,78 +560,45 @@ void MatrixLinearSystem<TMatrix, TVector>::makeIndependentLocalMatrixGroups()
     IndependentContributors nonMappedContributors;
     IndependentContributors mappedContributors;
 
-    for (auto& [component, localMatrix] : m_stiffness)
+    // The components are visited in the order of the scene graph traversal
+    // so that their contributions are summed in an order which does not change between runs
+    const auto dispatch = [this, &nonMappedContributors, &mappedContributors]<Contribution c>(
+        const auto& components, const auto& localMatrices, auto member)
     {
-        const auto& mappedMatrices = getLocalMatrixMap<Contribution::STIFFNESS>().mappedLocalMatrix;
-        if (mappedMatrices.find(component) == mappedMatrices.end()) //this component is not mapped
+        const auto& mappedMatrices = this->template getLocalMatrixMap<c>().mappedLocalMatrix;
+        for (auto* component : components)
         {
-            //confirmation that this component is not mapped:
-            if (m_mappingGraph.hasAnyMappingInput(component))
+            const auto it = localMatrices.find(component);
+            if (it == localMatrices.end())
             {
-                dmsg_error() << "A mapped component has no mapped local matrix. This should not happen.";
                 continue;
             }
 
-            nonMappedContributors.m_stiffness.insert({component, localMatrix});
-        }
-        else
-        {
-            mappedContributors.m_stiffness.insert({component, localMatrix});
-        }
-    }
-
-    for (auto& [component, localMatrix] : m_damping)
-    {
-        const auto& mappedMatrices = getLocalMatrixMap<Contribution::DAMPING>().mappedLocalMatrix;
-        if (mappedMatrices.find(component) == mappedMatrices.end()) //this component is not mapped
-        {
-            //confirmation that this component is not mapped:
-            if (m_mappingGraph.hasAnyMappingInput(component))
+            if (mappedMatrices.find(component) == mappedMatrices.end()) //this component is not mapped
             {
-                dmsg_error() << "A mapped component has no mapped local matrix. This should not happen.";
-                continue;
-            }
-
-            nonMappedContributors.m_damping.insert({component, localMatrix});
-        }
-        else
-        {
-            mappedContributors.m_damping.insert({component, localMatrix});
-        }
-    }
-
-    for (auto& [component, localMatrix] : m_geometricStiffness)
-    {
-        const auto& mappedMatrices = getLocalMatrixMap<Contribution::GEOMETRIC_STIFFNESS>().mappedLocalMatrix;
-        if (mappedMatrices.find(component) == mappedMatrices.end()) //this component is not mapped
-        {
-            nonMappedContributors.m_geometricStiffness.insert({component, localMatrix});
-        }
-        else
-        {
-            mappedContributors.m_geometricStiffness.insert({component, localMatrix});
-        }
-    }
+                if constexpr (c != Contribution::GEOMETRIC_STIFFNESS)
+                {
+                    //confirmation that this component is not mapped:
+                    if (this->m_mappingGraph.hasAnyMappingInput(component))
+                    {
+                        dmsg_error() << "A mapped component has no mapped local matrix. This should not happen.";
+                        continue;
+                    }
+                }
 
-    for (auto& [component, localMatrix] : m_mass)
-    {
-        const auto& mappedMatrices = getLocalMatrixMap<Contribution::MASS>().mappedLocalMatrix;
-        if (mappedMatrices.find(component) == mappedMatrices.end()) //this component is not mapped
-        {
-            //confirmation that this component is not mapped:
-            if (m_mappingGraph.hasAnyMappingInput(component))
+                (nonMappedContributors.*member).emplace_back(component, it->second);
+            }
+            else
             {
-                dmsg_error() << "A mapped component has no mapped local matrix. This should not happen.";
-                continue;
+                (mappedContributors.*member).emplace_back(component, it->second);
             }
-
-            nonMappedContributors.m_mass.insert({component, localMatrix});
-        }
-        else
-        {
-            mappedContributors.m_mass.insert({component, localMatrix});
         }
-    }
+    };
+
+    dispatch.template operator()<Contribution::STIFFNESS>(this->m_forceFields, m_stiffness, &IndependentContributors::m_stiffness);
+    dispatch.template operator()<Contribution::DAMPING>(this->m_forceFields, m_damping, &IndependentContributors::m_damping);
+    dispatch.template operator()<Contribution::GEOMETRIC_STIFFNESS>(this->m_mechanicalMappings, m_geometricStiffness, &IndependentContributors::m_geometricStiffness);
+    dispatch.template operator()<Contribution::MASS>(this->m_masses, m_mass, &IndependentContributors::m_mass);
 
     m_independentContributors.push_back(nonMappedContributors);
     m_independentContributors.push_back(mappedContributors);

Sofa/Component/LinearSystem/tests/MatrixLinearSystem_test.cpp

@@ -370,3 +370,119 @@ TEST(LinearSystem, MatrixSystem_buggyForceField)
     }
 }
 
+
+/// Records the order in which the stiffness matrices of the force fields are built
+template<class DataTypes>
+class OrderRecordingForceField : public sofa::core::behavior::ForceField<DataTypes>
+{
+public:
+    SOFA_CLASS(OrderRecordingForceField, sofa::core::behavior::ForceField<DataTypes>);
+    inline static std::vector<std::string> buildOrder;
+
+    void buildStiffnessMatrix(sofa::core::behavior::StiffnessMatrix* matrix) override
+    {
+        buildOrder.push_back(this->getName());
+        auto dfdx = matrix->getForceDerivativeIn(this->mstate).withRespectToPositionsIn(this->mstate);
+        dfdx(0, 0) += 1.;
+    }
+    void addForce(const sofa::core::MechanicalParams*, typename Inherit1::DataVecDeriv&, const typename Inherit1::DataVecCoord&, const typename Inherit1::DataVecDeriv&) override {}
+    void addDForce(const sofa::core::MechanicalParams*, typename Inherit1::DataVecDeriv&, const typename Inherit1::DataVecDeriv&) override {}
+    SReal getPotentialEnergy(const sofa::core::MechanicalParams*, const typename Inherit1::DataVecCoord&) const override { return 0; }
+};
+
+/// The force fields contributing to the same entries of the global matrix are assembled in an order which does not
+/// depend on their addresses (the rounding of the sums would change from one run to another): here, in the order of
+/// the scene graph
+TEST(LinearSystem, MatrixSystem_contributionsInAnOrderIndependentOfTheAddresses)
+{
+    using FF = OrderRecordingForceField<sofa::defaulttype::Vec3Types>;
+    const sofa::simulation::Node::SPtr root = sofa::core::objectmodel::New<sofa::simulation::Node>();
+    using MatrixSystem = sofa::component::linearsystem::MatrixLinearSystem<sofa::linearalgebra::FullMatrix<SReal>, sofa::linearalgebra::FullVector<SReal> >;
+    const MatrixSystem::SPtr linearSystem = sofa::core::objectmodel::New<MatrixSystem>();
+    root->addObject(linearSystem);
+    const auto mstate = sofa::core::objectmodel::New<sofa::component::statecontainer::MechanicalObject<sofa::defaulttype::Vec3Types> >();
+    root->addObject(mstate);
+    mstate->resize(2);
+
+    // allocated in an order, added to the scene graph in the reverse order
+    std::vector<FF::SPtr> forceFields;
+    for (int i = 0; i < 8; ++i)
+    {
+        forceFields.push_back(sofa::core::objectmodel::New<FF>());
+        forceFields.back()->setName("forceField" + std::to_string(i));
+    }
+    for (auto it = forceFields.rbegin(); it != forceFields.rend(); ++it)
+    {
+        root->addObject(*it);
+    }
+
+    auto mparams = *sofa::core::MechanicalParams::defaultInstance();
+    mparams.setKFactor(1._sreal);
+    root->init(&mparams);
+
+    FF::buildOrder.clear();
+    linearSystem->buildSystemMatrix(&mparams);
+    const std::vector<std::string> expected { "forceField7", "forceField6", "forceField5", "forceField4",
+                                              "forceField3", "forceField2", "forceField1", "forceField0" };
+    EXPECT_EQ(FF::buildOrder, expected);
+}
+
+/// Adds its value to the entry (0, 0) of the stiffness matrix
+template<class DataTypes>
+class ValueForceField : public sofa::core::behavior::ForceField<DataTypes>
+{
+public:
+    SOFA_CLASS(ValueForceField, sofa::core::behavior::ForceField<DataTypes>);
+    SReal value {};
+
+    void buildStiffnessMatrix(sofa::core::behavior::StiffnessMatrix* matrix) override
+    {
+        auto dfdx = matrix->getForceDerivativeIn(this->mstate).withRespectToPositionsIn(this->mstate);
+        dfdx(0, 0) += value;
+    }
+    void addForce(const sofa::core::MechanicalParams*, typename Inherit1::DataVecDeriv&, const typename Inherit1::DataVecCoord&, const typename Inherit1::DataVecDeriv&) override {}
+    void addDForce(const sofa::core::MechanicalParams*, typename Inherit1::DataVecDeriv&, const typename Inherit1::DataVecDeriv&) override {}
+    SReal getPotentialEnergy(const sofa::core::MechanicalParams*, const typename Inherit1::DataVecCoord&) const override { return 0; }
+};
+
+/// The same scene gives the same global matrix, whatever the order in which its components are allocated: three
+/// force fields add 1, 1e16 and -1e16 to the same entry, whose floating-point sum is 0 or 1 depending on the order of
+/// the additions. The scene (same components, names and graph order) is built twice, with the force fields allocated
+/// in two opposite orders: the assembled entries must be identical
+TEST(LinearSystem, MatrixSystem_sameMatrixWhateverTheAddressesOfTheComponents)
+{
+    using FF = ValueForceField<sofa::defaulttype::Vec3Types>;
+    using MatrixSystem = sofa::component::linearsystem::MatrixLinearSystem<sofa::linearalgebra::FullMatrix<SReal>, sofa::linearalgebra::FullVector<SReal> >;
+
+    const auto assembledEntry = [](bool reverseAllocation)
+    {
+        const sofa::simulation::Node::SPtr root = sofa::core::objectmodel::New<sofa::simulation::Node>();
+        const MatrixSystem::SPtr linearSystem = sofa::core::objectmodel::New<MatrixSystem>();
+        root->addObject(linearSystem);
+        const auto mstate = sofa::core::objectmodel::New<sofa::component::statecontainer::MechanicalObject<sofa::defaulttype::Vec3Types> >();
+        root->addObject(mstate);
+        mstate->resize(1);
+
+        const std::vector<std::pair<std::string, SReal>> forceFields { {"a", 1.}, {"b", 1e16}, {"c", -1e16} };
+        std::vector<FF::SPtr> allocated(forceFields.size());
+        for (std::size_t k = 0; k < forceFields.size(); ++k)
+        {
+            const std::size_t i = reverseAllocation ? forceFields.size() - 1 - k : k;
+            allocated[i] = sofa::core::objectmodel::New<FF>();
+        }
+        for (std::size_t i = 0; i < forceFields.size(); ++i)
+        {
+            allocated[i]->setName(forceFields[i].first);
+            allocated[i]->value = forceFields[i].second;
+            root->addObject(allocated[i]);
+        }
+
+        auto mparams = *sofa::core::MechanicalParams::defaultInstance();
+        mparams.setKFactor(1._sreal);
+        root->init(&mparams);
+        linearSystem->buildSystemMatrix(&mparams);
+        return linearSystem->getSystemMatrix()->element(0, 0);
+    };
+
+    EXPECT_EQ(assembledEntry(false), assembledEntry(true));
+}