[Lagrangian.Solver] UnbuiltConstraintSolver: implement hotstart

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

@@ -186,9 +186,15 @@ bool GenericConstraintSolver::buildSystem(const core::ConstraintParams *cParams,
     // suppress the constraints that are on DOFS currently concerned by projective constraint
     applyProjectiveConstraintOnConstraintMatrix(cParams);
 
+    // Get constraint info for hot-start BEFORE clear() resets the constraint count check
+    preClearCorrection(cParams);
+
     //clear and/or resize based on the number of constraints
     current_cp->clear(numConstraints);
 
+    // Restore initial guess from previous timestep AFTER clear() zeros the forces
+    postClearCorrection();
+
     getConstraintViolation(cParams, &current_cp->dFree);
 
     {
@@ -380,6 +386,8 @@ void GenericConstraintSolver::storeConstraintLambdas(const core::ConstraintParam
 
 bool GenericConstraintSolver::applyCorrection(const core::ConstraintParams *cParams, MultiVecId res1, MultiVecId res2)
 {
+    doPreApplyCorrection();
+
     computeAndApplyMotionCorrection(cParams, res1, res2);
     storeConstraintLambdas(cParams);
 
@@ -440,7 +448,4 @@ void GenericConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W, co
     }
 }
 
-
-
-
 } //namespace sofa::component::constraint::lagrangian::solver

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

@@ -96,6 +96,9 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver :
     sofa::core::MultiVecDerivId m_dxId;
 
     virtual void initializeConstraintProblems();
+    virtual void preApplyCorrection() { doPreApplyCorrection(); }
+    void preClearCorrection(const core::ConstraintParams* cparams) { doPreClearCorrection(cparams); }
+    void postClearCorrection() { doPostClearCorrection(); }
 
     /*****
      *
@@ -122,11 +125,13 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver :
      *
      */
     virtual void doSolve( GenericConstraintProblem * problem, SReal timeout = 0.0) = 0;
+
+    virtual void doPreApplyCorrection() { }
+    virtual void doPreClearCorrection(const core::ConstraintParams* cparams) { SOFA_UNUSED(cparams); }
+    virtual void doPostClearCorrection() { }
 
     virtual void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization);
 
-
-
 private:
 
     sofa::type::vector<core::behavior::BaseConstraintCorrection*> filteredConstraintCorrections() const;

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

@@ -24,9 +24,18 @@
 #include <sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.h>
 #include <sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h>
 
+#include <sofa/helper/ScopedAdvancedTimer.h>
+
 namespace sofa::component::constraint::lagrangian::solver
 {
 
+UnbuiltConstraintSolver::UnbuiltConstraintSolver()
+: GenericConstraintSolver()
+, d_initialGuess(initData(&d_initialGuess, false, "initialGuess", "Activate constraint force history to possibly improve convergence (hot start)."))
+{
+
+}
+
 void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem, unsigned int numConstraints)
 {
     SOFA_UNUSED(cParams);
@@ -37,13 +46,14 @@ void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cPara
         return;
     }
 
