[Lagrangian] Refactoring of GenericConstraintSolver (#5666)

* First working version of exploded constraint problrms. Still lots of info in GenericCosntraintSolver data that are dependent on the method type. Need to find a way of setting the solving method by adding a component in the scene.

* Changed all problems into solvers. It makes more sense to use an inheritance. Now the constraintProblem is only a container

* Apply changes to scenes, tests and tutorials

* Apply review comments

* change ownership of d_multithreading

* Add comment on do methods and changed signature to add constraint problem

* Add scene check to fix and display info if GenericConstraintSolver is still being used

* Revert "Add scene check to fix and display info if GenericConstraintSolver is still being used"

This reverts commit dd1c031.

* Add component change for GenericConstraintSolver

---------

Co-authored-by: Hugo <hugo.talbot@sofa-framework.org>

Author: bakpaul

Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp

@@ -44,10 +44,12 @@
 #include <sofa/simulation/CollisionVisitor.h>
 #include <sofa/simulation/SolveVisitor.h>
 #include <sofa/simulation/MainTaskSchedulerFactory.h>
+#include <sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h>
 
 #include <sofa/simulation/mechanicalvisitor/MechanicalVInitVisitor.h>
 using sofa::simulation::mechanicalvisitor::MechanicalVInitVisitor;
 
+
 #include <sofa/simulation/mechanicalvisitor/MechanicalBeginIntegrationVisitor.h>
 using sofa::simulation::mechanicalvisitor::MechanicalBeginIntegrationVisitor;
 
@@ -67,7 +69,7 @@ using namespace core::behavior;
 using namespace sofa::simulation;
 using sofa::helper::ScopedAdvancedTimer;
 
-using DefaultConstraintSolver = sofa::component::constraint::lagrangian::solver::GenericConstraintSolver;
+using DefaultConstraintSolver = sofa::component::constraint::lagrangian::solver::ProjectedGaussSeidelConstraintSolver;
 
 FreeMotionAnimationLoop::FreeMotionAnimationLoop() :
     d_solveVelocityConstraintFirst(initData(&d_solveVelocityConstraintFirst , false, "solveVelocityConstraintFirst", "solve separately velocity constraint violations before position constraint violations"))

Sofa/Component/Constraint/Lagrangian/Model/tests/scenes_test/BilateralInteractionConstraint.scn

@@ -2,7 +2,7 @@
 <Node name="root" dt="0.001" gravity="0 -981 0">
     <VisualStyle displayFlags="showForceFields" />
     <FreeMotionAnimationLoop />
-    <GenericConstraintSolver maxIterations="100" tolerance="1e-3" />
+    <ProjectedGaussSeidelConstraintSolver maxIterations="100" tolerance="1e-3" />
     <CollisionPipeline  depth="6" verbose="0" draw="0" />
     <BruteForceBroadPhase/>
     <BVHNarrowPhase/>

Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt

@@ -8,7 +8,13 @@ set(HEADER_FILES
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/init.h
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ConstraintSolverImpl.h
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintProblem.h
+
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.h
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintSolver.h
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintSolver.h
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintSolver.h
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintSolver.h
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltGaussSeidelConstraintSolver.h
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/LCPConstraintSolver.h
 
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/visitors/ConstraintStoreLambdaVisitor.h
@@ -20,7 +26,13 @@ set(SOURCE_FILES
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/init.cpp
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ConstraintSolverImpl.cpp
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintProblem.cpp
+
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.cpp
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintSolver.cpp
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintSolver.cpp
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintSolver.cpp
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintSolver.cpp
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltGaussSeidelConstraintSolver.cpp
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/LCPConstraintSolver.cpp
 
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/visitors/ConstraintStoreLambdaVisitor.cpp

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp

@@ -0,0 +1,115 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program is free software; you can redistribute it and/or modify it     *
+* under the terms of the GNU Lesser General Public License as published by    *
+* the Free Software Foundation; either version 2.1 of the License, or (at     *
+* your option) any later version.                                             *
+*                                                                             *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details.                                                           *
+*                                                                             *
+* You should have received a copy of the GNU Lesser General Public License    *
+* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: contact@sofa-framework.org                             *
+******************************************************************************/
+
+#include <sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h>
+
+#include <sofa/helper/ScopedAdvancedTimer.h>
+#include <sofa/simulation/MainTaskSchedulerFactory.h>
+#include <sofa/simulation/ParallelForEach.h>
+
+namespace sofa::component::constraint::lagrangian::solver
+{
+BuiltConstraintSolver::BuiltConstraintSolver()
+: d_multithreading(initData(&d_multithreading, false, "multithreading", "Build compliances concurrently"))
+{}
+
+void BuiltConstraintSolver::init()
+{
+    Inherit1::init();
+    if(d_multithreading.getValue())
+    {
+        simulation::MainTaskSchedulerFactory::createInRegistry()->init();
+    }
+}
+
+void BuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints)
+{
+    SOFA_UNUSED(numConstraints);
+    SCOPED_TIMER_VARNAME(getComplianceTimer, "Get Compliance");
+    dmsg_info() <<" computeCompliance in "  << l_constraintCorrections.size()<< " constraintCorrections" ;
+
+    const bool multithreading = d_multithreading.getValue();
+
+    const simulation::ForEachExecutionPolicy execution = multithreading ?
+        simulation::ForEachExecutionPolicy::PARALLEL :
+        simulation::ForEachExecutionPolicy::SEQUENTIAL;
+
+    simulation::TaskScheduler* taskScheduler = simulation::MainTaskSchedulerFactory::createInRegistry();
+    assert(taskScheduler);
+
+    //Used to prevent simultaneous accesses to the main compliance matrix
+    std::mutex mutex;
+
+    //Visits all constraint corrections to compute the compliance matrix projected
+    //in the constraint space.
+    simulation::forEachRange(execution, *taskScheduler,  l_constraintCorrections.begin(),  l_constraintCorrections.end(),
+        [&cParams, this, &multithreading, &mutex, problem](const auto& range)
+        {
+            ComplianceWrapper compliance(problem->W, multithreading);
+
+            for (auto it = range.start; it != range.end; ++it)
+            {
+                core::behavior::BaseConstraintCorrection* cc = *it;
+                if (cc->isActive())
+                {
+                    cc->addComplianceInConstraintSpace(cParams, &compliance.matrix());
+                }
+            }
+
+            std::lock_guard guard(mutex);
+            compliance.assembleMatrix();
+        });
+
+    addRegularization(problem->W,  d_regularizationTerm.getValue());
+    dmsg_info() << " computeCompliance_done "  ;
+}
+
+
+BuiltConstraintSolver::ComplianceWrapper::ComplianceMatrixType& BuiltConstraintSolver::ComplianceWrapper::matrix()
+{
+    if (m_isMultiThreaded)
+    {
+        if (!m_threadMatrix)
+        {
+            m_threadMatrix = std::make_unique<ComplianceMatrixType>();
+            m_threadMatrix->resize(m_complianceMatrix.rowSize(), m_complianceMatrix.colSize());
+        }
+        return *m_threadMatrix;
+    }
+    return m_complianceMatrix;
+}
+
+void BuiltConstraintSolver::ComplianceWrapper::assembleMatrix() const
+{
+    if (m_threadMatrix)
+    {
+        for (linearalgebra::BaseMatrix::Index j = 0; j < m_threadMatrix->rowSize(); ++j)
+        {
+            for (linearalgebra::BaseMatrix::Index l = 0; l < m_threadMatrix->colSize(); ++l)
+            {
+                m_complianceMatrix.add(j, l, m_threadMatrix->element(j,l));
+            }
+        }
+    }
+}
+
+}

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h

@@ -0,0 +1,68 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program is free software; you can redistribute it and/or modify it     *
+* under the terms of the GNU Lesser General Public License as published by    *
+* the Free Software Foundation; either version 2.1 of the License, or (at     *
+* your option) any later version.                                             *
+*                                                                             *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details.                                                           *
+*                                                                             *
+* You should have received a copy of the GNU Lesser General Public License    *
+* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: contact@sofa-framework.org                             *
+******************************************************************************/
+#pragma once
+
+#include <sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h>
+
+namespace sofa::component::constraint::lagrangian::solver
+{
+/**
+ *  \brief This component implements a generic way of building system for solvers that use a built
+ *  version of the constraint matrix. Any solver that uses a build matrix should inherit from this.
+ *  This component is purely virtual because doSolve is not defined and needs to be defined in the
+ *  inherited class
+ */
+class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintSolver : public GenericConstraintSolver
+{
+
+
+public:
+    SOFA_CLASS(BuiltConstraintSolver, GenericConstraintSolver);
+    Data<bool> d_multithreading; ///< Build compliances concurrently
+
+    BuiltConstraintSolver();
+
+    virtual void init() override;
+
+protected:
+    virtual void doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints) override;
+
+private:
+
+    struct ComplianceWrapper
+    {
+        using ComplianceMatrixType = sofa::linearalgebra::LPtrFullMatrix<SReal>;
+
+        ComplianceWrapper(ComplianceMatrixType& complianceMatrix, bool isMultiThreaded)
+        : m_isMultiThreaded(isMultiThreaded), m_complianceMatrix(complianceMatrix) {}
+
+        ComplianceMatrixType& matrix();
+
+        void assembleMatrix() const;
+
+    private:
+        bool m_isMultiThreaded { false };
+        ComplianceMatrixType& m_complianceMatrix;
+        std::unique_ptr<ComplianceMatrixType> m_threadMatrix;
+    };
+};
+}

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.cpp

@@ -51,11 +51,11 @@ void ConstraintProblem::clear(int nbConstraints)
     dFree.resize(nbConstraints);
     f.resize(nbConstraints);
 
-    static std::atomic<unsigned int> counter = 0;
+    static std::atomic<unsigned> counter = 0;
     problemId = counter.fetch_add(1, std::memory_order_relaxed);
 }
 
