diff --git a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp index d1571e8182c..dd3111f9609 100644 --- a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp +++ b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp @@ -44,10 +44,12 @@ #include #include #include +#include #include using sofa::simulation::mechanicalvisitor::MechanicalVInitVisitor; + #include using sofa::simulation::mechanicalvisitor::MechanicalBeginIntegrationVisitor; @@ -67,7 +69,7 @@ using namespace core::behavior; using namespace sofa::simulation; using sofa::helper::ScopedAdvancedTimer; -using DefaultConstraintSolver = sofa::component::constraint::lagrangian::solver::GenericConstraintSolver; +using DefaultConstraintSolver = sofa::component::constraint::lagrangian::solver::ProjectedGaussSeidelConstraintSolver; FreeMotionAnimationLoop::FreeMotionAnimationLoop() : d_solveVelocityConstraintFirst(initData(&d_solveVelocityConstraintFirst , false, "solveVelocityConstraintFirst", "solve separately velocity constraint violations before position constraint violations")) diff --git a/Sofa/Component/Constraint/Lagrangian/Model/tests/scenes_test/BilateralInteractionConstraint.scn b/Sofa/Component/Constraint/Lagrangian/Model/tests/scenes_test/BilateralInteractionConstraint.scn index b447ce22419..21bd26bf1af 100644 --- a/Sofa/Component/Constraint/Lagrangian/Model/tests/scenes_test/BilateralInteractionConstraint.scn +++ b/Sofa/Component/Constraint/Lagrangian/Model/tests/scenes_test/BilateralInteractionConstraint.scn @@ -2,7 +2,7 @@ - + diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt b/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt index 0cf052b1d98..66634a1927f 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt +++ b/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt @@ -8,7 +8,13 @@ set(HEADER_FILES ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/init.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ConstraintSolverImpl.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintProblem.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintSolver.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintSolver.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintSolver.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintSolver.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltGaussSeidelConstraintSolver.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/LCPConstraintSolver.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/visitors/ConstraintStoreLambdaVisitor.h @@ -20,7 +26,13 @@ set(SOURCE_FILES ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/init.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ConstraintSolverImpl.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintProblem.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintSolver.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintSolver.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintSolver.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintSolver.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltGaussSeidelConstraintSolver.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/LCPConstraintSolver.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/visitors/ConstraintStoreLambdaVisitor.cpp 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 new file mode 100644 index 00000000000..a2e36c175a1 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp @@ -0,0 +1,115 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#include + +#include +#include +#include + +namespace sofa::component::constraint::lagrangian::solver +{ +BuiltConstraintSolver::BuiltConstraintSolver() +: d_multithreading(initData(&d_multithreading, false, "multithreading", "Build compliances concurrently")) +{} + +void BuiltConstraintSolver::init() +{ + Inherit1::init(); + if(d_multithreading.getValue()) + { + simulation::MainTaskSchedulerFactory::createInRegistry()->init(); + } +} + +void BuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints) +{ + SOFA_UNUSED(numConstraints); + SCOPED_TIMER_VARNAME(getComplianceTimer, "Get Compliance"); + dmsg_info() <<" computeCompliance in " << l_constraintCorrections.size()<< " constraintCorrections" ; + + const bool multithreading = d_multithreading.getValue(); + + const simulation::ForEachExecutionPolicy execution = multithreading ? + simulation::ForEachExecutionPolicy::PARALLEL : + simulation::ForEachExecutionPolicy::SEQUENTIAL; + + simulation::TaskScheduler* taskScheduler = simulation::MainTaskSchedulerFactory::createInRegistry(); + assert(taskScheduler); + + //Used to prevent simultaneous accesses to the main compliance matrix + std::mutex mutex; + + //Visits all constraint corrections to compute the compliance matrix projected + //in the constraint space. + simulation::forEachRange(execution, *taskScheduler, l_constraintCorrections.begin(), l_constraintCorrections.end(), + [&cParams, this, &multithreading, &mutex, problem](const auto& range) + { + ComplianceWrapper compliance(problem->W, multithreading); + + for (auto it = range.start; it != range.end; ++it) + { + core::behavior::BaseConstraintCorrection* cc = *it; + if (cc->isActive()) + { + cc->addComplianceInConstraintSpace(cParams, &compliance.matrix()); + } + } + + std::lock_guard guard(mutex); + compliance.assembleMatrix(); + }); + + addRegularization(problem->W, d_regularizationTerm.getValue()); + dmsg_info() << " computeCompliance_done " ; +} + + +BuiltConstraintSolver::ComplianceWrapper::ComplianceMatrixType& BuiltConstraintSolver::ComplianceWrapper::matrix() +{ + if (m_isMultiThreaded) + { + if (!m_threadMatrix) + { + m_threadMatrix = std::make_unique(); + m_threadMatrix->resize(m_complianceMatrix.rowSize(), m_complianceMatrix.colSize()); + } + return *m_threadMatrix; + } + return m_complianceMatrix; +} + +void BuiltConstraintSolver::ComplianceWrapper::assembleMatrix() const +{ + if (m_threadMatrix) + { + for (linearalgebra::BaseMatrix::Index j = 0; j < m_threadMatrix->rowSize(); ++j) + { + for (linearalgebra::BaseMatrix::Index l = 0; l < m_threadMatrix->colSize(); ++l) + { + m_complianceMatrix.add(j, l, m_threadMatrix->element(j,l)); + } + } + } +} + +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h new file mode 100644 index 00000000000..c9416a574e0 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h @@ -0,0 +1,68 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include + +namespace sofa::component::constraint::lagrangian::solver +{ +/** + * \brief This component implements a generic way of building system for solvers that use a built + * version of the constraint matrix. Any solver that uses a build matrix should inherit from this. + * This component is purely virtual because doSolve is not defined and needs to be defined in the + * inherited class + */ +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintSolver : public GenericConstraintSolver +{ + + +public: + SOFA_CLASS(BuiltConstraintSolver, GenericConstraintSolver); + Data d_multithreading; ///< Build compliances concurrently + + BuiltConstraintSolver(); + + virtual void init() override; + +protected: + virtual void doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints) override; + +private: + + struct ComplianceWrapper + { + using ComplianceMatrixType = sofa::linearalgebra::LPtrFullMatrix; + + ComplianceWrapper(ComplianceMatrixType& complianceMatrix, bool isMultiThreaded) + : m_isMultiThreaded(isMultiThreaded), m_complianceMatrix(complianceMatrix) {} + + ComplianceMatrixType& matrix(); + + void assembleMatrix() const; + + private: + bool m_isMultiThreaded { false }; + ComplianceMatrixType& m_complianceMatrix; + std::unique_ptr m_threadMatrix; + }; +}; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.cpp index 2e9105b45b9..3c2216f1165 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.cpp @@ -51,11 +51,11 @@ void ConstraintProblem::clear(int nbConstraints) dFree.resize(nbConstraints); f.resize(nbConstraints); - static std::atomic counter = 0; + static std::atomic counter = 0; problemId = counter.fetch_add(1, std::memory_order_relaxed); } -unsigned int ConstraintProblem::getProblemId() +unsigned ConstraintProblem::getProblemId() const { return problemId; } diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h index 7eac329bc70..f1cebf5b448 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h @@ -59,6 +59,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem // Returns the number of scalar constraints, or equivalently the number of Lagrange multipliers int getDimension() const { return dimension; } + void setDimension(int dim) { dimension = dim; } SReal** getW() { return W.lptr(); } SReal* getDfree() { return dFree.ptr(); } @@ -66,11 +67,11 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem virtual void solveTimed(SReal tolerance, int maxIt, SReal timeout) = 0; - unsigned int getProblemId(); + unsigned getProblemId() const; protected: int dimension; - unsigned int problemId; + unsigned problemId; }; @@ -93,6 +94,10 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintSolverImpl : pub void removeConstraintCorrection(core::behavior::BaseConstraintCorrection *s) override; + MultiLink< ConstraintSolverImpl, + core::behavior::BaseConstraintCorrection, + BaseLink::FLAG_STOREPATH> l_constraintCorrections; + protected: void postBuildSystem(const core::ConstraintParams* cParams) override; @@ -100,9 +105,6 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintSolverImpl : pub void clearConstraintCorrections(); - MultiLink< ConstraintSolverImpl, - core::behavior::BaseConstraintCorrection, - BaseLink::FLAG_STOREPATH> l_constraintCorrections; /// Calls the method resetConstraint on all the mechanical states and BaseConstraintSet /// In the case of a MechanicalObject, it clears the constraint jacobian matrix diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.cpp index 0a5e6a281f9..8d5ad099e5e 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.cpp @@ -74,644 +74,15 @@ void GenericConstraintProblem::solveTimed(SReal tol, int maxIt, SReal timeout) tolerance = tol; maxIterations = maxIt; - gaussSeidel(timeout); + m_solver->doSolve(this, timeout); tolerance = tempTol; maxIterations = tempMaxIt; } -// Debug is only available when called directly by the solver (not in haptic thread) -void GenericConstraintProblem::gaussSeidel(SReal timeout, GenericConstraintSolver* solver) +void GenericConstraintProblem::setSolver(GenericConstraintSolver* solver) { - if(!solver) - return; - - const int dimension = getDimension(); - - if(!dimension) - { - currentError = 0.0; - currentIterations = 0; - return; - } - - const SReal t0 = (SReal)sofa::helper::system::thread::CTime::getTime() ; - const SReal timeScale = 1.0 / (SReal)sofa::helper::system::thread::CTime::getTicksPerSec(); - - SReal *dfree = getDfree(); - SReal *force = getF(); - SReal **w = getW(); - SReal tol = tolerance; - SReal *d = _d.ptr(); - - SReal error=0.0; - bool convergence = false; - sofa::type::vector tempForces; - - if(sor != 1.0) - { - tempForces.resize(dimension); - } - - if(scaleTolerance && !allVerified) - { - tol *= dimension; - } - - for(int i=0; iinit(i, w, force); - i += constraintsResolutions[i]->getNbLines(); - } - - bool showGraphs = false; - sofa::type::vector* graph_residuals = nullptr; - std::map < std::string, sofa::type::vector > *graph_forces = nullptr, *graph_violations = nullptr; - - showGraphs = solver->d_computeGraphs.getValue(); - - if(showGraphs) - { - graph_forces = solver->d_graphForces.beginEdit(); - graph_forces->clear(); - - graph_violations = solver->d_graphViolations.beginEdit(); - graph_violations->clear(); - - graph_residuals = &(*solver->d_graphErrors.beginEdit())["Error"]; - graph_residuals->clear(); - } - - sofa::type::vector tabErrors(dimension); - - int iterCount = 0; - - for(int i=0; i& graph_force = (*graph_forces)[oss.str()]; - graph_force.push_back(force[j]); - - sofa::type::vector& graph_violation = (*graph_violations)[oss.str()]; - graph_violation.push_back(d[j]); - } - - graph_residuals->push_back(error); - } - - if(sor != 1.0) - { - for(int j=0; j timeout) - { - - msg_info_when(solver!=nullptr, solver) << "TimeOut" ; - - currentError = error; - currentIterations = i+1; - return; - } - else if(allVerified) - { - if(constraintsAreVerified) - { - convergence = true; - break; - } - } - else if(error < tol) // do not stop at the first iteration (that is used for initial guess computation) - { - convergence = true; - break; - } - } - - sofa::helper::AdvancedTimer::valSet("GS iterations", currentIterations); - - result_output(solver, force, error, iterCount, convergence); - - if(showGraphs) - { - solver->d_graphErrors.endEdit(); - - sofa::type::vector& graph_constraints = (*solver->d_graphConstraints.beginEdit())["Constraints"]; - graph_constraints.clear(); - - for(int j=0; jgetNbLines(); - - if(tabErrors[j]) - graph_constraints.push_back(tabErrors[j]); - else if(constraintsResolutions[j]->getTolerance()) - graph_constraints.push_back(constraintsResolutions[j]->getTolerance()); - else - graph_constraints.push_back(tol); - - j += nbDofs; - } - solver->d_graphConstraints.endEdit(); - - solver->d_graphForces.endEdit(); - } -} - - -void GenericConstraintProblem::unbuiltGaussSeidel(SReal timeout, GenericConstraintSolver* solver) -{ - if(!solver) - return; - - if(!dimension) - { - currentError = 0.0; - currentIterations = 0; - return; - } - - SReal t0 = (SReal)sofa::helper::system::thread::CTime::getTime(); - SReal timeScale = 1.0 / (SReal)sofa::helper::system::thread::CTime::getTicksPerSec(); - - SReal *dfree = getDfree(); - SReal *force = getF(); - SReal **w = getW(); - SReal tol = tolerance; - - SReal *d = _d.ptr(); - - unsigned int iter = 0, nb = 0; - - SReal error=0.0; - - bool convergence = false; - sofa::type::vector tempForces; - if(sor != 1.0) tempForces.resize(dimension); - - if(scaleTolerance && !allVerified) - tol *= dimension; - - - for(int i=0; iinit(i, w, force); - i += constraintsResolutions[i]->getNbLines(); - } - memset(force, 0, dimension * sizeof(SReal)); // Erase previous forces for the time being - - - bool showGraphs = false; - sofa::type::vector* graph_residuals = nullptr; - std::map < std::string, sofa::type::vector > *graph_forces = nullptr, *graph_violations = nullptr; - sofa::type::vector tabErrors; - - - showGraphs = solver->d_computeGraphs.getValue(); - - if(showGraphs) - { - graph_forces = solver->d_graphForces.beginEdit(); - graph_forces->clear(); - - graph_violations = solver->d_graphViolations.beginEdit(); - graph_violations->clear(); - - graph_residuals = &(*solver->d_graphErrors.beginEdit())["Error"]; - graph_residuals->clear(); - } - - tabErrors.resize(dimension); - - // temporary buffers - std::vector errF; - std::vector tempF; - - for(iter=0; iter < static_cast(maxIterations); iter++) - { - bool constraintsAreVerified = true; - if(sor != 1.0) - { - std::copy_n(force, dimension, tempForces.begin()); - } - - error=0.0; - for (auto it_c = this->constraints_sequence.begin(); it_c != constraints_sequence.end(); ) // increment of it_c realized at the end of the loop - { - const auto j = *it_c; - //1. nbLines provide the dimension of the constraint - nb = constraintsResolutions[j]->getNbLines(); - - //2. for each line we compute the actual value of d - // (a)d is set to dfree - if(nb > errF.size()) - { - errF.resize(nb); - } - std::copy_n(&force[j], nb, errF.begin()); - std::copy_n(&dfree[j], nb, &d[j]); - - // (b) contribution of forces are added to d - for (auto* el : cclist_elems[j]) - { - if (el) - el->addConstraintDisplacement(d, j, j+nb-1); - } - - //3. the specific resolution of the constraint(s) is called - constraintsResolutions[j]->resolution(j, w, d, force, dfree); - - //4. the error is measured (displacement due to the new resolution (i.e. due to the new force)) - SReal contraintError = 0.0; - if(nb > 1) - { - for(unsigned int l=0; l tol) - constraintsAreVerified = false; - - contraintError += lineError; - } - } - else - { - contraintError = fabs(w[j][j] * (force[j] - errF[0])); - if(contraintError > tol) - constraintsAreVerified = false; - } - - if(constraintsResolutions[j]->getTolerance()) - { - if(contraintError > constraintsResolutions[j]->getTolerance()) - constraintsAreVerified = false; - contraintError *= tol / constraintsResolutions[j]->getTolerance(); - } - - error += contraintError; - tabErrors[j] = contraintError; - - //5. the force is updated for the constraint corrections - bool update = false; - for(unsigned int l=0; l tempF.size()) - { - tempF.resize(nb); - } - std::copy_n(&force[j], nb, tempF.begin()); - for(unsigned int l=0; lsetConstraintDForce(force, j, j+nb-1, update); - } - - std::copy_n(tempF.begin(), nb, &force[j]); - } - std::advance(it_c, nb); - } - - if(showGraphs) - { - for(int j=0; j& graph_force = (*graph_forces)[oss.str()]; - graph_force.push_back(force[j]); - - sofa::type::vector& graph_violation = (*graph_violations)[oss.str()]; - graph_violation.push_back(d[j]); - } - - graph_residuals->push_back(error); - } - - if(sor != 1.0) - { - for(int j=0; j timeout) - { - currentError = error; - currentIterations = iter+1; - return; - } - } - else if(allVerified) - { - if(constraintsAreVerified) - { - convergence = true; - break; - } - } - else if(error < tol) - { - convergence = true; - break; - } - } - - - - sofa::helper::AdvancedTimer::valSet("GS iterations", currentIterations); - - result_output(solver, force, error, iter, convergence); - - if(showGraphs) - { - solver->d_graphErrors.endEdit(); - - sofa::type::vector& graph_constraints = (*solver->d_graphConstraints.beginEdit())["Constraints"]; - graph_constraints.clear(); - - for(int j=0; jgetNbLines(); - - if(tabErrors[j]) - graph_constraints.push_back(tabErrors[j]); - else if(constraintsResolutions[j]->getTolerance()) - graph_constraints.push_back(constraintsResolutions[j]->getTolerance()); - else - graph_constraints.push_back(tol); - - j += nb; - } - solver->d_graphConstraints.endEdit(); - - solver->d_graphForces.endEdit(); - } -} - -void GenericConstraintProblem::NNCG(GenericConstraintSolver* solver, int iterationNewton) -{ - if(!solver) - return; - - const int dimension = getDimension(); - - if(!dimension) - { - currentError = 0.0; - currentIterations = 0; - return; - } - - m_lam.clear(); - m_lam.resize(dimension); - m_deltaF.clear(); - m_deltaF.resize(dimension); - m_deltaF_new.clear(); - m_deltaF_new.resize(dimension); - m_p.clear(); - m_p.resize(dimension); - - - SReal *dfree = getDfree(); - SReal *force = getF(); - SReal **w = getW(); - SReal tol = tolerance; - - SReal *d = _d.ptr(); - - - SReal error = 0.0; - bool convergence = false; - sofa::type::vector tempForces; - - if(sor != 1.0) - { - tempForces.resize(dimension); - } - - if(scaleTolerance && !allVerified) - { - tol *= dimension; - } - - for(int i=0; iinit(i, w, force); - i += constraintsResolutions[i]->getNbLines(); - } - - sofa::type::vector tabErrors(dimension); - - { - // perform one iteration of ProjectedGaussSeidel - bool constraintsAreVerified = true; - std::copy_n(force, dimension, std::begin(m_lam)); - - gaussSeidel_increment(false, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, tabErrors); - - for(int j=0; j 1) - { - m_p.clear(); - m_p.resize(dimension); - } - else - { - for(int j=0; j& tabErrors) const -{ - for(int j=0; jgetNbLines(); - - //2. for each line we compute the actual value of d - // (a)d is set to dfree - - std::vector errF(&force[j], &force[j+nb]); - std::copy_n(&dfree[j], nb, &d[j]); - - // (b) contribution of forces are added to d => TODO => optimization (no computation when force= 0 !!) - for(int k=0; kresolution(j, w, d, force, dfree); - - //4. the error is measured (displacement due to the new resolution (i.e. due to the new force)) - if(measureError) - { - SReal contraintError = 0.0; - if(nb > 1) - { - for(unsigned int l=0; l tol) - { - constraintsAreVerified = false; - } - - contraintError += lineError; - } - } - else - { - contraintError = fabs(w[j][j] * (force[j] - errF[0])); - if(contraintError > tol) - { - constraintsAreVerified = false; - } - } - - const bool givenTolerance = (bool)constraintsResolutions[j]->getTolerance(); - - if(givenTolerance) - { - if(contraintError > constraintsResolutions[j]->getTolerance()) - { - constraintsAreVerified = false; - } - contraintError *= tol / constraintsResolutions[j]->getTolerance(); - } - - error += contraintError; - tabErrors[j] = contraintError; - } - else - { - constraintsAreVerified = true; - } - - j += nb; - } + m_solver = solver; } void GenericConstraintProblem::result_output(GenericConstraintSolver *solver, SReal *force, SReal error, int iterCount, bool convergence) @@ -736,4 +107,7 @@ void GenericConstraintProblem::result_output(GenericConstraintSolver *solver, SR } } + + + } diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.h index 3103c9eac80..77cad9cf4d7 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.h @@ -33,54 +33,46 @@ class GenericConstraintSolver; class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintProblem : public ConstraintProblem { public: - sofa::linearalgebra::FullVector _d; - std::vector constraintsResolutions; - bool scaleTolerance, allVerified; - SReal sor; - SReal sceneTime; - SReal currentError; - int currentIterations; - - // For unbuilt version : - linearalgebra::SparseMatrix Wdiag; - std::list constraints_sequence; - bool change_sequence; typedef std::vector< core::behavior::BaseConstraintCorrection* > ConstraintCorrections; - typedef std::vector< core::behavior::BaseConstraintCorrection* >::iterator ConstraintCorrectionIterator; - - std::vector< ConstraintCorrections > cclist_elems; + GenericConstraintProblem(GenericConstraintSolver* solver) + : scaleTolerance(true) + , allVerified(false) + , sor(1.0) + , currentError(0.0) + , currentIterations(0) + , m_solver(solver) + {} - GenericConstraintProblem() : scaleTolerance(true), allVerified(false), sor(1.0) - , sceneTime(0.0), currentError(0.0), currentIterations(0) - , change_sequence(false) {} - ~GenericConstraintProblem() override { freeConstraintResolutions(); } + ~GenericConstraintProblem() override + { + freeConstraintResolutions(); + } void clear(int nbConstraints) override; void freeConstraintResolutions(); + int getNumConstraints(); + int getNumConstraintGroups(); + void result_output(GenericConstraintSolver* solver, SReal *force, SReal error, int iterCount, bool convergence); void solveTimed(SReal tol, int maxIt, SReal timeout) override; - /// Projective Gauss Seidel method building the compliance matrix - void gaussSeidel(SReal timeout=0, GenericConstraintSolver* solver = nullptr); - /// Projective Gauss Seidel unbuilt method - void unbuiltGaussSeidel(SReal timeout=0, GenericConstraintSolver* solver = nullptr); - /// Method from: - /// A nonsmooth nonlinear conjugate gradient method for interactive contact force problems - /// - 2010, Silcowitz, Morten and Niebe, Sarah and Erleben, Kenny - void NNCG(GenericConstraintSolver* solver = nullptr, int iterationNewton = 1); - - void gaussSeidel_increment(bool measureError, SReal *dfree, SReal *force, SReal **w, SReal tol, SReal *d, int dim, bool& constraintsAreVerified, SReal& error, sofa::type::vector& tabErrors) const; - void result_output(GenericConstraintSolver* solver, SReal *force, SReal error, int iterCount, bool convergence); + void setSolver(GenericConstraintSolver* solver); - int getNumConstraints(); - int getNumConstraintGroups(); + sofa::linearalgebra::FullVector _d; + std::vector constraintsResolutions; + bool scaleTolerance, allVerified; + SReal sor; + SReal currentError; + int currentIterations; -protected: sofa::linearalgebra::FullVector m_lam; sofa::linearalgebra::FullVector m_deltaF; sofa::linearalgebra::FullVector m_deltaF_new; sofa::linearalgebra::FullVector m_p; +protected: + + GenericConstraintSolver* m_solver; }; } diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp index 35408c73274..ac7362c155c 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include @@ -40,6 +39,9 @@ using sofa::simulation::mechanicalvisitor::MechanicalVOpVisitor; #include using sofa::simulation::mechanicalvisitor::MechanicalProjectJacobianMatrixVisitor; + + + namespace sofa::component::constraint::lagrangian::solver { @@ -60,18 +62,14 @@ void clearMultiVecId(sofa::core::objectmodel::BaseContext* ctx, const sofa::core } -static constexpr GenericConstraintSolver::ResolutionMethod defaultResolutionMethod("ProjectedGaussSeidel"); GenericConstraintSolver::GenericConstraintSolver() - : d_resolutionMethod( initData(&d_resolutionMethod, defaultResolutionMethod, "resolutionMethod", ("Method used to solve the constraint problem\n" + ResolutionMethod::dataDescription()).c_str())) - , d_maxIt(initData(&d_maxIt, 1000, "maxIterations", "maximal number of iterations of the Gauss-Seidel algorithm")) + : d_maxIt(initData(&d_maxIt, 1000, "maxIterations", "maximal number of iterations of iterative algorithm")) , d_tolerance(initData(&d_tolerance, 0.001_sreal, "tolerance", "residual error threshold for termination of the Gauss-Seidel algorithm")) , d_sor(initData(&d_sor, 1.0_sreal, "sor", "Successive Over Relaxation parameter (0-2)")) , d_regularizationTerm(initData(&d_regularizationTerm, 0.0_sreal, "regularizationTerm", "Add regularization factor times the identity matrix to the compliance W when solving constraints")) , d_scaleTolerance(initData(&d_scaleTolerance, true, "scaleTolerance", "Scale the error tolerance with the number of constraints")) , d_allVerified(initData(&d_allVerified, false, "allVerified", "All constraints must be verified (each constraint's error < tolerance)")) - , d_newtonIterations(initData(&d_newtonIterations, 100, "newtonIterations", "Maximum iteration number of Newton (for the NonsmoothNonlinearConjugateGradient solver only)")) - , d_multithreading(initData(&d_multithreading, false, "multithreading", "Build compliances concurrently")) , d_computeGraphs(initData(&d_computeGraphs, false, "computeGraphs", "Compute graphs of errors and forces during resolution")) , d_graphErrors(initData(&d_graphErrors, "graphErrors", "Sum of the constraints' errors at each iteration")) , d_graphConstraints(initData(&d_graphConstraints, "graphConstraints", "Graph of each constraint's error at the end of the resolution")) @@ -81,12 +79,10 @@ GenericConstraintSolver::GenericConstraintSolver() , d_currentNumConstraintGroups(initData(&d_currentNumConstraintGroups, 0, "currentNumConstraintGroups", "OUTPUT: current number of constraints")) , d_currentIterations(initData(&d_currentIterations, 0, "currentIterations", "OUTPUT: current number of constraint groups")) , d_currentError(initData(&d_currentError, 0.0_sreal, "currentError", "OUTPUT: current error")) - , d_reverseAccumulateOrder(initData(&d_reverseAccumulateOrder, false, "reverseAccumulateOrder", "True to accumulate constraints from nodes in reversed order (can be necessary when using multi-mappings or interaction constraints not following the node hierarchy)")) , d_constraintForces(initData(&d_constraintForces,"constraintForces","OUTPUT: constraint forces (stored only if computeConstraintForces=True)")) , d_computeConstraintForces(initData(&d_computeConstraintForces,false, "computeConstraintForces", "enable the storage of the constraintForces.")) - , current_cp(&m_cpBuffer[0]) , last_cp(nullptr) { addAlias(&d_maxIt, "maxIt"); @@ -117,10 +113,18 @@ GenericConstraintSolver::GenericConstraintSolver() } GenericConstraintSolver::~GenericConstraintSolver() -{} +{ + for (unsigned i=0; i< CP_BUFFER_SIZE; ++i) + { + delete m_cpBuffer[i]; + } +} -void GenericConstraintSolver::init() + + +void GenericConstraintSolver:: init() { + this->initializeConstraintProblems(); ConstraintSolverImpl::init(); simulation::common::VectorOperations vop(sofa::core::execparams::defaultInstance(), this->getContext()); @@ -134,20 +138,15 @@ void GenericConstraintSolver::init() dx.realloc(&vop,false,true, core::VecIdProperties{"constraint_dx", GetClass()->className}); m_dxId = dx.id(); } +} - if(d_multithreading.getValue()) +void GenericConstraintSolver::initializeConstraintProblems() +{ + for (unsigned i=0; i< CP_BUFFER_SIZE; ++i) { - simulation::MainTaskSchedulerFactory::createInRegistry()->init(); - } - - if(d_newtonIterations.isSet()) - { - static constexpr ResolutionMethod NonsmoothNonlinearConjugateGradient("NonsmoothNonlinearConjugateGradient"); - if (d_resolutionMethod.getValue() != NonsmoothNonlinearConjugateGradient) - { - msg_warning() << "data \"newtonIterations\" is not only taken into account when using the NonsmoothNonlinearConjugateGradient solver"; - } + m_cpBuffer[i] = new GenericConstraintProblem(this); } + current_cp = m_cpBuffer[0]; } void GenericConstraintSolver::cleanup() @@ -209,170 +208,12 @@ bool GenericConstraintSolver::buildSystem(const core::ConstraintParams *cParams, MechanicalGetConstraintResolutionVisitor(cParams, current_cp->constraintsResolutions).execute(getContext()); } - // Resolution depending on the method selected - switch ( d_resolutionMethod.getValue() ) - { - case ResolutionMethod("ProjectedGaussSeidel"): - case ResolutionMethod("NonsmoothNonlinearConjugateGradient"): - { - buildSystem_matrixAssembly(cParams); - break; - } - case ResolutionMethod("UnbuiltGaussSeidel"): - { - buildSystem_matrixFree(numConstraints); - break; - } - default: - msg_error() << "Wrong \"resolutionMethod\" given"; - } - return true; -} + this->doBuildSystem(cParams, current_cp, numConstraints); -void GenericConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W) -{ - const SReal regularization = d_regularizationTerm.getValue(); - if (regularization>std::numeric_limits::epsilon()) - { - for (int i=0; iisActive()) continue; - - current_cp->constraints_sequence.resize(numConstraints); - std::iota(current_cp->constraints_sequence.begin(), current_cp->constraints_sequence.end(), 0); - - // some constraint corrections (e.g LinearSolverConstraintCorrection) - // can change the order of the constraints, to optimize later computations - cc->resetForUnbuiltResolution(current_cp->getF(), current_cp->constraints_sequence); - } - - sofa::linearalgebra::SparseMatrix* Wdiag = ¤t_cp->Wdiag; - Wdiag->resize(numConstraints, numConstraints); - - // for each contact, the constraint corrections that are involved with the contact are memorized - current_cp->cclist_elems.clear(); - current_cp->cclist_elems.resize(numConstraints); - const int nbCC = l_constraintCorrections.size(); - for (unsigned int i = 0; i < numConstraints; i++) - current_cp->cclist_elems[i].resize(nbCC, nullptr); - - unsigned int nbObjects = 0; - for (unsigned int c_id = 0; c_id < numConstraints;) - { - bool foundCC = false; - nbObjects++; - const unsigned int l = current_cp->constraintsResolutions[c_id]->getNbLines(); - - for (unsigned int j = 0; j < l_constraintCorrections.size(); j++) - { - core::behavior::BaseConstraintCorrection* cc = l_constraintCorrections[j]; - if (!cc->isActive()) continue; - if (cc->hasConstraintNumber(c_id)) - { - current_cp->cclist_elems[c_id][j] = cc; - cc->getBlockDiagonalCompliance(Wdiag, c_id, c_id + l - 1); - foundCC = true; - } - } - - msg_error_when(!foundCC) << "No constraintCorrection found for constraint" << c_id ; - - SReal** w = current_cp->getW(); - for(unsigned int m = c_id; m < c_id + l; m++) - for(unsigned int n = c_id; n < c_id + l; n++) - w[m][n] = Wdiag->element(m, n); - - c_id += l; - } - - current_cp->change_sequence = false; - if(current_cp->constraints_sequence.size() == nbObjects) - current_cp->change_sequence=true; - - addRegularization(current_cp->W); - addRegularization(current_cp->Wdiag); - -} - -GenericConstraintSolver::ComplianceWrapper::ComplianceMatrixType& GenericConstraintSolver:: -ComplianceWrapper::matrix() -{ - if (m_isMultiThreaded) - { - if (!m_threadMatrix) - { - m_threadMatrix = std::make_unique(); - m_threadMatrix->resize(m_complianceMatrix.rowSize(), m_complianceMatrix.colSize()); - } - return *m_threadMatrix; - } - return m_complianceMatrix; -} - -void GenericConstraintSolver::ComplianceWrapper::assembleMatrix() const -{ - if (m_threadMatrix) - { - for (linearalgebra::BaseMatrix::Index j = 0; j < m_threadMatrix->rowSize(); ++j) - { - for (linearalgebra::BaseMatrix::Index l = 0; l < m_threadMatrix->colSize(); ++l) - { - m_complianceMatrix.add(j, l, m_threadMatrix->element(j,l)); - } - } - } + return true; } -void GenericConstraintSolver::buildSystem_matrixAssembly(const core::ConstraintParams *cParams) -{ - SCOPED_TIMER_VARNAME(getComplianceTimer, "Get Compliance"); - dmsg_info() <<" computeCompliance in " << l_constraintCorrections.size()<< " constraintCorrections" ; - - const bool multithreading = d_multithreading.getValue(); - - const simulation::ForEachExecutionPolicy execution = multithreading ? - simulation::ForEachExecutionPolicy::PARALLEL : - simulation::ForEachExecutionPolicy::SEQUENTIAL; - - simulation::TaskScheduler* taskScheduler = simulation::MainTaskSchedulerFactory::createInRegistry(); - assert(taskScheduler); - - //Used to prevent simultaneous accesses to the main compliance matrix - std::mutex mutex; - - //Visits all constraint corrections to compute the compliance matrix projected - //in the constraint space. - simulation::forEachRange(execution, *taskScheduler, l_constraintCorrections.begin(), l_constraintCorrections.end(), - [&cParams, this, &multithreading, &mutex](const auto& range) - { - ComplianceWrapper compliance(current_cp->W, multithreading); - - for (auto it = range.start; it != range.end; ++it) - { - core::behavior::BaseConstraintCorrection* cc = *it; - if (cc->isActive()) - { - cc->addComplianceInConstraintSpace(cParams, &compliance.matrix()); - } - } - - std::lock_guard guard(mutex); - compliance.assembleMatrix(); - }); - - addRegularization(current_cp->W); - dmsg_info() << " computeCompliance_done " ; -} void GenericConstraintSolver::rebuildSystem(const SReal massFactor, const SReal forceFactor) { @@ -429,36 +270,17 @@ bool GenericConstraintSolver::solveSystem(const core::ConstraintParams * /*cPara current_cp->allVerified = d_allVerified.getValue(); current_cp->sor = d_sor.getValue(); - - // Resolution depending on the method selected - switch ( d_resolutionMethod.getValue()) + if (notMuted()) { - case ResolutionMethod("ProjectedGaussSeidel"): { - if (notMuted()) - { - std::stringstream tmp; - tmp << "---> Before Resolution" << msgendl ; - printLCP(tmp, current_cp->getDfree(), current_cp->getW(), current_cp->getF(), current_cp->getDimension(), true); + std::stringstream tmp; + tmp << "---> Before Resolution" << msgendl ; + printLCP(tmp, current_cp->getDfree(), current_cp->getW(), current_cp->getF(), current_cp->getDimension(), true); - msg_info() << tmp.str() ; - } - SCOPED_TIMER_VARNAME(gaussSeidelTimer, "ConstraintsGaussSeidel"); - current_cp->gaussSeidel(0, this); - break; - } - case ResolutionMethod("UnbuiltGaussSeidel"): { - SCOPED_TIMER_VARNAME(unbuiltGaussSeidelTimer, "ConstraintsUnbuiltGaussSeidel"); - current_cp->unbuiltGaussSeidel(0, this); - break; - } - case ResolutionMethod("NonsmoothNonlinearConjugateGradient"): { - current_cp->NNCG(this, d_newtonIterations.getValue()); - break; - } - default: - msg_error() << "Wrong \"resolutionMethod\" given"; + msg_info() << tmp.str() ; } + this->doSolve(current_cp, 0.0); + this->d_currentError.setValue(current_cp->currentError); this->d_currentIterations.setValue(current_cp->currentIterations); @@ -591,7 +413,7 @@ void GenericConstraintSolver::lockConstraintProblem(sofa::core::objectmodel::Bas for (unsigned int i = 0; i < CP_BUFFER_SIZE; ++i) { - GenericConstraintProblem* p = &m_cpBuffer[i]; + GenericConstraintProblem* p = m_cpBuffer[i]; if (p == p1 || p == p2) { m_cpIsLocked[i] = true; @@ -616,10 +438,18 @@ sofa::core::MultiVecDerivId GenericConstraintSolver::getDx() const return m_dxId; } -void registerGenericConstraintSolver(sofa::core::ObjectFactory* factory) +void GenericConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization) { - factory->registerObjects(core::ObjectRegistrationData("A Generic Constraint Solver using the Linear Complementarity Problem formulation to solve Constraint based components") - .add< GenericConstraintSolver >()); + if (regularization>std::numeric_limits::epsilon()) + { + for (int i=0; i d_resolutionMethod; ///< Method used to solve the constraint problem, among: "ProjectedGaussSeidel", "UnbuiltGaussSeidel" or "for NonsmoothNonlinearConjugateGradient" - - Data d_maxIt; ///< maximal number of iterations of the Gauss-Seidel algorithm + Data d_maxIt; ///< maximal number of iterations of iterative algorithm Data d_tolerance; ///< residual error threshold for termination of the Gauss-Seidel algorithm Data d_sor; ///< Successive Over Relaxation parameter (0-2) Data< SReal > d_regularizationTerm; ///< add regularization*Id to W when solving for constraints Data d_scaleTolerance; ///< Scale the error tolerance with the number of constraints Data d_allVerified; ///< All constraints must be verified (each constraint's error < tolerance) - Data d_newtonIterations; ///< Maximum iteration number of Newton (for the NonsmoothNonlinearConjugateGradient solver only) - Data d_multithreading; ///< Build compliances concurrently Data d_computeGraphs; ///< Compute graphs of errors and forces during resolution Data > > d_graphErrors; ///< Sum of the constraints' errors at each iteration Data > > d_graphConstraints; ///< Graph of each constraint's error at the end of the resolution @@ -87,7 +76,6 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : Data d_currentNumConstraintGroups; ///< OUTPUT: current number of constraints Data d_currentIterations; ///< OUTPUT: current number of constraint groups Data d_currentError; ///< OUTPUT: current error - Data d_reverseAccumulateOrder; ///< True to accumulate constraints from nodes in reversed order (can be necessary when using multi-mappings or interaction constraints not following the node hierarchy) Data> d_constraintForces; ///< OUTPUT: constraint forces (stored only if computeConstraintForces=True) Data d_computeConstraintForces; ///< The indices of the constraintForces to store in the constraintForce data field. @@ -99,38 +87,47 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : void clearConstraintProblemLocks(); static constexpr auto CP_BUFFER_SIZE = 10; - sofa::type::fixed_array m_cpBuffer; + sofa::type::fixed_array m_cpBuffer; sofa::type::fixed_array m_cpIsLocked; GenericConstraintProblem *current_cp, *last_cp; sofa::core::MultiVecDerivId m_lambdaId; sofa::core::MultiVecDerivId m_dxId; - void buildSystem_matrixFree(unsigned int numConstraints); + virtual void initializeConstraintProblems(); + + /***** + * + * @brief This internal method is used to build the system. It should use the list of constraint correction (l_constraintCorrections) to build the full constraint problem. + * + * @param cParams: Container providing quick access to all data related to the mechanics (position, velocity etc..) for all mstate + * @param problem: constraint problem containing data structures used for solving the constraint + * problem: the constraint matrix, the unknown vector, the free violation etc... + * The goal of this method is to fill parts of this structure to be then used to + * find the unknown vector. + * @param numConstraints: number of atomic constraint + * + */ + virtual void doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints) = 0; + + /***** + * + * @brief This internal method is used to solve the constraint problem. It essentially uses the constraint problem structures. + * + * @param problem: constraint problem containing data structures used for solving the constraint + * problem: the constraint matrix, the unknown vector, the free violation etc... + * The goal of this method is to use the problem structures to compute the final solution. + * @param timeout: timeout to use this solving method in a haptic thread. If the timeout is reached then the solving must stops. + * + */ + virtual void doSolve( GenericConstraintProblem * problem, SReal timeout = 0.0) = 0; + + + static void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization); - // Explicitly compute the compliance matrix projected in the constraint space - void buildSystem_matrixAssembly(const core::ConstraintParams *cParams); private: - struct ComplianceWrapper - { - using ComplianceMatrixType = sofa::linearalgebra::LPtrFullMatrix; - - ComplianceWrapper(ComplianceMatrixType& complianceMatrix, bool isMultiThreaded) - : m_isMultiThreaded(isMultiThreaded), m_complianceMatrix(complianceMatrix) {} - - ComplianceMatrixType& matrix(); - - void assembleMatrix() const; - - private: - bool m_isMultiThreaded { false }; - ComplianceMatrixType& m_complianceMatrix; - std::unique_ptr m_threadMatrix; - }; - - sofa::type::vector filteredConstraintCorrections() const; void computeAndApplyMotionCorrection(const core::ConstraintParams* cParams, GenericConstraintSolver::MultiVecId res1, GenericConstraintSolver::MultiVecId res2) const; diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.cpp new file mode 100644 index 00000000000..540daa6e895 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.cpp @@ -0,0 +1,172 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#include +#include +#include +#include + + +namespace sofa::component::constraint::lagrangian::solver +{ + +void NNCGConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal timeout) +{ + SCOPED_TIMER_VARNAME(unbuiltGaussSeidelTimer, "NonsmoothNonlinearConjugateGradient"); + + + const int dimension = problem->getDimension(); + + if(!dimension) + { + problem->currentError = 0.0; + problem->currentIterations = 0; + return; + } + + problem->m_lam.clear(); + problem->m_lam.resize(dimension); + problem->m_deltaF.clear(); + problem->m_deltaF.resize(dimension); + problem->m_deltaF_new.clear(); + problem->m_deltaF_new.resize(dimension); + problem->m_p.clear(); + problem->m_p.resize(dimension); + + + SReal *dfree = problem->getDfree(); + SReal *force = problem->getF(); + SReal **w = problem->getW(); + SReal tol = problem->tolerance; + + SReal *d = problem->_d.ptr(); + + + SReal error = 0.0; + bool convergence = false; + sofa::type::vector tempForces; + + if(problem->sor != 1.0) + { + tempForces.resize(dimension); + } + + if(problem->scaleTolerance && !problem->allVerified) + { + tol *= dimension; + } + + for(int i=0; iconstraintsResolutions[i]) + { + msg_error() << "Bad size of constraintsResolutions in GenericConstraintSolver" ; + break; + } + problem->constraintsResolutions[i]->init(i, w, force); + i += problem->constraintsResolutions[i]->getNbLines(); + } + + sofa::type::vector tabErrors(dimension); + + { + // perform one iteration of ProjectedGaussSeidel + bool constraintsAreVerified = true; + std::copy_n(force, dimension, std::begin(problem->m_lam)); + + gaussSeidel_increment(false, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, problem->constraintsResolutions, tabErrors); + + for(int j=0; jm_deltaF[j] = -(force[j] - problem->m_lam[j]); + problem->m_p[j] = - problem->m_deltaF[j]; + } + } + + + + int iterCount = 0; + + for(int i=1; im_lam[j] = force[j]; + } + + + error=0.0; + gaussSeidel_increment(true, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, problem->constraintsResolutions, tabErrors); + + + if(problem->allVerified) + { + if(constraintsAreVerified) + { + convergence = true; + break; + } + } + else if(error < tol) // do not stop at the first iteration (that is used for initial guess computation) + { + convergence = true; + break; + } + + + // NNCG update with the correction p + for(int j=0; jm_deltaF_new[j] = -(force[j] - problem->m_lam[j]); + } + + const SReal beta = problem->m_deltaF_new.dot(problem->m_deltaF_new) / problem->m_deltaF.dot(problem->m_deltaF); + problem->m_deltaF.eq(problem->m_deltaF_new, 1); + + if(beta > 1) + { + problem->m_p.clear(); + problem->m_p.resize(dimension); + } + else + { + for(int j=0; jm_p[j]; + problem->m_p[j] = beta*problem->m_p[j] -problem-> m_deltaF[j]; + } + } + } + + problem->result_output(this, force, error, iterCount, convergence); +} + +void registerNNCGConstraintSolver(sofa::core::ObjectFactory* factory) +{ + factory->registerObjects(core::ObjectRegistrationData("A Constraint Solver using the Linear Complementarity Problem formulation to solve Constraint based components using the Non-smooth Non-linear Conjugate Gradient method") + .add< NNCGConstraintSolver >()); +} + +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.h new file mode 100644 index 00000000000..26bd9e95eed --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.h @@ -0,0 +1,36 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include + +namespace sofa::component::constraint::lagrangian::solver +{ +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API NNCGConstraintSolver : public ProjectedGaussSeidelConstraintSolver +{ +public: + SOFA_CLASS(NNCGConstraintSolver, ProjectedGaussSeidelConstraintSolver); + + virtual void doSolve(GenericConstraintProblem * problem , SReal timeout = 0.0) override; +}; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.cpp new file mode 100644 index 00000000000..efaeb6ed2d9 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.cpp @@ -0,0 +1,284 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#include +#include +#include +#include +#include + +namespace sofa::component::constraint::lagrangian::solver +{ + +void ProjectedGaussSeidelConstraintSolver::doSolve( GenericConstraintProblem * problem ,SReal timeout) +{ + SCOPED_TIMER_VARNAME(gaussSeidelTimer, "ConstraintsGaussSeidel"); + + + const int dimension = problem->getDimension(); + + if(!dimension) + { + problem->currentError = 0.0; + problem->currentIterations = 0; + return; + } + + const SReal t0 = (SReal)sofa::helper::system::thread::CTime::getTime() ; + const SReal timeScale = 1.0 / (SReal)sofa::helper::system::thread::CTime::getTicksPerSec(); + + SReal *dfree = problem->getDfree(); + SReal *force = problem->getF(); + SReal **w = problem->getW(); + SReal tol = problem->tolerance; + SReal *d = problem->_d.ptr(); + + SReal error=0.0; + bool convergence = false; + sofa::type::vector tempForces; + + if(problem->sor != 1.0) + { + tempForces.resize(dimension); + } + + if(problem->scaleTolerance && !problem->allVerified) + { + tol *= dimension; + } + + for(int i=0; iconstraintsResolutions[i]) + { + msg_error()<< "Bad size of constraintsResolutions in GenericConstraintSolver" ; + break; + } + problem->constraintsResolutions[i]->init(i, w, force); + i += problem->constraintsResolutions[i]->getNbLines(); + } + + bool showGraphs = false; + sofa::type::vector* graph_residuals = nullptr; + std::map < std::string, sofa::type::vector > *graph_forces = nullptr, *graph_violations = nullptr; + + showGraphs = d_computeGraphs.getValue(); + + if(showGraphs) + { + graph_forces = d_graphForces.beginEdit(); + graph_forces->clear(); + + graph_violations = d_graphViolations.beginEdit(); + graph_violations->clear(); + + graph_residuals = &(*d_graphErrors.beginEdit())["Error"]; + graph_residuals->clear(); + } + + sofa::type::vector tabErrors(dimension); + + int iterCount = 0; + + for(int i=0; imaxIterations; i++) + { + iterCount ++; + bool constraintsAreVerified = true; + + if(problem->sor != 1.0) + { + std::copy_n(force, dimension, tempForces.begin()); + } + + error=0.0; + gaussSeidel_increment(true, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, problem->constraintsResolutions, tabErrors); + + if(showGraphs) + { + for(int j=0; j& graph_force = (*graph_forces)[oss.str()]; + graph_force.push_back(force[j]); + + sofa::type::vector& graph_violation = (*graph_violations)[oss.str()]; + graph_violation.push_back(d[j]); + } + + graph_residuals->push_back(error); + } + + if(problem->sor != 1.0) + { + for(int j=0; jsor * force[j] + (1-problem->sor) * tempForces[j]; + } + } + + const SReal t1 = (SReal)sofa::helper::system::thread::CTime::getTime(); + const SReal dt = (t1 - t0)*timeScale; + + if(timeout && dt > timeout) + { + + msg_info() << "TimeOut" ; + + problem->currentError = error; + problem->currentIterations = i+1; + return; + } + else if(problem->allVerified) + { + if(constraintsAreVerified) + { + convergence = true; + break; + } + } + else if(error < tol) // do not stop at the first iteration (that is used for initial guess computation) + { + convergence = true; + break; + } + } + + sofa::helper::AdvancedTimer::valSet("GS iterations", problem->currentIterations); + + problem->result_output(this, force, error, iterCount, convergence); + + if(showGraphs) + { + d_graphErrors.endEdit(); + + sofa::type::vector& graph_constraints = (*d_graphConstraints.beginEdit())["Constraints"]; + graph_constraints.clear(); + + for(int j=0; jconstraintsResolutions[j]->getNbLines(); + + if(tabErrors[j]) + graph_constraints.push_back(tabErrors[j]); + else if(problem->constraintsResolutions[j]->getTolerance()) + graph_constraints.push_back(problem->constraintsResolutions[j]->getTolerance()); + else + graph_constraints.push_back(tol); + + j += nbDofs; + } + d_graphConstraints.endEdit(); + + d_graphForces.endEdit(); + } +} +void ProjectedGaussSeidelConstraintSolver::gaussSeidel_increment(bool measureError, SReal *dfree, SReal *force, SReal **w, SReal tol, SReal *d, int dim, bool& constraintsAreVerified, SReal& error, std::vector& constraintCorrections, sofa::type::vector& tabErrors) const +{ + for(int j=0; jgetNbLines(); + + //2. for each line we compute the actual value of d + // (a)d is set to dfree + + std::vector errF(&force[j], &force[j+nb]); + std::copy_n(&dfree[j], nb, &d[j]); + + // (b) contribution of forces are added to d => TODO => optimization (no computation when force= 0 !!) + for(int k=0; kresolution(j, w, d, force, dfree); + + //4. the error is measured (displacement due to the new resolution (i.e. due to the new force)) + if(measureError) + { + SReal contraintError = 0.0; + if(nb > 1) + { + for(unsigned int l=0; l tol) + { + constraintsAreVerified = false; + } + + contraintError += lineError; + } + } + else + { + contraintError = fabs(w[j][j] * (force[j] - errF[0])); + if(contraintError > tol) + { + constraintsAreVerified = false; + } + } + + const bool givenTolerance = (bool)constraintCorrections[j]->getTolerance(); + + if(givenTolerance) + { + if(contraintError > constraintCorrections[j]->getTolerance()) + { + constraintsAreVerified = false; + } + contraintError *= tol / constraintCorrections[j]->getTolerance(); + } + + error += contraintError; + tabErrors[j] = contraintError; + } + else + { + constraintsAreVerified = true; + } + + j += nb; + } +} + + +void registerProjectedGaussSeidelConstraintSolver(sofa::core::ObjectFactory* factory) +{ + factory->registerObjects(core::ObjectRegistrationData("A Constraint Solver using the Linear Complementarity Problem formulation to solve Constraint based components using a Projected Gauss-Seidel iterative method") + .add< ProjectedGaussSeidelConstraintSolver >()); +} + + +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h new file mode 100644 index 00000000000..35fd4a903c4 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h @@ -0,0 +1,39 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include + +namespace sofa::component::constraint::lagrangian::solver +{ +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ProjectedGaussSeidelConstraintSolver : public BuiltConstraintSolver +{ +public: + SOFA_CLASS(ProjectedGaussSeidelConstraintSolver, BuiltConstraintSolver); +protected: + virtual void doSolve(GenericConstraintProblem * problem , SReal timeout = 0.0) override; + + void gaussSeidel_increment(bool measureError, SReal *dfree, SReal *force, SReal **w, SReal tol, SReal *d, int dim, bool& constraintsAreVerified, SReal& error, std::vector& constraintCorrections, sofa::type::vector& tabErrors) const; + +}; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.h new file mode 100644 index 00000000000..579d94efcb1 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.h @@ -0,0 +1,52 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include + +namespace sofa::component::constraint::lagrangian::solver +{ + +/** + * \brief This class adds components needed for unbuilt solvers to the GenericConstraintProblem + * This needs to be used by unbuilt solvers. + */ +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltConstraintProblem : public GenericConstraintProblem +{ +public: + typedef std::vector< core::behavior::BaseConstraintCorrection* > ConstraintCorrections; + + UnbuiltConstraintProblem(GenericConstraintSolver* solver) + : GenericConstraintProblem(solver) + {} + + + linearalgebra::SparseMatrix Wdiag; /** UNBUILT **/ + std::list constraints_sequence; /** UNBUILT **/ + std::vector< ConstraintCorrections > cclist_elems; /** UNBUILT **/ + + +}; +} diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp new file mode 100644 index 00000000000..830dfda7f9a --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp @@ -0,0 +1,106 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#include +#include +#include + +namespace sofa::component::constraint::lagrangian::solver +{ + +void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem, unsigned int numConstraints) +{ + SOFA_UNUSED(cParams); + UnbuiltConstraintProblem* c_current_cp = dynamic_cast(problem); + if (c_current_cp == nullptr) + { + msg_error()<<"Constraint problem must derive from UnbuiltConstraintProblem"; + return; + } + + for (const auto& cc : l_constraintCorrections) + { + if (!cc->isActive()) continue; + + c_current_cp->constraints_sequence.resize(numConstraints); + std::iota(c_current_cp->constraints_sequence.begin(), c_current_cp->constraints_sequence.end(), 0); + + // some constraint corrections (e.g LinearSolverConstraintCorrection) + // can change the order of the constraints, to optimize later computations + cc->resetForUnbuiltResolution(c_current_cp->getF(), c_current_cp->constraints_sequence); + } + + sofa::linearalgebra::SparseMatrix* localWdiag = &c_current_cp->Wdiag; + localWdiag->resize(numConstraints, numConstraints); + + // for each contact, the constraint corrections that are involved with the contact are memorized + c_current_cp->cclist_elems.clear(); + c_current_cp->cclist_elems.resize(numConstraints); + const int nbCC = l_constraintCorrections.size(); + for (unsigned int i = 0; i < numConstraints; i++) + c_current_cp->cclist_elems[i].resize(nbCC, nullptr); + + unsigned int nbObjects = 0; + for (unsigned int c_id = 0; c_id < numConstraints;) + { + bool foundCC = false; + nbObjects++; + const unsigned int l = c_current_cp->constraintsResolutions[c_id]->getNbLines(); + + for (unsigned int j = 0; j < l_constraintCorrections.size(); j++) + { + core::behavior::BaseConstraintCorrection* cc = l_constraintCorrections[j]; + if (!cc->isActive()) continue; + if (cc->hasConstraintNumber(c_id)) + { + c_current_cp->cclist_elems[c_id][j] = cc; + cc->getBlockDiagonalCompliance(localWdiag, c_id, c_id + l - 1); + foundCC = true; + } + } + + msg_error_when(!foundCC) << "No constraintCorrection found for constraint" << c_id ; + + SReal** w = c_current_cp->getW(); + for(unsigned int m = c_id; m < c_id + l; m++) + for(unsigned int n = c_id; n < c_id + l; n++) + w[m][n] = localWdiag->element(m, n); + + c_id += l; + } + + addRegularization(c_current_cp->W, d_regularizationTerm.getValue()); + addRegularization(c_current_cp->Wdiag, d_regularizationTerm.getValue()); + +} + +void UnbuiltConstraintSolver::initializeConstraintProblems() +{ + for (unsigned i=0; i< CP_BUFFER_SIZE; ++i) + { + m_cpBuffer[i] = new UnbuiltConstraintProblem(this); + } + current_cp = m_cpBuffer[0]; +} + + +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.h new file mode 100644 index 00000000000..fe6ff57c60d --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.h @@ -0,0 +1,48 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include + + + +namespace sofa::component::constraint::lagrangian::solver +{ +/** + * \brief This component implements a generic way of preparing system for solvers that doesn't need + * a build version of the constraint matrix. Any solver that are based on an unbuilt system should + * inherit from this. + * This component is purely virtual because doSolve is not defined and needs to be defined in the + * inherited class + */ +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltConstraintSolver : public GenericConstraintSolver +{ +public: + SOFA_CLASS(UnbuiltConstraintSolver, GenericConstraintSolver); + + virtual void initializeConstraintProblems() override; + +protected: + virtual void doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem, unsigned int numConstraints) override; +}; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp new file mode 100644 index 00000000000..7f447554367 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp @@ -0,0 +1,300 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#include +#include +#include +#include +#include +#include + + +namespace sofa::component::constraint::lagrangian::solver +{ + +void UnbuiltGaussSeidelConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal timeout) +{ + UnbuiltConstraintProblem* c_current_cp = dynamic_cast(problem); + if (c_current_cp == nullptr) + { + msg_error()<<"Constraint problem must derive from UnbuiltConstraintProblem"; + return; + } + + SCOPED_TIMER_VARNAME(unbuiltGaussSeidelTimer, "ConstraintsUnbuiltGaussSeidel"); + + + if(!c_current_cp->getDimension()) + { + c_current_cp->currentError = 0.0; + c_current_cp->currentIterations = 0; + return; + } + + SReal t0 = (SReal)sofa::helper::system::thread::CTime::getTime(); + SReal timeScale = 1.0 / (SReal)sofa::helper::system::thread::CTime::getTicksPerSec(); + + SReal *dfree = c_current_cp->getDfree(); + SReal *force = c_current_cp->getF(); + SReal **w = c_current_cp->getW(); + SReal tol = c_current_cp->tolerance; + + SReal *d = c_current_cp->_d.ptr(); + + unsigned int iter = 0, nb = 0; + + SReal error=0.0; + + bool convergence = false; + sofa::type::vector tempForces; + if(c_current_cp->sor != 1.0) tempForces.resize(c_current_cp->getDimension()); + + if(c_current_cp->scaleTolerance && !c_current_cp->allVerified) + tol *= c_current_cp->getDimension(); + + + for(int i=0; igetDimension(); ) + { + if(!c_current_cp->constraintsResolutions[i]) + { + msg_warning() << "Bad size of constraintsResolutions in GenericConstraintSolver" ; + c_current_cp->setDimension(i); + break; + } + c_current_cp->constraintsResolutions[i]->init(i, w, force); + i += c_current_cp->constraintsResolutions[i]->getNbLines(); + } + memset(force, 0, c_current_cp->getDimension() * sizeof(SReal)); // Erase previous forces for the time being + + + bool showGraphs = false; + sofa::type::vector* graph_residuals = nullptr; + std::map < std::string, sofa::type::vector > *graph_forces = nullptr, *graph_violations = nullptr; + sofa::type::vector tabErrors; + + + showGraphs = d_computeGraphs.getValue(); + + if(showGraphs) + { + graph_forces = d_graphForces.beginEdit(); + graph_forces->clear(); + + graph_violations = d_graphViolations.beginEdit(); + graph_violations->clear(); + + graph_residuals = &(*d_graphErrors.beginEdit())["Error"]; + graph_residuals->clear(); + } + + tabErrors.resize(c_current_cp->getDimension()); + + // temporary buffers + std::vector errF; + std::vector tempF; + + for(iter=0; iter < static_cast(c_current_cp->maxIterations); iter++) + { + bool constraintsAreVerified = true; + if(c_current_cp->sor != 1.0) + { + std::copy_n(force, c_current_cp->getDimension(), tempForces.begin()); + } + + error=0.0; + for (auto it_c = c_current_cp->constraints_sequence.begin(); it_c != c_current_cp->constraints_sequence.end(); ) // increment of it_c realized at the end of the loop + { + const auto j = *it_c; + //1. nbLines provide the dimension of the constraint + nb = c_current_cp->constraintsResolutions[j]->getNbLines(); + + //2. for each line we compute the actual value of d + // (a)d is set to dfree + if(nb > errF.size()) + { + errF.resize(nb); + } + std::copy_n(&force[j], nb, errF.begin()); + std::copy_n(&dfree[j], nb, &d[j]); + + // (b) contribution of forces are added to d + for (auto* el : c_current_cp->cclist_elems[j]) + { + if (el) + el->addConstraintDisplacement(d, j, j+nb-1); + } + + //3. the specific resolution of the constraint(s) is called + c_current_cp->constraintsResolutions[j]->resolution(j, w, d, force, dfree); + + //4. the error is measured (displacement due to the new resolution (i.e. due to the new force)) + SReal contraintError = 0.0; + if(nb > 1) + { + for(unsigned int l=0; l tol) + constraintsAreVerified = false; + + contraintError += lineError; + } + } + else + { + contraintError = fabs(w[j][j] * (force[j] - errF[0])); + if(contraintError > tol) + constraintsAreVerified = false; + } + + if(c_current_cp->constraintsResolutions[j]->getTolerance()) + { + if(contraintError > c_current_cp->constraintsResolutions[j]->getTolerance()) + constraintsAreVerified = false; + contraintError *= tol / c_current_cp->constraintsResolutions[j]->getTolerance(); + } + + error += contraintError; + tabErrors[j] = contraintError; + + //5. the force is updated for the constraint corrections + bool update = false; + for(unsigned int l=0; l tempF.size()) + { + tempF.resize(nb); + } + std::copy_n(&force[j], nb, tempF.begin()); + for(unsigned int l=0; lcclist_elems[j]) + { + if (el) + el->setConstraintDForce(force, j, j+nb-1, update); + } + + std::copy_n(tempF.begin(), nb, &force[j]); + } + std::advance(it_c, nb); + } + + if(showGraphs) + { + for(int j=0; jgetDimension(); j++) + { + std::ostringstream oss; + oss << "f" << j; + + sofa::type::vector& graph_force = (*graph_forces)[oss.str()]; + graph_force.push_back(force[j]); + + sofa::type::vector& graph_violation = (*graph_violations)[oss.str()]; + graph_violation.push_back(d[j]); + } + + graph_residuals->push_back(error); + } + + if(c_current_cp->sor != 1.0) + { + for(int j=0; jgetDimension(); j++) + force[j] = c_current_cp->sor * force[j] + (1-c_current_cp->sor) * tempForces[j]; + } + if(timeout) + { + SReal t1 = (SReal)sofa::helper::system::thread::CTime::getTime(); + SReal dt = (t1 - t0)*timeScale; + + if(dt > timeout) + { + c_current_cp->currentError = error; + c_current_cp->currentIterations = iter+1; + return; + } + } + else if(c_current_cp->allVerified) + { + if(constraintsAreVerified) + { + convergence = true; + break; + } + } + else if(error < tol) + { + convergence = true; + break; + } + } + + + + sofa::helper::AdvancedTimer::valSet("GS iterations", c_current_cp->currentIterations); + + c_current_cp->result_output(this, force, error, iter, convergence); + + if(showGraphs) + { + d_graphErrors.endEdit(); + + sofa::type::vector& graph_constraints = (*d_graphConstraints.beginEdit())["Constraints"]; + graph_constraints.clear(); + + for(int j=0; jgetDimension(); ) + { + nb = c_current_cp->constraintsResolutions[j]->getNbLines(); + + if(tabErrors[j]) + graph_constraints.push_back(tabErrors[j]); + else if(c_current_cp->constraintsResolutions[j]->getTolerance()) + graph_constraints.push_back(c_current_cp->constraintsResolutions[j]->getTolerance()); + else + graph_constraints.push_back(tol); + + j += nb; + } + d_graphConstraints.endEdit(); + + d_graphForces.endEdit(); + } +} + +void registerUnbuiltGaussSeidelConstraintSolver(sofa::core::ObjectFactory* factory) +{ + factory->registerObjects(core::ObjectRegistrationData("A Constraint Solver using the Linear Complementarity Problem formulation to solve Constraint based components using an Unbuilt version of the Gauss-Seidel iterative method") + .add< UnbuiltGaussSeidelConstraintSolver >()); +} + +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.h new file mode 100644 index 00000000000..8daabb81e3c --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.h @@ -0,0 +1,36 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include + +namespace sofa::component::constraint::lagrangian::solver +{ +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltGaussSeidelConstraintSolver : public UnbuiltConstraintSolver +{ +public: + SOFA_CLASS(UnbuiltGaussSeidelConstraintSolver, UnbuiltConstraintSolver); + + virtual void doSolve(GenericConstraintProblem * problem , SReal timeout = 0.0) override; +}; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/init.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/init.cpp index d814d090dd3..59d9dba7435 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/init.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/init.cpp @@ -26,7 +26,9 @@ namespace sofa::component::constraint::lagrangian::solver { -extern void registerGenericConstraintSolver(sofa::core::ObjectFactory* factory); +extern void registerNNCGConstraintSolver(sofa::core::ObjectFactory* factory); +extern void registerProjectedGaussSeidelConstraintSolver(sofa::core::ObjectFactory* factory); +extern void registerUnbuiltGaussSeidelConstraintSolver(sofa::core::ObjectFactory* factory); extern void registerLCPConstraintSolver(sofa::core::ObjectFactory* factory); extern "C" { @@ -53,7 +55,9 @@ const char* getModuleVersion() void registerObjects(sofa::core::ObjectFactory* factory) { - registerGenericConstraintSolver(factory); + registerNNCGConstraintSolver(factory); + registerProjectedGaussSeidelConstraintSolver(factory); + registerUnbuiltGaussSeidelConstraintSolver(factory); registerLCPConstraintSolver(factory); } diff --git a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/BaseTetrahedronFEMForceField_test.h b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/BaseTetrahedronFEMForceField_test.h index b6a46790cd2..5d1b92953b6 100644 --- a/Sofa/Component/SolidMechanics/FEM/Elastic/tests/BaseTetrahedronFEMForceField_test.h +++ b/Sofa/Component/SolidMechanics/FEM/Elastic/tests/BaseTetrahedronFEMForceField_test.h @@ -137,7 +137,7 @@ class BaseTetrahedronFEMForceField_test : public sofa::testing::BaseTest Sofa.Component.Constraint.Projective }); - simpleapi::createObject(m_root, "GenericConstraintSolver", { {"tolerance", "1e-3"}, {"maxIt", "1000"} }); + simpleapi::createObject(m_root, "ProjectedGaussSeidelConstraintSolver", { {"tolerance", "1e-3"}, {"maxIt", "1000"} }); simpleapi::createObject(m_root, "RegularGridTopology", { {"name", "grid"}, {"n", sofa::simpleapi::str(nbrGrid)}, {"min", "0 0 20"}, {"max", "10 40 30"} }); diff --git a/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp b/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp index a07f6cb8669..313ebc08b5a 100644 --- a/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp +++ b/Sofa/framework/Helper/src/sofa/helper/ComponentChange.cpp @@ -621,6 +621,18 @@ std::map > movedComponents = { std::map > uncreatableComponents = { + /***********************/ + // REMOVED SINCE v25.12 + + { "GenericConstraintSolver", + ComponentChange().withCustomMessage("GenericConstraintSolver has been replaced since v25.12 by new objects, which names relate to the method used:\n" + " - ProjectedGaussSeidel (if you where using this component without setting 'resolutionMethod' or by setting it to 'ProjectedGaussSeidel')\n" + " - UnbuiltGaussSeidel (if you where using this component without setting 'resolutionMethod=\"UnbuiltGaussSeidel\"')\n" + " - NNCG (if you where using this component without setting 'resolutionMethod=\"NonsmoothNonlinearConjugateGradient\"')\n" + " --> For NNCG, data 'newtonIterations' has been replaced by 'maxIterations'" + )}, + + /***********************/ // REMOVED SINCE v25.06 diff --git a/Sofa/framework/Helper/src/sofa/helper/ComponentChange.h b/Sofa/framework/Helper/src/sofa/helper/ComponentChange.h index 6ce6b4f9bf9..7bb005e8f4b 100644 --- a/Sofa/framework/Helper/src/sofa/helper/ComponentChange.h +++ b/Sofa/framework/Helper/src/sofa/helper/ComponentChange.h @@ -47,6 +47,8 @@ class SOFA_HELPER_API ComponentChange std::string m_changeVersion; const std::string& getMessage() const { return m_message; } const std::string& getVersion() const { return m_changeVersion; } + + ComponentChange& withCustomMessage(const std::string& message) { m_message = message; return *this; } }; class SOFA_HELPER_API Deprecated : public ComponentChange diff --git a/Sofa/framework/Simulation/Core/tests/GenericConstraintSolver_test.cpp b/Sofa/framework/Simulation/Core/tests/ProjectedGaussSeidelConstraintSolver_test.cpp similarity index 91% rename from Sofa/framework/Simulation/Core/tests/GenericConstraintSolver_test.cpp rename to Sofa/framework/Simulation/Core/tests/ProjectedGaussSeidelConstraintSolver_test.cpp index ddf25a4cd8e..077892b374b 100644 --- a/Sofa/framework/Simulation/Core/tests/GenericConstraintSolver_test.cpp +++ b/Sofa/framework/Simulation/Core/tests/ProjectedGaussSeidelConstraintSolver_test.cpp @@ -30,7 +30,7 @@ namespace /** Test the UncoupledConstraintCorrection class */ -struct GenericConstraintSolver_test : BaseSimulationTest +struct ProjectedGaussSeidelConstraintSolver_test : BaseSimulationTest { void SetUp() override { @@ -49,7 +49,7 @@ struct GenericConstraintSolver_test : BaseSimulationTest " " " " " \n" - " \n" + " \n" " \n" " \n" " \n" @@ -66,7 +66,7 @@ struct GenericConstraintSolver_test : BaseSimulationTest }; /// run the tests -TEST_F(GenericConstraintSolver_test, checkConstraintForce) +TEST_F(ProjectedGaussSeidelConstraintSolver_test, checkConstraintForce) { EXPECT_MSG_NOEMIT(Error); enableConstraintForce(); diff --git a/applications/plugins/ArticulatedSystemPlugin/examples/ArticulatedArm/header.py b/applications/plugins/ArticulatedSystemPlugin/examples/ArticulatedArm/header.py index 8f64bb6a21c..2df9619b8a8 100644 --- a/applications/plugins/ArticulatedSystemPlugin/examples/ArticulatedArm/header.py +++ b/applications/plugins/ArticulatedSystemPlugin/examples/ArticulatedArm/header.py @@ -6,7 +6,7 @@ def addHeader(rootNode): rootNode.addObject('DefaultVisualManagerLoop') rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('GenericConstraintSolver', maxIterations=50, tolerance=1e-5, printLog=False) + rootNode.addObject('ProjectedGaussSeidelConstraintSolver', maxIterations=50, tolerance=1e-5, printLog=False) rootNode.addObject('BackgroundSetting', color=[1., 1., 1., 1.]) rootNode.findData('dt').value=0.01 rootNode.gravity = [0,-9810,0] diff --git a/applications/plugins/PersistentContact/examples/grasping.scn b/applications/plugins/PersistentContact/examples/grasping.scn index 7f4bab7eb72..2c221faff97 100644 --- a/applications/plugins/PersistentContact/examples/grasping.scn +++ b/applications/plugins/PersistentContact/examples/grasping.scn @@ -2,7 +2,7 @@ - + diff --git a/applications/plugins/Sensable/examples/SimpleBox-Method2.scn b/applications/plugins/Sensable/examples/SimpleBox-Method2.scn index f9d5c362f91..30fa16c5bf9 100644 --- a/applications/plugins/Sensable/examples/SimpleBox-Method2.scn +++ b/applications/plugins/Sensable/examples/SimpleBox-Method2.scn @@ -7,7 +7,7 @@ - + diff --git a/applications/plugins/SofaHAPI/examples/SofaHAPI1.scn b/applications/plugins/SofaHAPI/examples/SofaHAPI1.scn index ef2c5275e58..6d44a4ee7bf 100644 --- a/applications/plugins/SofaHAPI/examples/SofaHAPI1.scn +++ b/applications/plugins/SofaHAPI/examples/SofaHAPI1.scn @@ -7,7 +7,7 @@ - + diff --git a/applications/plugins/SofaMatrix/examples/ComplianceMatrixExporter.scn b/applications/plugins/SofaMatrix/examples/ComplianceMatrixExporter.scn index c25e38f1c05..eeb60a8b0b0 100644 --- a/applications/plugins/SofaMatrix/examples/ComplianceMatrixExporter.scn +++ b/applications/plugins/SofaMatrix/examples/ComplianceMatrixExporter.scn @@ -17,7 +17,7 @@ - + diff --git a/applications/plugins/SofaMatrix/examples/ComplianceMatrixImage.scn b/applications/plugins/SofaMatrix/examples/ComplianceMatrixImage.scn index be9d5abe5b2..660f53115ec 100644 --- a/applications/plugins/SofaMatrix/examples/ComplianceMatrixImage.scn +++ b/applications/plugins/SofaMatrix/examples/ComplianceMatrixImage.scn @@ -17,7 +17,7 @@ - + diff --git a/applications/plugins/Xitact/examples/xitactTest.scn b/applications/plugins/Xitact/examples/xitactTest.scn index 8b9ce1ecf9a..c2680fa0ee5 100644 --- a/applications/plugins/Xitact/examples/xitactTest.scn +++ b/applications/plugins/Xitact/examples/xitactTest.scn @@ -3,7 +3,7 @@ - + diff --git a/applications/projects/SceneChecking/src/SceneChecking/SceneCheckCollisionResponse.cpp b/applications/projects/SceneChecking/src/SceneChecking/SceneCheckCollisionResponse.cpp index 3b3054cbe0a..8c559aee30f 100644 --- a/applications/projects/SceneChecking/src/SceneChecking/SceneCheckCollisionResponse.cpp +++ b/applications/projects/SceneChecking/src/SceneChecking/SceneCheckCollisionResponse.cpp @@ -82,9 +82,9 @@ void SceneCheckCollisionResponse::doCheckOn(Node* node) sofa::core::behavior::ConstraintSolver* constraintSolver; root->get(constraintSolver, sofa::core::objectmodel::BaseContext::SearchRoot); - if (!constraintSolver || ( constraintSolver && ( constraintSolver->getClassName() != "GenericConstraintSolver" )) ) + if (!constraintSolver || ( constraintSolver && ( constraintSolver->getClassName() != "ProjectedGaussSeidelConstraintSolver" )) ) { - m_message <<"A GenericConstraintSolver must be in the scene to solve StickContactConstraint" << msgendl; + m_message <<"A ProjectedGaussSeidelConstraintSolver must be in the scene to solve StickContactConstraint" << msgendl; } } /// If FrictionContactConstraint is chosen, make sure the scene includes a FreeMotionAnimationLoop diff --git a/examples/Benchmark/Performance/TorusFall.scn b/examples/Benchmark/Performance/TorusFall.scn index 6ef6ee21985..1f24dba9312 100644 --- a/examples/Benchmark/Performance/TorusFall.scn +++ b/examples/Benchmark/Performance/TorusFall.scn @@ -30,7 +30,7 @@ - + diff --git a/examples/Component/Collision/Response/RuleBasedContactManager.scn b/examples/Component/Collision/Response/RuleBasedContactManager.scn index f662f4cad79..24ba8ed5c7d 100644 --- a/examples/Component/Collision/Response/RuleBasedContactManager.scn +++ b/examples/Component/Collision/Response/RuleBasedContactManager.scn @@ -20,7 +20,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_NNCG.scn b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_NNCG.scn index 887c5450289..1bd2192b88e 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_NNCG.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_NNCG.scn @@ -22,7 +22,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_PGS.scn b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_PGS.scn index 8b4a059a9f7..bfa2d71c677 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_PGS.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_PGS.scn @@ -22,7 +22,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_Rigid.scn b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_Rigid.scn index ea11b09640e..31f93a6c1e8 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_Rigid.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_Rigid.scn @@ -18,7 +18,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_Soft_Rigid_Bodies.scn b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_Soft_Rigid_Bodies.scn index 16d94bdda46..b8f8f81479b 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_Soft_Rigid_Bodies.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_Soft_Rigid_Bodies.scn @@ -28,7 +28,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_UGS.scn b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_UGS.scn index 4825af99e4f..44ede0e31d6 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_UGS.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_UGS.scn @@ -22,7 +22,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_regularization_solvable.scn b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_regularization_solvable.scn index a6041cc23b3..2e10347f367 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_regularization_solvable.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_regularization_solvable.scn @@ -17,7 +17,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_regularization_unsolvable.scn b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_regularization_unsolvable.scn index 62402d07a99..038917c14ae 100644 --- a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_regularization_unsolvable.scn +++ b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_regularization_unsolvable.scn @@ -17,7 +17,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Rigid3.scn b/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Rigid3.scn index 4cfe48d6334..adef70eaade 100644 --- a/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Rigid3.scn +++ b/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Rigid3.scn @@ -21,7 +21,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Vec3.scn b/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Vec3.scn index 91340fdf114..2aaf4554dac 100644 --- a/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Vec3.scn +++ b/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Vec3.scn @@ -24,7 +24,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/FrictionContact_VelocityConstraints.scn b/examples/Component/Constraint/Lagrangian/FrictionContact_VelocityConstraints.scn index ecfb237bb17..2132cb08012 100644 --- a/examples/Component/Constraint/Lagrangian/FrictionContact_VelocityConstraints.scn +++ b/examples/Component/Constraint/Lagrangian/FrictionContact_VelocityConstraints.scn @@ -16,7 +16,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/InextensiblePendulum.scn b/examples/Component/Constraint/Lagrangian/InextensiblePendulum.scn index 8d795223efb..836ad3d44a4 100644 --- a/examples/Component/Constraint/Lagrangian/InextensiblePendulum.scn +++ b/examples/Component/Constraint/Lagrangian/InextensiblePendulum.scn @@ -23,7 +23,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/SlidingLagrangianConstraint.scn b/examples/Component/Constraint/Lagrangian/SlidingLagrangianConstraint.scn index 242a47486c5..c89c3cd609b 100644 --- a/examples/Component/Constraint/Lagrangian/SlidingLagrangianConstraint.scn +++ b/examples/Component/Constraint/Lagrangian/SlidingLagrangianConstraint.scn @@ -20,7 +20,7 @@ - + diff --git a/examples/Component/Constraint/Projective/LinearVelocityProjectiveConstraint.scn b/examples/Component/Constraint/Projective/LinearVelocityProjectiveConstraint.scn index 6f5924d34f4..cdfef263603 100644 --- a/examples/Component/Constraint/Projective/LinearVelocityProjectiveConstraint.scn +++ b/examples/Component/Constraint/Projective/LinearVelocityProjectiveConstraint.scn @@ -23,7 +23,7 @@ - + diff --git a/examples/Component/Engine/Select/NearestPointROI.scn b/examples/Component/Engine/Select/NearestPointROI.scn index dbdc142c607..1946e612341 100644 --- a/examples/Component/Engine/Select/NearestPointROI.scn +++ b/examples/Component/Engine/Select/NearestPointROI.scn @@ -21,7 +21,7 @@ - + - + diff --git a/examples/Demos/SofaWasher.scn b/examples/Demos/SofaWasher.scn index ffb1eb3d0f8..e7f01600304 100644 --- a/examples/Demos/SofaWasher.scn +++ b/examples/Demos/SofaWasher.scn @@ -32,7 +32,7 @@ - + diff --git a/examples/Demos/fallingBeamAugmentedLagrangianCollision.scn b/examples/Demos/fallingBeamAugmentedLagrangianCollision.scn index 173e6d836df..324247538c5 100644 --- a/examples/Demos/fallingBeamAugmentedLagrangianCollision.scn +++ b/examples/Demos/fallingBeamAugmentedLagrangianCollision.scn @@ -27,7 +27,7 @@ - + diff --git a/examples/Demos/fallingBeamLagrangianCollision.scn b/examples/Demos/fallingBeamLagrangianCollision.scn index 4de55435c38..29e7059ba44 100644 --- a/examples/Demos/fallingBeamLagrangianCollision.scn +++ b/examples/Demos/fallingBeamLagrangianCollision.scn @@ -27,7 +27,7 @@ - + diff --git a/examples/Demos/fallingSOFA.scn b/examples/Demos/fallingSOFA.scn index f90a5642226..978637fd861 100644 --- a/examples/Demos/fallingSOFA.scn +++ b/examples/Demos/fallingSOFA.scn @@ -38,7 +38,7 @@ - + diff --git a/examples/Demos/sofa_1000PR.scn b/examples/Demos/sofa_1000PR.scn index 5d75b340267..7c44da2aa24 100644 --- a/examples/Demos/sofa_1000PR.scn +++ b/examples/Demos/sofa_1000PR.scn @@ -24,7 +24,7 @@ - + diff --git a/examples/SimpleAPI/fallingSOFA.cpp b/examples/SimpleAPI/fallingSOFA.cpp index 324f69ca5d6..4425463eab6 100644 --- a/examples/SimpleAPI/fallingSOFA.cpp +++ b/examples/SimpleAPI/fallingSOFA.cpp @@ -49,7 +49,7 @@ sofa::simulation::Node::SPtr createScene(const sofa::simpleapi::Simulation::SPtr sofa::simpleapi::createObject(root, "VisualStyle", {{"displayFlags", "showVisual"}}); sofa::simpleapi::createObject(root, "ConstraintAttachButtonSetting"); sofa::simpleapi::createObject(root, "FreeMotionAnimationLoop"); - sofa::simpleapi::createObject(root, "GenericConstraintSolver",{{"maxIterations","50"}, {"tolerance","1.0e-6"}}); + sofa::simpleapi::createObject(root, "ProjectedGaussSeidelConstraintSolver",{{"maxIterations","50"}, {"tolerance","1.0e-6"}}); sofa::simpleapi::createObject(root, "CollisionPipeline",{{"name","Pipeline"}}); sofa::simpleapi::createObject(root, "ParallelBruteForceBroadPhase",{{"name","BroadPhase"}});