diff --git a/src/ConstraintGeometry/constraint/ConstraintInsertion.h b/src/ConstraintGeometry/constraint/ConstraintInsertion.h index fca4e70..1da03cc 100644 --- a/src/ConstraintGeometry/constraint/ConstraintInsertion.h +++ b/src/ConstraintGeometry/constraint/ConstraintInsertion.h @@ -13,12 +13,15 @@ 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 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 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..437013a 100644 --- a/src/ConstraintGeometry/constraint/InsertionResolution.h +++ b/src/ConstraintGeometry/constraint/InsertionResolution.h @@ -3,6 +3,8 @@ #include #include #include +#include +#include namespace sofa { @@ -11,7 +13,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 +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; }; }