diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp index 2268ba89ace..62184e50cf5 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp @@ -25,12 +25,16 @@ #include #include #include +#include namespace sofa::component::constraint::lagrangian::solver { BuiltConstraintSolver::BuiltConstraintSolver() : d_multithreading(initData(&d_multithreading, false, "multithreading", "Build compliances concurrently")) +, d_useSVDForRegularization(initData(&d_useSVDForRegularization, false, "useSVDForRegularization", "Use SVD decomposiiton of the compliance matrix to project singular values smaller than regularization to the regularization term. Only works with built")) +, d_svdSingularValueNullSpaceCriteriaFactor(initData(&d_svdSingularValueNullSpaceCriteriaFactor, 0.01, "svdSingularValueNullSpaceCriteriaFactor", "Fraction of the highest singular value bellow which a singular value will be supposed to belong to the nullspace")) +, d_svdSingularVectorNullSpaceCriteriaFactor(initData(&d_svdSingularVectorNullSpaceCriteriaFactor, 0.001, "svdSingularVectorNullSpaceCriteriaFactor", "Absolute value bellow which a component of a normalized base vector will be considered null")) {} void BuiltConstraintSolver::init() @@ -42,6 +46,86 @@ void BuiltConstraintSolver::init() } } +void BuiltConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization) +{ + + if (d_useSVDForRegularization.getValue()) + { + if (regularization>std::numeric_limits::epsilon()) + { + auto * FullW = dynamic_cast * >(&W); + if (FullW == nullptr) + { + msg_error()<<"BuiltConstraintSolver expect a LPtrFullMatrix for W but didn't receive one. The constraint problem is wrong. Please fix the code or just deactivate SVD regularization."; + return; + } + const size_t problemSize = FullW->rowSize(); + + Eigen::Map> EigenW(FullW->ptr(),problemSize, problemSize) ; + Eigen::JacobiSVD svd( EigenW, Eigen::ComputeFullV | Eigen::ComputeFullU ); + + + + //Given the SVD, loop over all singular values, and those that are smaller than 1% of the highest one are considered to be the null space + std::vector nullSpaceIndicator(problemSize, false); + int nullSpaceBegin = -1; + for(size_t i=0; i d_svdSingularVectorNullSpaceCriteriaFactor.getValue(); + } + + if (f_printLog.getValue()) + { + std::stringstream msg ; + msg <<"Unregularized diagonal : "; + for(size_t i=0; i d_multithreading; ///< Build compliances concurrently + Data d_useSVDForRegularization; ///< Use SVD decomposiiton of the compliance matrix to project singular values smaller than regularization to the regularization term. Only works with built + Data d_svdSingularValueNullSpaceCriteriaFactor; ///< Fraction of the highest singular value bellow which a singular value will be supposed to belong to the nullspace + Data d_svdSingularVectorNullSpaceCriteriaFactor; ///< Absolute value bellow which a component of a normalized base vector will be considered null + BuiltConstraintSolver(); @@ -45,6 +49,8 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintSolver : pu protected: virtual void doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints) override; + virtual void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization) override; + private: diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h index 36b16ea6ef0..bfc87af5565 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h @@ -124,8 +124,8 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : */ virtual void doSolve( GenericConstraintProblem * problem, SReal timeout = 0.0) = 0; + virtual void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization); - static void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization); private: diff --git a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_svd_regularization_solvable.scn b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_svd_regularization_solvable.scn new file mode 100644 index 00000000000..059c065fca2 --- /dev/null +++ b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_svd_regularization_solvable.scn @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +