[Lagrangian] Add stopping condition in NNCG

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

@@ -47,6 +47,8 @@
 #include <sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h>
 
 #include <sofa/simulation/mechanicalvisitor/MechanicalVInitVisitor.h>
+
+#include "sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h"
 using sofa::simulation::mechanicalvisitor::MechanicalVInitVisitor;
 
 
@@ -102,7 +104,7 @@ void FreeMotionAnimationLoop::init()
         l_constraintSolver.set(this->getContext()->get<sofa::core::behavior::ConstraintSolver>(core::objectmodel::BaseContext::SearchDown));
         if (!l_constraintSolver)
         {
-            if (const auto constraintSolver = sofa::core::objectmodel::New<DefaultConstraintSolver>())
+            if (const auto constraintSolver = sofa::core::objectmodel::New<constraint::lagrangian::solver::ProjectedGaussSeidelConstraintSolver>())
             {
                 getContext()->addObject(constraintSolver);
                 constraintSolver->setName( this->getContext()->getNameHelper().resolveName(constraintSolver->getClassName(), {}));

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"))
 {}

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<bool> d_multithreading; ///< Build compliances concurrently

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

@@ -35,7 +35,7 @@ namespace sofa::component::constraint::lagrangian::solver
 {
 
 
-class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem
+class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API   ConstraintProblem
 {
 public:
     // The compliance matrix projected in the constraint space

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;

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;

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<int> d_maxIt; ///< maximal number of iterations of iterative algorithm
     Data<SReal> d_tolerance; ///< residual error threshold for termination of the Gauss-Seidel algorithm
     Data<SReal> d_sor; ///< Successive Over Relaxation parameter (0-2)
     Data< SReal > d_regularizationTerm; ///< add regularization*Id to W when solving for constraints
     Data<bool> d_scaleTolerance; ///< Scale the error tolerance with the number of constraints
     Data<bool> d_allVerified; ///< All constraints must be verified (each constraint's error < tolerance)
+
     Data<bool> d_computeGraphs; ///< Compute graphs of errors and forces during resolution
     Data<std::map < std::string, sofa::type::vector<SReal> > > d_graphErrors; ///< Sum of the constraints' errors at each iteration
     Data<std::map < std::string, sofa::type::vector<SReal> > > d_graphConstraints; ///< Graph of each constraint's error at the end of the resolution

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,18 +69,21 @@ void NNCGConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal ti
         tempForces.resize(dimension);
     }
 
+
     if(problem->scaleTolerance && !problem->allVerified)
     {
         tol *= dimension;
     }
 
     for(int i=0; i<dimension; )
     {
+
         if(!problem->constraintsResolutions[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);

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; i<problem->maxIterations; 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<core::behavior::ConstraintResolution*>& constraintCorrections, sofa::type::vector<SReal>& tabErrors) const
 {
     for(int j=0; j<dim; ) // increment of j realized at the end of the loop

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

@@ -30,6 +30,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ProjectedGaussSeidelConstr
 {
 public:
     SOFA_CLASS(ProjectedGaussSeidelConstraintSolver, BuiltConstraintSolver);
+
 protected:
     virtual void doSolve(GenericConstraintProblem * problem , SReal timeout = 0.0) override;
 

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

@@ -29,6 +29,7 @@
 namespace sofa::component::constraint::lagrangian::solver
 {
 
+
 /**
  *  \brief This class adds components needed for unbuilt solvers to the GenericConstraintProblem
  *  This needs to be used by unbuilt solvers.
@@ -42,7 +43,6 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltConstraintProblem :
     : GenericConstraintProblem(solver)
     {}
 
-
     linearalgebra::SparseMatrix<SReal> Wdiag; /** UNBUILT **/
     std::list<unsigned int> constraints_sequence; /** UNBUILT **/
     std::vector< ConstraintCorrections > cclist_elems; /** UNBUILT **/

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:

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<UnbuiltConstraintProblem*>(problem);