-unsigned int ConstraintProblem::getProblemId()
+unsigned ConstraintProblem::getProblemId() const
 {
     return problemId;
 }

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h

@@ -59,18 +59,19 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem
 
     // Returns the number of scalar constraints, or equivalently the number of Lagrange multipliers
     int getDimension() const { return dimension; }
+    void setDimension(int dim) { dimension = dim; }
 
     SReal** getW() { return W.lptr(); }
     SReal* getDfree() { return dFree.ptr(); }
     SReal* getF() { return f.ptr(); }
 
     virtual void solveTimed(SReal tolerance, int maxIt, SReal timeout) = 0;
 
-    unsigned int getProblemId();
+    unsigned getProblemId() const;
 
 protected:
     int dimension;
-    unsigned int problemId;
+    unsigned problemId;
 };
 
 
@@ -93,16 +94,17 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintSolverImpl : pub
 
     void removeConstraintCorrection(core::behavior::BaseConstraintCorrection *s) override;
 
+    MultiLink< ConstraintSolverImpl,
+        core::behavior::BaseConstraintCorrection,
+        BaseLink::FLAG_STOREPATH> l_constraintCorrections;
+
 protected:
 
     void postBuildSystem(const core::ConstraintParams* cParams) override;
     void postSolveSystem(const core::ConstraintParams* cParams) override;
 
     void clearConstraintCorrections();
 
-    MultiLink< ConstraintSolverImpl,
-        core::behavior::BaseConstraintCorrection,
-        BaseLink::FLAG_STOREPATH> l_constraintCorrections;
 
     /// Calls the method resetConstraint on all the mechanical states and BaseConstraintSet
     /// In the case of a MechanicalObject, it clears the constraint jacobian matrix