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..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 @@ -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, ¤t_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 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..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 @@ -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 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 551d03e6836..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 @@ -24,9 +24,18 @@ #include #include +#include + 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,24 @@ void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cPara } +void UnbuiltConstraintSolver::doPreApplyCorrection() +{ + if (!d_initialGuess.getValue()) + return; + + // 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 +128,107 @@ 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() +{ + 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 +} 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..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(); } - 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* graph_residuals = nullptr; std::map < std::string, sofa::type::vector > *graph_forces = nullptr, *graph_violations = nullptr; @@ -298,4 +302,4 @@ void registerUnbuiltGaussSeidelConstraintSolver(sofa::core::ObjectFactory* facto .add< UnbuiltGaussSeidelConstraintSolver >()); } -} \ No newline at end of file +}