[LinearSolver] Fix applyConstraintForce broken in sofa-framework#5648

Author: alxbilger

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

@@ -204,6 +204,9 @@ void ProjectedGaussSeidelConstraintSolver::gaussSeidel_increment(bool measureErr
 {
     for(int j=0; j<dim; ) // increment of j realized at the end of the loop
     {
+        assert(j < constraintCorrections.size());
+        assert(constraintCorrections[j] != nullptr);
+
         //1. nbLines provide the dimension of the constraint
         const unsigned int nb = constraintCorrections[j]->getNbLines();
 
@@ -288,4 +291,4 @@ void registerProjectedGaussSeidelConstraintSolver(sofa::core::ObjectFactory* fac
 }
 
 
-}
+}

Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/MatrixLinearSolver.inl

@@ -492,14 +492,15 @@ template<class Matrix, class Vector>
 void MatrixLinearSolver<Matrix,Vector>::applyConstraintForce(const sofa::core::ConstraintParams* cparams, sofa::core::MultiVecDerivId dx, const linearalgebra::BaseVector* f)
 {
     auto* systemMatrix = l_linearSystem->getSystemMatrix();
+    auto* lhsVector = l_linearSystem->getSolutionVector();
     auto* rhsVector = l_linearSystem->getRHSVector();
 
     rhsVector->clear();
     rhsVector->resize(systemMatrix->colSize());
     /// rhs = J^t * f
     internalData.projectForceInConstraintSpace(rhsVector, f);
     /// lhs = M^-1 * rhs
-    this->solve(*systemMatrix, *rhsVector, *rhsVector);
+    this->solve(*systemMatrix, *lhsVector, *rhsVector);
 
     l_linearSystem->dispatchSystemSolution(dx);
     l_linearSystem->dispatchSystemRHS(cparams->lambda());

examples/Component/Constraint/Lagrangian/InextensiblePendulum.scn

@@ -3,21 +3,23 @@
 <!-- A pendulum made of a string of particles connected by distance constraints -->
 <!-- Inspired by the CompliantPendulum.scn from the Compliant plugin -->
 <Node name="Root" gravity="0 -10 0" time="0" animate="0"  dt="0.01">
-    <RequiredPlugin name="Sofa.Component.AnimationLoop"/> <!-- Needed to use components [FreeMotionAnimationLoop] -->
-    <RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Correction"/> <!-- Needed to use components [GenericConstraintCorrection] -->
-    <RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Model"/> <!-- Needed to use components [UniformLagrangianConstraint] -->
-    <RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Solver"/> <!-- Needed to use components [GenericConstraintSolver] -->
-    <RequiredPlugin name="Sofa.Component.Constraint.Projective"/> <!-- Needed to use components [FixedProjectiveConstraint] -->
-    <RequiredPlugin name="Sofa.Component.Engine.Select"/> <!-- Needed to use components [PointsFromIndices] -->
-    <RequiredPlugin name="Sofa.Component.Engine.Transform"/> <!-- Needed to use components [TransformEngine] -->
-    <RequiredPlugin name="Sofa.Component.IO.Mesh"/> <!-- Needed to use components [StringMeshCreator] -->
-    <RequiredPlugin name="Sofa.Component.LinearSolver.Direct"/> <!-- Needed to use components [EigenSimplicialLLT] -->
-    <RequiredPlugin name="Sofa.Component.Mapping.NonLinear"/> <!-- Needed to use components [DistanceMapping] -->
-    <RequiredPlugin name="Sofa.Component.Mass"/> <!-- Needed to use components [DiagonalMass] -->
-    <RequiredPlugin name="Sofa.Component.ODESolver.Backward"/> <!-- Needed to use components [EulerImplicitSolver] -->
-    <RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
-    <RequiredPlugin name="Sofa.Component.Topology.Container.Dynamic"/> <!-- Needed to use components [EdgeSetGeometryAlgorithms EdgeSetTopologyContainer] -->
-    <RequiredPlugin name="Sofa.Component.Visual"/> <!-- Needed to use components [TrailRenderer VisualStyle] -->
+    <Node name="plugins">
+        <RequiredPlugin name="Sofa.Component.AnimationLoop"/> <!-- Needed to use components [FreeMotionAnimationLoop] -->
+        <RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Correction"/> <!-- Needed to use components [GenericConstraintCorrection] -->
+        <RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Model"/> <!-- Needed to use components [UniformLagrangianConstraint] -->
+        <RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Solver"/> <!-- Needed to use components [GenericConstraintSolver] -->
+        <RequiredPlugin name="Sofa.Component.Constraint.Projective"/> <!-- Needed to use components [FixedProjectiveConstraint] -->
+        <RequiredPlugin name="Sofa.Component.Engine.Select"/> <!-- Needed to use components [PointsFromIndices] -->
+        <RequiredPlugin name="Sofa.Component.Engine.Transform"/> <!-- Needed to use components [TransformEngine] -->
+        <RequiredPlugin name="Sofa.Component.IO.Mesh"/> <!-- Needed to use components [StringMeshCreator] -->
+        <RequiredPlugin name="Sofa.Component.LinearSolver.Direct"/> <!-- Needed to use components [EigenSimplicialLLT] -->
+        <RequiredPlugin name="Sofa.Component.Mapping.NonLinear"/> <!-- Needed to use components [DistanceMapping] -->
+        <RequiredPlugin name="Sofa.Component.Mass"/> <!-- Needed to use components [DiagonalMass] -->
+        <RequiredPlugin name="Sofa.Component.ODESolver.Backward"/> <!-- Needed to use components [EulerImplicitSolver] -->
+        <RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
+        <RequiredPlugin name="Sofa.Component.Topology.Container.Dynamic"/> <!-- Needed to use components [EdgeSetGeometryAlgorithms EdgeSetTopologyContainer] -->
+        <RequiredPlugin name="Sofa.Component.Visual"/> <!-- Needed to use components [TrailRenderer VisualStyle] -->
+    </Node>
 
     <DefaultVisualManagerLoop/>
     <VisualStyle displayFlags="showVisualModels showBehaviorModels showMappings showForceFields" />