From 3af40561ca8410783c851bc307f81bb01dc156c1 Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Tue, 20 Jan 2026 13:32:46 +0900 Subject: [PATCH 1/5] fix resetForUnbuilt in UnbuiltGCSolver --- .../lagrangian/solver/UnbuiltConstraintSolver.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp index 551d03e6836..b12caccab5b 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp @@ -37,13 +37,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); From 57090596f7ff55340830dbc766832e43f35d7cb7 Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Thu, 15 Jan 2026 10:28:28 +0900 Subject: [PATCH 2/5] implement hot start (keeping forces between steps) --- .../solver/GenericConstraintSolver.cpp | 111 ++++++++++++++++++ .../solver/GenericConstraintSolver.h | 31 ++++- .../UnbuiltGaussSeidelConstraintSolver.cpp | 6 +- 3 files changed, 144 insertions(+), 4 deletions(-) diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp index 3d10afb5224..2f046c04137 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp @@ -70,6 +70,7 @@ GenericConstraintSolver::GenericConstraintSolver() , d_regularizationTerm(initData(&d_regularizationTerm, 0.0_sreal, "regularizationTerm", "Add regularization factor times the identity matrix to the compliance W when solving constraints")) , d_scaleTolerance(initData(&d_scaleTolerance, true, "scaleTolerance", "Scale the error tolerance with the number of constraints")) , d_allVerified(initData(&d_allVerified, false, "allVerified", "All constraints must be verified (each constraint's error < tolerance)")) + , d_initialGuess(initData(&d_initialGuess, true, "initialGuess", "Activate constraint force history to improve convergence (hot start)")) , d_computeGraphs(initData(&d_computeGraphs, false, "computeGraphs", "Compute graphs of errors and forces during resolution")) , d_graphErrors(initData(&d_graphErrors, "graphErrors", "Sum of the constraints' errors at each iteration")) , d_graphConstraints(initData(&d_graphConstraints, "graphConstraints", "Graph of each constraint's error at the end of the resolution")) @@ -186,9 +187,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 + getConstraintInfo(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 + computeInitialGuess(); + getConstraintViolation(cParams, ¤t_cp->dFree); { @@ -380,6 +387,9 @@ void GenericConstraintSolver::storeConstraintLambdas(const core::ConstraintParam bool GenericConstraintSolver::applyCorrection(const core::ConstraintParams *cParams, MultiVecId res1, MultiVecId res2) { + // Save forces for hot-start in next timestep + keepContactForcesValue(); + computeAndApplyMotionCorrection(cParams, res1, res2); storeConstraintLambdas(cParams); @@ -440,7 +450,108 @@ void GenericConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W, co } } +void GenericConstraintSolver::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 GenericConstraintSolver::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(m_previousForces.size())) + { + for (int l = 0; l < nbl; ++l) + { + force[c0 + c * nbl + l] = m_previousForces[prevIndex + l]; + } + } + } + } +} + +void GenericConstraintSolver::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; +} } //namespace sofa::component::constraint::lagrangian::solver diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h index fe70f01834e..2274041efae 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -67,6 +68,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : Data d_scaleTolerance; ///< Scale the error tolerance with the number of constraints Data d_allVerified; ///< All constraints must be verified (each constraint's error < tolerance) + Data d_initialGuess; ///< Activate constraint force history to improve convergence (hot start) Data d_computeGraphs; ///< Compute graphs of errors and forces during resolution Data > > d_graphErrors; ///< Sum of the constraints' errors at each iteration Data > > d_graphConstraints; ///< Graph of each constraint's error at the end of the resolution @@ -125,7 +127,34 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : virtual void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization); - + // 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 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 m_previousConstraints; + type::vector m_previousForces; + VecConstraintBlockInfo m_constraintBlockInfo; + VecPersistentID m_constraintIds; + unsigned int m_numConstraints{0}; ///< Number of constraints from current/previous timestep private: diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp index b9cb2e3c0b0..9631aac46f2 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp @@ -84,8 +84,8 @@ void UnbuiltGaussSeidelConstraintSolver::doSolve(GenericConstraintProblem * prob 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 - + // Note: force array is now initialized by GenericConstraintSolver::computeInitialGuess() + // for hot-start support. Do not zero forces here. bool showGraphs = false; sofa::type::vector* graph_residuals = nullptr; @@ -298,4 +298,4 @@ void registerUnbuiltGaussSeidelConstraintSolver(sofa::core::ObjectFactory* facto .add< UnbuiltGaussSeidelConstraintSolver >()); } -} \ No newline at end of file +} From 54e79e1d87931d76dea283bc00a0614a08d6a62d Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Wed, 21 Jan 2026 15:03:39 +0900 Subject: [PATCH 3/5] set all the hot start setup in unbuiltsolver directly --- .../solver/GenericConstraintSolver.cpp | 112 +-------------- .../solver/GenericConstraintSolver.h | 38 +----- .../solver/UnbuiltConstraintSolver.cpp | 129 ++++++++++++++++++ .../solver/UnbuiltConstraintSolver.h | 44 +++++- 4 files changed, 181 insertions(+), 142 deletions(-) diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp index 2f046c04137..cccd1dc8894 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp @@ -70,7 +70,6 @@ GenericConstraintSolver::GenericConstraintSolver() , d_regularizationTerm(initData(&d_regularizationTerm, 0.0_sreal, "regularizationTerm", "Add regularization factor times the identity matrix to the compliance W when solving constraints")) , d_scaleTolerance(initData(&d_scaleTolerance, true, "scaleTolerance", "Scale the error tolerance with the number of constraints")) , d_allVerified(initData(&d_allVerified, false, "allVerified", "All constraints must be verified (each constraint's error < tolerance)")) - , d_initialGuess(initData(&d_initialGuess, true, "initialGuess", "Activate constraint force history to improve convergence (hot start)")) , d_computeGraphs(initData(&d_computeGraphs, false, "computeGraphs", "Compute graphs of errors and forces during resolution")) , d_graphErrors(initData(&d_graphErrors, "graphErrors", "Sum of the constraints' errors at each iteration")) , d_graphConstraints(initData(&d_graphConstraints, "graphConstraints", "Graph of each constraint's error at the end of the resolution")) @@ -188,13 +187,13 @@ bool GenericConstraintSolver::buildSystem(const core::ConstraintParams *cParams, applyProjectiveConstraintOnConstraintMatrix(cParams); // Get constraint info for hot-start BEFORE clear() resets the constraint count check - getConstraintInfo(cParams); + 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 - computeInitialGuess(); + postClearCorrection(); getConstraintViolation(cParams, ¤t_cp->dFree); @@ -387,8 +386,7 @@ void GenericConstraintSolver::storeConstraintLambdas(const core::ConstraintParam bool GenericConstraintSolver::applyCorrection(const core::ConstraintParams *cParams, MultiVecId res1, MultiVecId res2) { - // Save forces for hot-start in next timestep - keepContactForcesValue(); + doPreApplyCorrection(); computeAndApplyMotionCorrection(cParams, res1, res2); storeConstraintLambdas(cParams); @@ -450,108 +448,4 @@ void GenericConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W, co } } -void GenericConstraintSolver::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 GenericConstraintSolver::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(m_previousForces.size())) - { - for (int l = 0; l < nbl; ++l) - { - force[c0 + c * nbl + l] = m_previousForces[prevIndex + l]; - } - } - } - } -} - -void GenericConstraintSolver::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; -} - - } //namespace sofa::component::constraint::lagrangian::solver diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h index 2274041efae..4622ab9af63 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h @@ -27,7 +27,6 @@ #include #include #include -#include #include #include @@ -68,7 +67,6 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : Data d_scaleTolerance; ///< Scale the error tolerance with the number of constraints Data d_allVerified; ///< All constraints must be verified (each constraint's error < tolerance) - Data d_initialGuess; ///< Activate constraint force history to improve convergence (hot start) Data d_computeGraphs; ///< Compute graphs of errors and forces during resolution Data > > d_graphErrors; ///< Sum of the constraints' errors at each iteration Data > > d_graphConstraints; ///< Graph of each constraint's error at the end of the resolution @@ -98,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(); } /***** * @@ -124,38 +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); - // 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 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 m_previousConstraints; - type::vector m_previousForces; - VecConstraintBlockInfo m_constraintBlockInfo; - VecPersistentID m_constraintIds; - unsigned int m_numConstraints{0}; ///< Number of constraints from current/previous timestep - private: sofa::type::vector filteredConstraintCorrections() const; diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp index b12caccab5b..188d2e57545 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp @@ -24,9 +24,18 @@ #include #include +#include + namespace sofa::component::constraint::lagrangian::solver { +UnbuiltConstraintSolver::UnbuiltConstraintSolver() +: GenericConstraintSolver() +, d_initialGuess(initData(&d_initialGuess, true, "initialGuess", "Activate constraint force history to improve convergence (hot start)")) +{ + +} + void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem, unsigned int numConstraints) { SOFA_UNUSED(cParams); @@ -94,6 +103,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) @@ -103,5 +127,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(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; +} + + + } diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.h index 4c606b64792..03229261afc 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.h @@ -24,6 +24,7 @@ #include #include +#include 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 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 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 m_previousConstraints; + type::vector m_previousForces; + VecConstraintBlockInfo m_constraintBlockInfo; + VecPersistentID m_constraintIds; + unsigned int m_numConstraints{0}; ///< Number of constraints from current/previous timestep }; -} \ No newline at end of file +} From 92d35ad04b795892353ae4fda4c43cb1af98633e Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Fri, 23 Jan 2026 10:24:46 +0900 Subject: [PATCH 4/5] set hotstart false by default and restore zero-ing forces if cold start --- .../lagrangian/solver/UnbuiltConstraintSolver.cpp | 6 ++---- .../solver/UnbuiltGaussSeidelConstraintSolver.cpp | 10 +++++++--- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp index 188d2e57545..eb48d9ca095 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp @@ -31,7 +31,7 @@ namespace sofa::component::constraint::lagrangian::solver UnbuiltConstraintSolver::UnbuiltConstraintSolver() : GenericConstraintSolver() -, d_initialGuess(initData(&d_initialGuess, true, "initialGuess", "Activate constraint force history to improve convergence (hot start)")) +, d_initialGuess(initData(&d_initialGuess, false, "initialGuess", "Activate constraint force history to possibly improve convergence (hot start).")) { } @@ -69,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++) @@ -153,7 +151,7 @@ void UnbuiltConstraintSolver::computeInitialGuess() { force[c] = 0.0; } - + // Then restore forces from previous timestep for matching persistent IDs for (const ConstraintBlockInfo& info : m_constraintBlockInfo) { diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp index 9631aac46f2..d780e05de06 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp +++ b/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(); } - // Note: force array is now initialized by GenericConstraintSolver::computeInitialGuess() - // for hot-start support. Do not zero forces here. + // zero forces if cold-start + if(!d_initialGuess.getValue()) + { + memset(force, 0, c_current_cp->getDimension() * sizeof(SReal)); + } + bool showGraphs = false; sofa::type::vector* graph_residuals = nullptr; std::map < std::string, sofa::type::vector > *graph_forces = nullptr, *graph_violations = nullptr; From f7190e9a725182489b87e72932c1b9d9a36c827f Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Thu, 29 Jan 2026 08:01:20 +0900 Subject: [PATCH 5/5] move initialguess condition --- .../lagrangian/solver/UnbuiltConstraintSolver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp index eb48d9ca095..5a000aa2f2f 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp @@ -103,6 +103,9 @@ void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cPara void UnbuiltConstraintSolver::doPreApplyCorrection() { + if (!d_initialGuess.getValue()) + return; + // Save forces for hot-start in next timestep keepContactForcesValue(); } @@ -184,9 +187,6 @@ void UnbuiltConstraintSolver::computeInitialGuess() void UnbuiltConstraintSolver::keepContactForcesValue() { - if (!d_initialGuess.getValue()) - return; - SCOPED_TIMER("KeepForces"); const SReal* force = current_cp->getF();