From 9a31e6dd27d60f9e041741fb82c125bcf013c2bd Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 13 Jul 2026 13:20:04 +0200 Subject: [PATCH 1/3] [constraint] Add the option to normalize the compliance block matrix before inversion --- .../constraint/ConstraintInsertion.h | 5 +++- .../constraint/InsertionResolution.h | 26 +++++++++++++++++-- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/ConstraintGeometry/constraint/ConstraintInsertion.h b/src/ConstraintGeometry/constraint/ConstraintInsertion.h index fca4e70..eda554e 100644 --- a/src/ConstraintGeometry/constraint/ConstraintInsertion.h +++ b/src/ConstraintGeometry/constraint/ConstraintInsertion.h @@ -13,12 +13,14 @@ class SOFA_CONSTRAINTGEOMETRY_API ConstraintInsertion : public TBaseConstraint d_frictionCoeff; Data > d_maxForce; Data > d_compliance; + Data d_scaleComplianceMatrix; core::objectmodel::SingleLink 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 inverting it, then rescale the inverse. Leaves the inverse mathematically unchanged, but prevents invertMatrix from rejecting a well-conditioned matrix whose entries are of a low magnitude")) , l_directions(initLink("directions", "link to the default direction")) {} void init() { // make sure we have a direction @@ -45,7 +47,8 @@ class SOFA_CONSTRAINTGEOMETRY_API ConstraintInsertion : public TBaseConstraint1) 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; diff --git a/src/ConstraintGeometry/constraint/InsertionResolution.h b/src/ConstraintGeometry/constraint/InsertionResolution.h index db55d24..ab0c63b 100644 --- a/src/ConstraintGeometry/constraint/InsertionResolution.h +++ b/src/ConstraintGeometry/constraint/InsertionResolution.h @@ -3,6 +3,7 @@ #include #include #include +#include namespace sofa { @@ -11,7 +12,8 @@ namespace constraintgeometry { class InsertionConstraintResolution : public sofa::core::behavior::ConstraintResolution { public: InsertionConstraintResolution(SReal frictionCoeff, double maxf1 = std::numeric_limits::max(),double maxf2 = std::numeric_limits::max(),double maxf3 = std::numeric_limits::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 +23,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 +37,25 @@ class InsertionConstraintResolution : public sofa::core::behavior::ConstraintRes } } - SOFA_UNUSED(sofa::type::invertMatrix(invW,temp)); + bool inverted = sofa::type::invertMatrix(invW, temp); + + // invertMatrix rejects on |det| < machine-eps returning zeros. The scale of the entries in + // this matrix block depend on the physical parameters in the scene. Depending on scene + // configuration, a matrix can be generated which may get rejected by invertMatrix due to + // its scale even though it may be well-conditioned. The matrix can be optionally scaled + // with the diagonal mean to avoid this. + 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); + if (inverted) invW *= invS; + } + } + + SOFA_UNUSED(inverted); } virtual void resolution(int line, double** w, double* d, double* force, double * /*dFree*/) @@ -70,6 +91,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; }; } From f0bd91d867ef6158b29cb71d74521cf2deeaed33 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 13 Jul 2026 13:39:22 +0200 Subject: [PATCH 2/3] [constraint] Add message for failure to invert --- .../constraint/ConstraintInsertion.h | 3 ++- .../constraint/InsertionResolution.h | 17 ++++++++++------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/ConstraintGeometry/constraint/ConstraintInsertion.h b/src/ConstraintGeometry/constraint/ConstraintInsertion.h index eda554e..1da03cc 100644 --- a/src/ConstraintGeometry/constraint/ConstraintInsertion.h +++ b/src/ConstraintGeometry/constraint/ConstraintInsertion.h @@ -20,7 +20,8 @@ class SOFA_CONSTRAINTGEOMETRY_API ConstraintInsertion : public TBaseConstraint #include #include +#include #include namespace sofa { @@ -39,11 +40,10 @@ class InsertionConstraintResolution : public sofa::core::behavior::ConstraintRes bool inverted = sofa::type::invertMatrix(invW, temp); - // invertMatrix rejects on |det| < machine-eps returning zeros. The scale of the entries in - // this matrix block depend on the physical parameters in the scene. Depending on scene - // configuration, a matrix can be generated which may get rejected by invertMatrix due to - // its scale even though it may be well-conditioned. The matrix can be optionally scaled - // with the diagonal mean to avoid this. + // 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; @@ -51,11 +51,14 @@ class InsertionConstraintResolution : public sofa::core::behavior::ConstraintRes { const double invS = 1.0 / s; inverted = sofa::type::invertMatrix(invW, temp * invS); - if (inverted) invW *= invS; + invW *= invS; } } - SOFA_UNUSED(inverted); + // 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 << "."; } virtual void resolution(int line, double** w, double* d, double* force, double * /*dFree*/) From 80d4d4295af37fca4704fd03f11b1bbe384fa9e4 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis <70031729+th-skam@users.noreply.github.com> Date: Mon, 13 Jul 2026 14:55:39 +0200 Subject: [PATCH 3/3] Update src/ConstraintGeometry/constraint/InsertionResolution.h Co-authored-by: erik pernod --- src/ConstraintGeometry/constraint/InsertionResolution.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ConstraintGeometry/constraint/InsertionResolution.h b/src/ConstraintGeometry/constraint/InsertionResolution.h index 3d45aaa..437013a 100644 --- a/src/ConstraintGeometry/constraint/InsertionResolution.h +++ b/src/ConstraintGeometry/constraint/InsertionResolution.h @@ -58,7 +58,7 @@ class InsertionConstraintResolution : public sofa::core::behavior::ConstraintRes // 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 << "."; + << "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*/)