diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt b/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt
index 66634a1927f..e9395e32b19 100644
--- a/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt
+++ b/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt
@@ -11,6 +11,7 @@ set(HEADER_FILES
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.h
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintSolver.h
+ ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ImprovedJacobiConstraintSolver.h
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintSolver.h
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintSolver.h
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintSolver.h
@@ -29,6 +30,7 @@ set(SOURCE_FILES
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.cpp
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintSolver.cpp
+ ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ImprovedJacobiConstraintSolver.cpp
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintSolver.cpp
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintSolver.cpp
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintSolver.cpp
diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ImprovedJacobiConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ImprovedJacobiConstraintSolver.cpp
new file mode 100644
index 00000000000..27cfcd0b9e3
--- /dev/null
+++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ImprovedJacobiConstraintSolver.cpp
@@ -0,0 +1,197 @@
+/******************************************************************************
+* SOFA, Simulation Open-Framework Architecture *
+* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
+* *
+* This program is free software; you can redistribute it and/or modify it *
+* under the terms of the GNU Lesser General Public License as published by *
+* the Free Software Foundation; either version 2.1 of the License, or (at *
+* your option) any later version. *
+* *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with this program. If not, see . *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt) *
+* *
+* Contact information: contact@sofa-framework.org *
+******************************************************************************/
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace sofa::component::constraint::lagrangian::solver
+{
+
+ImprovedJacobiConstraintSolver::ImprovedJacobiConstraintSolver()
+ : BuiltConstraintSolver()
+ , d_useSpectralCorrection(initData(&d_useSpectralCorrection,false,"useSpectralCorrection","If set to true, the solution found after each iteration will be multiplied by spectralCorrectionFactor*2/spr(W), with spr() denoting the spectral radius."))
+ , d_spectralCorrectionFactor(initData(&d_spectralCorrectionFactor,1.0,"spectralCorrectionFactor","Factor used to modulate the spectral correction"))
+ , d_useConjugateResidue(initData(&d_useConjugateResidue,false,"useConjugateResidue","If set to true, the solution found after each iteration will be corrected along the solution direction using `\\lambda^{i+1} -= beta^{i} * (\\lambda^{i} - \\lambda^{i-1})` with beta following the formula beta^{i} = min(1, (i/maxIterations)^{conjugateResidueSpeedFactor}) "))
+ , d_conjugateResidueSpeedFactor(initData(&d_conjugateResidueSpeedFactor,10.0,"conjugateResidueSpeedFactor","Factor used to modulate the speed in which beta used in the conjugate residue part reaches 1.0. The higher the value, the slower the reach. "))
+{
+
+}
+
+
+void ImprovedJacobiConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal timeout)
+{
+ SCOPED_TIMER_VARNAME(gaussSeidelTimer, "ImprovedJacobiConstraintSolver");
+
+
+ const int dimension = problem->getDimension();
+
+ if(!dimension)
+ {
+ problem->currentError = 0.0;
+ problem->currentIterations = 0;
+ return;
+ }
+
+ SReal *dfree = problem->getDfree();
+ SReal *force = problem->getF();
+ SReal **w = problem->getW();
+ SReal tol = problem->tolerance;
+ SReal *d = problem->_d.ptr();
+
+ std::copy_n(dfree, dimension, d);
+
+ for(unsigned i=0; i< dimension; ++i)
+ {
+ force[i] = 0;
+ }
+
+ std::vector lastF;
+ lastF.resize(problem->getDimension(), 0.0);
+
+ std::vector deltaF;
+ deltaF.resize(problem->getDimension(), 0.0);
+
+ std::vector correctedD;
+ correctedD.resize(problem->getDimension(), 0.0);
+
+
+ SReal error=0.0;
+ bool convergence = false;
+ if(problem->scaleTolerance && !problem->allVerified)
+ {
+ tol *= dimension;
+ }
+
+ 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();
+ }
+
+ sofa::type::vector tabErrors(dimension);
+
+ int iterCount = 0;
+
+ SReal rho = 1.0;
+
+ if (d_useSpectralCorrection.getValue())
+ {
+ Eigen::Map> EigenW(w[0],dimension, dimension) ;
+ SReal eigenRadius = 0;
+ for(auto s : EigenW.eigenvalues())
+ {
+ eigenRadius=std::max(eigenRadius,norm(s));
+ }
+ rho = d_spectralCorrectionFactor.getValue()*std::min(1.0, 0.9 * 2/eigenRadius);
+ }
+
+ for(int i=0; imaxIterations; i++)
+ {
+ iterCount ++;
+ bool constraintsAreVerified = true;
+
+ error=0.0;
+ SReal beta = d_useConjugateResidue.getValue() * std::min(1.0, pow( ((float)i)/problem->maxIterations,d_conjugateResidueSpeedFactor.getValue()));
+
+ for(int j=0; jconstraintsResolutions[j]->getNbLines();
+
+ for(unsigned l=j; lconstraintsResolutions[j]->getNbLines();
+
+ problem->constraintsResolutions[j]->resolution(j,w,correctedD.data(), force, dfree);
+ for(unsigned l=j; lallVerified)
+ {
+ if (constraintsAreVerified)
+ {
+ convergence = true;
+ break;
+ }
+ }
+ else if(error < tol && i > 0) // do not stop at the first iteration (that is used for initial guess computation)
+ {
+ convergence = true;
+ break;
+ }
+ }
+
+ sofa::helper::AdvancedTimer::valSet("GS iterations", problem->currentIterations);
+
+ problem->result_output(this, force, error, iterCount, convergence);
+
+}
+
+
+
+void registerImprovedJacobiConstraintSolver(sofa::core::ObjectFactory* factory)
+{
+ factory->registerObjects(core::ObjectRegistrationData("A Constraint Solver using the Linear Complementarity Problem formulation to solve Constraint based components using a Projected Jacobi iterative method")
+ .add< ImprovedJacobiConstraintSolver >());
+}
+
+
+}
\ No newline at end of file
diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ImprovedJacobiConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ImprovedJacobiConstraintSolver.h
new file mode 100644
index 00000000000..e96fe874d62
--- /dev/null
+++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ImprovedJacobiConstraintSolver.h
@@ -0,0 +1,50 @@
+/******************************************************************************
+* SOFA, Simulation Open-Framework Architecture *
+* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
+* *
+* This program is free software; you can redistribute it and/or modify it *
+* under the terms of the GNU Lesser General Public License as published by *
+* the Free Software Foundation; either version 2.1 of the License, or (at *
+* your option) any later version. *
+* *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with this program. If not, see . *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt) *
+* *
+* Contact information: contact@sofa-framework.org *
+******************************************************************************/
+#pragma once
+
+#include
+#include
+
+namespace sofa::component::constraint::lagrangian::solver
+{
+class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ImprovedJacobiConstraintSolver : public BuiltConstraintSolver
+{
+public:
+ SOFA_CLASS(ImprovedJacobiConstraintSolver, BuiltConstraintSolver);
+
+ Data d_useSpectralCorrection;
+ Data d_spectralCorrectionFactor;
+ Data d_useConjugateResidue;
+ Data d_conjugateResidueSpeedFactor;
+
+ ImprovedJacobiConstraintSolver();
+
+protected:
+ /**
+ * Based on paper
+ * Francu, Mihai & Moldoveanu, Florica. An Improved Jacobi Solver for Particle Simulation.
+ * VRPHYS 2014
+ **/
+ virtual void doSolve(GenericConstraintProblem * problem , SReal timeout = 0.0) override;
+
+};
+}
\ No newline at end of file
diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/init.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/init.cpp
index 59d9dba7435..c3cbd2d6ab0 100644
--- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/init.cpp
+++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/init.cpp
@@ -30,6 +30,7 @@ extern void registerNNCGConstraintSolver(sofa::core::ObjectFactory* factory);
extern void registerProjectedGaussSeidelConstraintSolver(sofa::core::ObjectFactory* factory);
extern void registerUnbuiltGaussSeidelConstraintSolver(sofa::core::ObjectFactory* factory);
extern void registerLCPConstraintSolver(sofa::core::ObjectFactory* factory);
+extern void registerImprovedJacobiConstraintSolver(sofa::core::ObjectFactory* factory);
extern "C" {
SOFA_EXPORT_DYNAMIC_LIBRARY void initExternalModule();
@@ -59,6 +60,7 @@ void registerObjects(sofa::core::ObjectFactory* factory)
registerProjectedGaussSeidelConstraintSolver(factory);
registerUnbuiltGaussSeidelConstraintSolver(factory);
registerLCPConstraintSolver(factory);
+ registerImprovedJacobiConstraintSolver(factory);
}
void init()
diff --git a/examples/.scene-tests b/examples/.scene-tests
index d2e9358ca1f..5bde8c58dae 100644
--- a/examples/.scene-tests
+++ b/examples/.scene-tests
@@ -37,3 +37,6 @@ ignore "Component/Controller/MechanicalStateController.scn"
# To be removed when scenes are fixed.
ignore "Benchmark/TopologicalChanges/ProjectToPlaneConstraint_RemovingMeshTest.scn"
ignore "Benchmark/TopologicalChanges/FixedPlaneConstraint_RemovingMeshTest.scn"
+
+iterations "/Component/Constraint/Lagrangian/Contact_ImprovedJacobi.scn" "50"
+timeout "Component/AnimationLoop/FreeMotionAnimationLoop.scn" "180"
\ No newline at end of file
diff --git a/examples/Component/Constraint/Lagrangian/Contact_ImprovedJacobi.scn b/examples/Component/Constraint/Lagrangian/Contact_ImprovedJacobi.scn
new file mode 100644
index 00000000000..bb990235229
--- /dev/null
+++ b/examples/Component/Constraint/Lagrangian/Contact_ImprovedJacobi.scn
@@ -0,0 +1,131 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+