diff --git a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp index dd3111f9609..8dc8ddc9536 100644 --- a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp +++ b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp @@ -47,6 +47,8 @@ #include #include + +#include using sofa::simulation::mechanicalvisitor::MechanicalVInitVisitor; @@ -102,7 +104,7 @@ void FreeMotionAnimationLoop::init() l_constraintSolver.set(this->getContext()->get(core::objectmodel::BaseContext::SearchDown)); if (!l_constraintSolver) { - if (const auto constraintSolver = sofa::core::objectmodel::New()) + if (const auto constraintSolver = sofa::core::objectmodel::New()) { getContext()->addObject(constraintSolver); constraintSolver->setName( this->getContext()->getNameHelper().resolveName(constraintSolver->getClassName(), {})); diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp index a2e36c175a1..2268ba89ace 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp @@ -28,6 +28,7 @@ namespace sofa::component::constraint::lagrangian::solver { + BuiltConstraintSolver::BuiltConstraintSolver() : d_multithreading(initData(&d_multithreading, false, "multithreading", "Build compliances concurrently")) {} diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h index c9416a574e0..9d4aa3bdf63 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h @@ -25,6 +25,7 @@ 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. @@ -34,7 +35,6 @@ namespace sofa::component::constraint::lagrangian::solver class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintSolver : public GenericConstraintSolver { - public: SOFA_CLASS(BuiltConstraintSolver, GenericConstraintSolver); Data d_multithreading; ///< Build compliances concurrently diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.cpp index 8d5ad099e5e..f5c0e879945 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.cpp @@ -74,6 +74,7 @@ void GenericConstraintProblem::solveTimed(SReal tol, int maxIt, SReal timeout) tolerance = tol; maxIterations = maxIt; + m_solver->doSolve(this, timeout); tolerance = tempTol; 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 ac7362c155c..13a6c019431 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 @@ -209,6 +209,7 @@ bool GenericConstraintSolver::buildSystem(const core::ConstraintParams *cParams, } + this->doBuildSystem(cParams, current_cp, numConstraints); return true; 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 3cd0993a7e1..36b16ea6ef0 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 @@ -60,12 +60,14 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : ConstraintProblem* getConstraintProblem() override; void lockConstraintProblem(sofa::core::objectmodel::BaseObject* from, ConstraintProblem* p1, ConstraintProblem* p2 = nullptr) override; + Data d_maxIt; ///< maximal number of iterations of iterative algorithm Data d_tolerance; ///< residual error threshold for termination of the Gauss-Seidel algorithm Data d_sor; ///< Successive Over Relaxation parameter (0-2) Data< SReal > d_regularizationTerm; ///< add regularization*Id to W when solving for constraints 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_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 diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.cpp index 540daa6e895..bbcb2b6f17d 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.cpp @@ -33,7 +33,6 @@ void NNCGConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal ti { SCOPED_TIMER_VARNAME(unbuiltGaussSeidelTimer, "NonsmoothNonlinearConjugateGradient"); - const int dimension = problem->getDimension(); if(!dimension) @@ -70,6 +69,7 @@ void NNCGConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal ti tempForces.resize(dimension); } + if(problem->scaleTolerance && !problem->allVerified) { tol *= dimension; @@ -77,11 +77,13 @@ void NNCGConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal ti for(int i=0; iconstraintsResolutions[i]) { msg_error() << "Bad size of constraintsResolutions in GenericConstraintSolver" ; break; } + problem->constraintsResolutions[i]->init(i, w, force); i += problem->constraintsResolutions[i]->getNbLines(); } @@ -91,6 +93,7 @@ void NNCGConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal ti { // perform one iteration of ProjectedGaussSeidel bool constraintsAreVerified = true; + std::copy_n(force, dimension, std::begin(problem->m_lam)); gaussSeidel_increment(false, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, problem->constraintsResolutions, tabErrors); @@ -118,6 +121,7 @@ void NNCGConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal ti error=0.0; + gaussSeidel_increment(true, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, problem->constraintsResolutions, tabErrors); @@ -158,6 +162,11 @@ void NNCGConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal ti problem->m_p[j] = beta*problem->m_p[j] -problem-> m_deltaF[j]; } } + //Stopping condition based on constraint evolution rate + if (problem->m_deltaF_new.norm() < tol) + { + break; + } } problem->result_output(this, force, error, iterCount, convergence); diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.cpp index efaeb6ed2d9..9979c53eb88 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.cpp @@ -29,11 +29,13 @@ namespace sofa::component::constraint::lagrangian::solver { + void ProjectedGaussSeidelConstraintSolver::doSolve( GenericConstraintProblem * problem ,SReal timeout) { SCOPED_TIMER_VARNAME(gaussSeidelTimer, "ConstraintsGaussSeidel"); + const int dimension = problem->getDimension(); if(!dimension) @@ -46,6 +48,7 @@ void ProjectedGaussSeidelConstraintSolver::doSolve( GenericConstraintProblem * p const SReal t0 = (SReal)sofa::helper::system::thread::CTime::getTime() ; const SReal timeScale = 1.0 / (SReal)sofa::helper::system::thread::CTime::getTicksPerSec(); + SReal *dfree = problem->getDfree(); SReal *force = problem->getF(); SReal **w = problem->getW(); @@ -99,17 +102,20 @@ void ProjectedGaussSeidelConstraintSolver::doSolve( GenericConstraintProblem * p int iterCount = 0; + for(int i=0; imaxIterations; i++) { iterCount ++; bool constraintsAreVerified = true; + if(problem->sor != 1.0) { std::copy_n(force, dimension, tempForces.begin()); } error=0.0; + gaussSeidel_increment(true, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, problem->constraintsResolutions, tabErrors); if(showGraphs) @@ -193,6 +199,7 @@ void ProjectedGaussSeidelConstraintSolver::doSolve( GenericConstraintProblem * p d_graphForces.endEdit(); } } + void ProjectedGaussSeidelConstraintSolver::gaussSeidel_increment(bool measureError, SReal *dfree, SReal *force, SReal **w, SReal tol, SReal *d, int dim, bool& constraintsAreVerified, SReal& error, std::vector& constraintCorrections, sofa::type::vector& tabErrors) const { for(int j=0; j Wdiag; /** UNBUILT **/ std::list constraints_sequence; /** UNBUILT **/ std::vector< ConstraintCorrections > cclist_elems; /** UNBUILT **/ 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 fe6ff57c60d..4c606b64792 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 @@ -28,6 +28,7 @@ namespace sofa::component::constraint::lagrangian::solver { + /** * \brief This component implements a generic way of preparing system for solvers that doesn't need * a build version of the constraint matrix. Any solver that are based on an unbuilt system should @@ -40,6 +41,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltConstraintSolver : public: SOFA_CLASS(UnbuiltConstraintSolver, GenericConstraintSolver); + virtual void initializeConstraintProblems() override; protected: 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 7f447554367..b9cb2e3c0b0 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 @@ -31,6 +31,7 @@ namespace sofa::component::constraint::lagrangian::solver { + void UnbuiltGaussSeidelConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal timeout) { UnbuiltConstraintProblem* c_current_cp = dynamic_cast(problem);