[constraint] Add the option to normalize the compliance block matrix before inversion

src/ConstraintGeometry/constraint/ConstraintInsertion.h

@@ -13,12 +13,15 @@ class SOFA_CONSTRAINTGEOMETRY_API ConstraintInsertion : public TBaseConstraint<c
     Data<SReal> d_frictionCoeff;
     Data<sofa::type::vector<double> > d_maxForce;
     Data<sofa::type::vector<double> > d_compliance;
+    Data<bool> d_scaleComplianceMatrix;
     core::objectmodel::SingleLink<ConstraintInsertion,ConstraintDirection, BaseLink::FLAG_STRONGLINK|BaseLink::FLAG_STOREPATH> l_directions;
 
     ConstraintInsertion()
     : d_frictionCoeff(initData(&d_frictionCoeff, 0.0, "frictionCoeff" , "static friction coefficient (should be less than 1)"))
     , d_maxForce(initData(&d_maxForce, "maxForce", "Max force"))
     , d_compliance(initData(&d_compliance, "compliance", "Max force"))
+    , d_scaleComplianceMatrix(initData(&d_scaleComplianceMatrix, false, "scaleComplianceMatrix"
+                , "If true, normalize the compliance matrix by the mean of its diagonal before inversion"))
     , l_directions(initLink("directions", "link to the default direction")) {}
 
     void init() { // make sure we have a direction
@@ -45,7 +48,8 @@ class SOFA_CONSTRAINTGEOMETRY_API ConstraintInsertion : public TBaseConstraint<c
         if (d_compliance.getValue().size()>1) comp[1]=d_compliance.getValue()[1];
         if (d_compliance.getValue().size()>2) comp[2]=d_compliance.getValue()[2];
 
-        return new InsertionConstraintResolution(d_frictionCoeff.getValue(), maxf[0],maxf[1],maxf[2],comp[0],comp[1],comp[2]);
+        return new InsertionConstraintResolution(d_frictionCoeff.getValue(), maxf[0],maxf[1],maxf[2],comp[0],comp[1],comp[2],
+                                                 d_scaleComplianceMatrix.getValue());
 
         std::cerr << "Error the size of the constraint is not correct in ConstraintInsertion size=" << cst->size() << std::endl;
         return NULL;

src/ConstraintGeometry/constraint/InsertionResolution.h

@@ -3,6 +3,8 @@
 #include <ConstraintGeometry/BaseConstraint.h>
 #include <ConstraintGeometry/directions/BindDirection.h>
 #include <sofa/core/behavior/ConstraintResolution.h>
+#include <sofa/helper/logging/Messaging.h>
+#include <cmath>
 
 namespace sofa {
 
@@ -11,7 +13,8 @@ namespace constraintgeometry {
 class InsertionConstraintResolution : public sofa::core::behavior::ConstraintResolution {
 public:
     InsertionConstraintResolution(SReal frictionCoeff, double maxf1 = std::numeric_limits<double>::max(),double maxf2 = std::numeric_limits<double>::max(),double maxf3 = std::numeric_limits<double>::max(),
-                                   double comp1 = 0.0,double comp2 = 0.0,double comp3 = 0.0) : sofa::core::behavior::ConstraintResolution(3) {
+                                   double comp1 = 0.0,double comp2 = 0.0,double comp3 = 0.0,
+                                   bool scaleComplianceMatrix = false) : sofa::core::behavior::ConstraintResolution(3) {
         m_maxForce[0] = maxf1;
         m_maxForce[1] = maxf2;
         m_maxForce[2] = maxf3;
@@ -21,6 +24,7 @@ class InsertionConstraintResolution : public sofa::core::behavior::ConstraintRes
         m_compliance[2] = comp3;
 
         m_frictionCoeff = frictionCoeff;
+        m_scaleComplianceMatrix = scaleComplianceMatrix;
     }
 
     virtual void init(int line, double** w, double * /*force*/)
@@ -34,7 +38,27 @@ class InsertionConstraintResolution : public sofa::core::behavior::ConstraintRes
             }
         }
 
-        SOFA_UNUSED(sofa::type::invertMatrix(invW,temp));
+        bool inverted = sofa::type::invertMatrix(invW, temp);
+
+        // invertMatrix rejects the inversion if |det| < machine-eps, leaves invW empty and returns 
+        // false. However, |det| depends on the problem scale and the physical parameters. 
+        // It can happen that a matrix is well-conditioned but with a small |det|. In that case, 
+        // proceed by scaling the matrix before inversion, then scale it back up afterwards.
+        if (!inverted && m_scaleComplianceMatrix)
+        {
+            const double s = (std::abs(temp[0][0]) + std::abs(temp[1][1]) + std::abs(temp[2][2])) / 3.0;
+            if (s > 0.0)
+            {
+                const double invS = 1.0 / s;
+                inverted = sofa::type::invertMatrix(invW, temp * invS);
+                invW *= invS;
+            }
+        }
+
+        // Matrix is rank-deficient. Nothing could be done.
+        if (!inverted)
+            msg_warning("InsertionConstraintResolution")
+                << "Failed to invert the compliance matrix of the constraint at line " << line << ". If not already done, try to scale compliance matrix using Data scaleComplianceMatrix";
     }
 
     virtual void resolution(int line, double** w, double* d, double* force, double * /*dFree*/)
@@ -70,6 +94,7 @@ class InsertionConstraintResolution : public sofa::core::behavior::ConstraintRes
     double m_compliance[3];
     sofa::type::Mat<3,3,double> invW;
     SReal m_frictionCoeff;
+    bool m_scaleComplianceMatrix;
 };
 
 }