+    // Initialize constraint sequence ONCE before iterating over constraint corrections
+    c_current_cp->constraints_sequence.resize(numConstraints);
+    std::iota(c_current_cp->constraints_sequence.begin(), c_current_cp->constraints_sequence.end(), 0);
+
     for (const auto& cc : l_constraintCorrections)
     {
         if (!cc->isActive()) continue;
 
-        c_current_cp->constraints_sequence.resize(numConstraints);
-        std::iota(c_current_cp->constraints_sequence.begin(), c_current_cp->constraints_sequence.end(), 0);
-
         // some constraint corrections (e.g LinearSolverConstraintCorrection)
         // can change the order of the constraints, to optimize later computations
         cc->resetForUnbuiltResolution(c_current_cp->getF(), c_current_cp->constraints_sequence);
@@ -59,11 +69,9 @@ void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cPara
     for (unsigned int i = 0; i < numConstraints; i++)
         c_current_cp->cclist_elems[i].resize(nbCC, nullptr);
 
-    unsigned int nbObjects = 0;
     for (unsigned int c_id = 0; c_id < numConstraints;)
     {
         bool foundCC = false;
-        nbObjects++;
         const unsigned int l = c_current_cp->constraintsResolutions[c_id]->getNbLines();
 
         for (unsigned int j = 0; j < l_constraintCorrections.size(); j++)
@@ -93,6 +101,21 @@ void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cPara
 
 }
 
+void UnbuiltConstraintSolver::doPreApplyCorrection()
+{
+    // Save forces for hot-start in next timestep
+    keepContactForcesValue();
+}
+void UnbuiltConstraintSolver::doPreClearCorrection(const core::ConstraintParams* cparams)
+{
+    getConstraintInfo(cparams);
+}
+
+void UnbuiltConstraintSolver::doPostClearCorrection()
+{
+    computeInitialGuess();
+}
+
 void UnbuiltConstraintSolver::initializeConstraintProblems()
 {
     for (unsigned i=0; i< CP_BUFFER_SIZE; ++i)
@@ -102,5 +125,110 @@ void UnbuiltConstraintSolver::initializeConstraintProblems()
     current_cp = m_cpBuffer[0].get();
 }
 
+void UnbuiltConstraintSolver::getConstraintInfo(const core::ConstraintParams* cparams)
+{
+    if (d_initialGuess.getValue() && (m_numConstraints != 0))
+    {
+        SCOPED_TIMER("GetConstraintInfo");
+        m_constraintBlockInfo.clear();
+        m_constraintIds.clear();
+        simulation::mechanicalvisitor::MechanicalGetConstraintInfoVisitor(cparams, m_constraintBlockInfo, m_constraintIds).execute(getContext());
+    }
+}
+
+void UnbuiltConstraintSolver::computeInitialGuess()
+{
+    if (!d_initialGuess.getValue() || m_numConstraints == 0)
+        return;
+
+    SCOPED_TIMER("InitialGuess");
+
+    SReal* force = current_cp->getF();
+    const int numConstraints = current_cp->getDimension();
+
+    // First, zero all forces
+    for (int c = 0; c < numConstraints; c++)
+    {
+        force[c] = 0.0;
+    }
+
+    // Then restore forces from previous timestep for matching persistent IDs
+    for (const ConstraintBlockInfo& info : m_constraintBlockInfo)
+    {
+        if (!info.parent) continue;
+        if (!info.hasId) continue;
+
+        auto previt = m_previousConstraints.find(info.parent);
+        if (previt == m_previousConstraints.end()) continue;
+
+        const ConstraintBlockBuf& buf = previt->second;
+        const int c0 = info.const0;
+        const int nbl = (info.nbLines < buf.nbLines) ? info.nbLines : buf.nbLines;
+
+        for (int c = 0; c < info.nbGroups; ++c)
+        {
+            auto it = buf.persistentToConstraintIdMap.find(m_constraintIds[info.offsetId + c]);
+            if (it == buf.persistentToConstraintIdMap.end()) continue;
+
+            const int prevIndex = it->second;
+            if (prevIndex >= 0 && prevIndex + nbl <= static_cast<int>(m_previousForces.size()))
+            {
+                for (int l = 0; l < nbl; ++l)
+                {
+                    force[c0 + c * nbl + l] = m_previousForces[prevIndex + l];
+                }
+            }
+        }
+    }
+}
+
+void UnbuiltConstraintSolver::keepContactForcesValue()
+{
+    if (!d_initialGuess.getValue())
+        return;
+
+    SCOPED_TIMER("KeepForces");
+
+    const SReal* force = current_cp->getF();
+    const unsigned int numConstraints = current_cp->getDimension();
+
+    // Store current forces
+    m_previousForces.resize(numConstraints);
+    for (unsigned int c = 0; c < numConstraints; ++c)
+    {
+        m_previousForces[c] = force[c];
+    }
+
+    // Clear previous history (mark all as invalid)
+    for (auto& previousConstraint : m_previousConstraints)
+    {
+        ConstraintBlockBuf& buf = previousConstraint.second;
+        for (auto& it2 : buf.persistentToConstraintIdMap)
+        {
+            it2.second = -1;
+        }
+    }
+
+    // Fill info from current constraint IDs
+    for (const ConstraintBlockInfo& info : m_constraintBlockInfo)
+    {
+        if (!info.parent) continue;
+        if (!info.hasId) continue;
+
+        ConstraintBlockBuf& buf = m_previousConstraints[info.parent];
+        buf.nbLines = info.nbLines;
+
+        for (int c = 0; c < info.nbGroups; ++c)
+        {
+            buf.persistentToConstraintIdMap[m_constraintIds[info.offsetId + c]] = info.const0 + c * info.nbLines;
+        }
+    }
+
+    // Update constraint count for next iteration
+    m_numConstraints = numConstraints;
+}
+
+
+
 
 }

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

@@ -24,6 +24,7 @@
 #include <sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h>
 #include <sofa/core/behavior/ConstraintResolution.h>
 
+#include <sofa/simulation/mechanicalvisitor/MechanicalGetConstraintInfoVisitor.h>
 
 
 namespace sofa::component::constraint::lagrangian::solver
@@ -40,11 +41,50 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltConstraintSolver :
 {
 public:
     SOFA_CLASS(UnbuiltConstraintSolver, GenericConstraintSolver);
+
+protected:
+    UnbuiltConstraintSolver();
 
-
+public:
     virtual void initializeConstraintProblems() override;
 
+    Data<bool> d_initialGuess; ///< Activate constraint force history to improve convergence (hot start)
+
 protected:
     virtual void doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem, unsigned int numConstraints) override;
+
+    virtual void doPreApplyCorrection() override;
+    virtual void doPreClearCorrection(const core::ConstraintParams* cparams) override;
+    virtual void doPostClearCorrection() override;
+
+    ///<
+    // Hot-start mechanism types
+    typedef core::behavior::BaseLagrangianConstraint::ConstraintBlockInfo ConstraintBlockInfo;
+    typedef core::behavior::BaseLagrangianConstraint::PersistentID PersistentID;
+    typedef core::behavior::BaseLagrangianConstraint::VecConstraintBlockInfo VecConstraintBlockInfo;
+    typedef core::behavior::BaseLagrangianConstraint::VecPersistentID VecPersistentID;
+
+    class ConstraintBlockBuf
+    {
+    public:
+        std::map<PersistentID, int> persistentToConstraintIdMap;
+        int nbLines{0}; ///< how many dofs (i.e. lines in the matrix) are used by each constraint
+    };
+
+    /// Compute initial guess for constraint forces from previous timestep
+    void computeInitialGuess();
+
+    /// Save constraint forces for use as initial guess in next timestep
+    void keepContactForcesValue();
+
+    /// Get constraint info (block info and persistent IDs) for hot-start
+    void getConstraintInfo(const core::ConstraintParams* cparams);
+
+    // Hot-start data storage
+    std::map<core::behavior::BaseLagrangianConstraint*, ConstraintBlockBuf> m_previousConstraints;
+    type::vector<SReal> m_previousForces;
+    VecConstraintBlockInfo m_constraintBlockInfo;
+    VecPersistentID m_constraintIds;
+    unsigned int m_numConstraints{0}; ///< Number of constraints from current/previous timestep
 };
-}
+}

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

@@ -77,16 +77,20 @@ void UnbuiltGaussSeidelConstraintSolver::doSolve(GenericConstraintProblem * prob
     {
         if(!c_current_cp->constraintsResolutions[i])
         {
-            msg_warning() << "Bad size of constraintsResolutions in GenericConstraintSolver" ;
+            msg_warning() << "Bad size of constraintsResolutions" ;
             c_current_cp->setDimension(i);
             break;
         }
         c_current_cp->constraintsResolutions[i]->init(i, w, force);
         i += c_current_cp->constraintsResolutions[i]->getNbLines();
     }
-    memset(force, 0, c_current_cp->getDimension() * sizeof(SReal));	// Erase previous forces for the time being
-
 
+    // zero forces if cold-start
+    if(!d_initialGuess.getValue())
+    {
+        memset(force, 0, c_current_cp->getDimension() * sizeof(SReal));
+    }
+
     bool showGraphs = false;
     sofa::type::vector<SReal>* graph_residuals = nullptr;
     std::map < std::string, sofa::type::vector<SReal> > *graph_forces = nullptr, *graph_violations = nullptr;
@@ -298,4 +302,4 @@ void registerUnbuiltGaussSeidelConstraintSolver(sofa::core::ObjectFactory* facto
         .add< UnbuiltGaussSeidelConstraintSolver >());
 }
 
-}
+}