From ad0a832aa8fe4da32bd365d33641c6ba535020c1 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Mon, 25 Aug 2025 17:38:10 +0200 Subject: [PATCH 01/10] First working version of exploded constraint problrms. Still lots of info in GenericCosntraintSolver data that are dependent on the method type. Need to find a way of setting the solving method by adding a component in the scene. --- .../Lagrangian/Solver/CMakeLists.txt | 11 + .../solver/BuiltConstraintProblem.cpp | 104 +++ .../solver/BuiltConstraintProblem.h | 54 ++ .../lagrangian/solver/ConstraintSolverImpl.h | 7 +- .../solver/GenericConstraintProblem.cpp | 648 +----------------- .../solver/GenericConstraintProblem.h | 57 +- .../solver/GenericConstraintSolver.cpp | 241 ++----- .../solver/GenericConstraintSolver.h | 26 +- .../solver/NNCGConstraintProblem.cpp | 167 +++++ .../lagrangian/solver/NNCGConstraintProblem.h | 36 + .../ProjectedGaussSeidelConstraintProblem.cpp | 277 ++++++++ .../ProjectedGaussSeidelConstraintProblem.h | 39 ++ .../solver/UnbuiltConstraintProblem.cpp | 87 +++ .../solver/UnbuiltConstraintProblem.h | 41 ++ .../UnbuiltGaussSeidelConstraintProblem.cpp | 287 ++++++++ .../UnbuiltGaussSeidelConstraintProblem.h | 36 + 16 files changed, 1226 insertions(+), 892 deletions(-) create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.cpp create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.h create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintProblem.cpp create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintProblem.h create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.cpp create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.h create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.cpp create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.h create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.cpp create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.h diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt b/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt index 0cf052b1d98..de46170f58c 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt +++ b/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt @@ -7,7 +7,13 @@ set(HEADER_FILES ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/config.h.in ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/init.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ConstraintSolverImpl.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintProblem.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintProblem.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintProblem.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintProblem.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintProblem.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltGaussSeidelConstraintProblem.h + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/LCPConstraintSolver.h @@ -19,7 +25,12 @@ set(HEADER_FILES set(SOURCE_FILES ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/init.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ConstraintSolverImpl.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintProblem.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintProblem.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintProblem.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintProblem.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintProblem.cpp + ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltGaussSeidelConstraintProblem.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/LCPConstraintSolver.cpp diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.cpp new file mode 100644 index 00000000000..f3d0100a7b0 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.cpp @@ -0,0 +1,104 @@ +/****************************************************************************** +* 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 BuiltConstraintProblem::buildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints, GenericConstraintSolver* solver ) + { + SOFA_UNUSED(numConstraints); + SCOPED_TIMER_VARNAME(getComplianceTimer, "Get Compliance"); + dmsg_info() <<" computeCompliance in " << solver->l_constraintCorrections.size()<< " constraintCorrections" ; + + const bool multithreading = solver->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, solver->l_constraintCorrections.begin(), solver->l_constraintCorrections.end(), + [&cParams, this, &multithreading, &mutex](const auto& range) + { + ComplianceWrapper compliance(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(W, solver->d_regularizationTerm.getValue()); + dmsg_info() << " computeCompliance_done " ; + } + + + BuiltConstraintProblem::ComplianceWrapper::ComplianceMatrixType& BuiltConstraintProblem::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 BuiltConstraintProblem::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/BuiltConstraintProblem.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.h new file mode 100644 index 00000000000..9970305970e --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.h @@ -0,0 +1,54 @@ +/****************************************************************************** +* 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 +{ +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintProblem : public GenericConstraintProblem +{ +public: + SOFA_CLASS(BuiltConstraintProblem, GenericConstraintProblem); + + virtual void buildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints, GenericConstraintSolver* solver = nullptr) 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.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h index 7eac329bc70..53ad96716b5 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 @@ -93,6 +93,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 +104,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..692f8126d92 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,645 +74,12 @@ void GenericConstraintProblem::solveTimed(SReal tol, int maxIt, SReal timeout) tolerance = tol; maxIterations = maxIt; - gaussSeidel(timeout); + solve(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) -{ - 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; - } -} void GenericConstraintProblem::result_output(GenericConstraintSolver *solver, SReal *force, SReal error, int iterCount, bool convergence) { @@ -736,4 +103,17 @@ void GenericConstraintProblem::result_output(GenericConstraintSolver *solver, SR } } + +void GenericConstraintProblem::addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization) +{ + if (regularization>std::numeric_limits::epsilon()) + { + for (int i=0; i _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; + SOFA_CLASS(GenericConstraintProblem, BaseObject); typedef std::vector< core::behavior::BaseConstraintCorrection* > ConstraintCorrections; - typedef std::vector< core::behavior::BaseConstraintCorrection* >::iterator ConstraintCorrectionIterator; - - std::vector< ConstraintCorrections > cclist_elems; + GenericConstraintProblem() + : scaleTolerance(true) + , allVerified(false) + , sor(1.0) + , currentError(0.0) + , currentIterations(0) + {} - 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); + virtual void buildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints, GenericConstraintSolver* solver = nullptr) = 0; + virtual void solve( SReal timeout = 0.0, GenericConstraintSolver* solver = nullptr) = 0; - 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); + static void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization); - int getNumConstraints(); - int getNumConstraintGroups(); + sofa::linearalgebra::FullVector _d; // + std::vector constraintsResolutions; // + bool scaleTolerance, allVerified; // + SReal sor; /** GAUSS-SEIDEL **/ + SReal currentError; // + int currentIterations; // protected: sofa::linearalgebra::FullVector m_lam; 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..c3d4e0ff259 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 @@ -40,6 +40,12 @@ using sofa::simulation::mechanicalvisitor::MechanicalVOpVisitor; #include using sofa::simulation::mechanicalvisitor::MechanicalProjectJacobianMatrixVisitor; + +#include +#include +#include + + namespace sofa::component::constraint::lagrangian::solver { @@ -63,14 +69,12 @@ 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 the Gauss-Seidel 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")) @@ -86,7 +90,6 @@ GenericConstraintSolver::GenericConstraintSolver() , 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"); @@ -114,12 +117,35 @@ GenericConstraintSolver::GenericConstraintSolver() d_maxIt.setRequired(true); d_tolerance.setRequired(true); + + + for (unsigned i=0; i< CP_BUFFER_SIZE; ++i) + { + switch ( d_resolutionMethod.getValue()) + { + case ResolutionMethod("ProjectedGaussSeidel"): { + m_cpBuffer[i] = new ProjectedGaussSeidelConstraintProblem; + break; + } + case ResolutionMethod("UnbuiltGaussSeidel"): { + m_cpBuffer[i] = new UnbuiltGaussSeidelConstraintProblem; + break; + } + case ResolutionMethod("NonsmoothNonlinearConjugateGradient"): { + m_cpBuffer[i] = new NNCGConstraintProblem; + break; + } + } + } + current_cp = m_cpBuffer[0]; + + } GenericConstraintSolver::~GenericConstraintSolver() {} -void GenericConstraintSolver::init() +void GenericConstraintSolver:: init() { ConstraintSolverImpl::init(); @@ -140,14 +166,6 @@ void GenericConstraintSolver::init() 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"; - } - } } void GenericConstraintSolver::cleanup() @@ -209,170 +227,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; -} - -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; -} + current_cp->buildSystem(cParams, numConstraints, this); -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 +289,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() ; } + current_cp->solve(0, this); + this->d_currentError.setValue(current_cp->currentError); this->d_currentIterations.setValue(current_cp->currentIterations); @@ -591,7 +432,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; diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h index 09ccb277cbd..59e2c81c109 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h @@ -53,20 +53,19 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : bool prepareStates(const core::ConstraintParams * /*cParams*/, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; bool buildSystem(const core::ConstraintParams * /*cParams*/, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; - void rebuildSystem(SReal massFactor, SReal forceFactor) override; - void addRegularization(linearalgebra::BaseMatrix & W); + void rebuildSystem(const SReal massFactor, const SReal forceFactor) override; bool solveSystem(const core::ConstraintParams * /*cParams*/, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; bool applyCorrection(const core::ConstraintParams * /*cParams*/, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override; void computeResidual(const core::ExecParams* /*params*/) override; ConstraintProblem* getConstraintProblem() override; void lockConstraintProblem(sofa::core::objectmodel::BaseObject* from, ConstraintProblem* p1, ConstraintProblem* p2 = nullptr) override; + MAKE_SELECTABLE_ITEMS(ResolutionMethod, sofa::helper::Item{"ProjectedGaussSeidel", "Projected Gauss-Seidel"}, sofa::helper::Item{"UnbuiltGaussSeidel", "Gauss-Seidel where the matrix is not assembled"}, sofa::helper::Item{"NonsmoothNonlinearConjugateGradient", "Non-smooth non-linear conjugate gradient"} ); - Data< ResolutionMethod > 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 @@ -75,7 +74,6 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : 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 @@ -99,7 +97,7 @@ 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; @@ -113,24 +111,6 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : 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/NNCGConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintProblem.cpp new file mode 100644 index 00000000000..5889941a682 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintProblem.cpp @@ -0,0 +1,167 @@ +/****************************************************************************** +* 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 NNCGConstraintProblem::solve( SReal timeout, GenericConstraintSolver* solver) +{ + SCOPED_TIMER_VARNAME(unbuiltGaussSeidelTimer, "NonsmoothNonlinearConjugateGradient"); + + 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; jd_maxIt.getValue(); i++) + { + iterCount ++; + bool constraintsAreVerified = true; + + for(int j=0; j 1) + { + m_p.clear(); + m_p.resize(dimension); + } + else + { + for(int j=0; j. * +******************************************************************************* +* 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 NNCGConstraintProblem : public ProjectedGaussSeidelConstraintProblem +{ +public: + SOFA_CLASS(NNCGConstraintProblem, ProjectedGaussSeidelConstraintProblem); + + virtual void solve( SReal timeout = 0.0, GenericConstraintSolver* solver = nullptr); +}; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.cpp new file mode 100644 index 00000000000..0d62d7c4141 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.cpp @@ -0,0 +1,277 @@ +/****************************************************************************** +* 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 ProjectedGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstraintSolver* solver) +{ + SCOPED_TIMER_VARNAME(gaussSeidelTimer, "ConstraintsGaussSeidel"); + + 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 ProjectedGaussSeidelConstraintProblem::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 +{ + 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; + } +} + +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.h new file mode 100644 index 00000000000..ef2f289bb16 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.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 ProjectedGaussSeidelConstraintProblem : public BuiltConstraintProblem +{ +public: + SOFA_CLASS(ProjectedGaussSeidelConstraintProblem, BuiltConstraintProblem); + + virtual void solve( SReal timeout = 0.0, GenericConstraintSolver* solver = nullptr); + + 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; + +}; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.cpp new file mode 100644 index 00000000000..d5ece473b2f --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.cpp @@ -0,0 +1,87 @@ +/****************************************************************************** +* 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 + +namespace sofa::component::constraint::lagrangian::solver +{ + +void UnbuiltConstraintProblem::buildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints, GenericConstraintSolver* solver ) +{ + for (const auto& cc : solver -> l_constraintCorrections) + { + if (!cc->isActive()) continue; + + constraints_sequence.resize(numConstraints); + std::iota(constraints_sequence.begin(), constraints_sequence.end(), 0); + + // some constraint corrections (e.g LinearSolverConstraintCorrection) + // can change the order of the constraints, to optimize later computations + cc->resetForUnbuiltResolution(getF(), constraints_sequence); + } + + sofa::linearalgebra::SparseMatrix* localWdiag = &Wdiag; + localWdiag->resize(numConstraints, numConstraints); + + // for each contact, the constraint corrections that are involved with the contact are memorized + cclist_elems.clear(); + cclist_elems.resize(numConstraints); + const int nbCC = solver->l_constraintCorrections.size(); + for (unsigned int i = 0; i < numConstraints; i++) + 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 = constraintsResolutions[c_id]->getNbLines(); + + for (unsigned int j = 0; j < solver->l_constraintCorrections.size(); j++) + { + core::behavior::BaseConstraintCorrection* cc = solver->l_constraintCorrections[j]; + if (!cc->isActive()) continue; + if (cc->hasConstraintNumber(c_id)) + { + 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 = 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(W, solver->d_regularizationTerm.getValue()); + addRegularization(Wdiag, solver->d_regularizationTerm.getValue()); + +} + +} \ 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..8438870d316 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.h @@ -0,0 +1,41 @@ +/****************************************************************************** +* 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 UnbuiltConstraintProblem : public GenericConstraintProblem +{ +public: + SOFA_CLASS(UnbuiltConstraintProblem, GenericConstraintProblem); + + virtual void buildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints, GenericConstraintSolver* solver = nullptr) override; + +protected: + linearalgebra::SparseMatrix Wdiag; /** UNBUILT **/ + std::list constraints_sequence; /** UNBUILT **/ + std::vector< ConstraintCorrections > cclist_elems; /** UNBUILT **/ +}; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.cpp new file mode 100644 index 00000000000..a9499871d0a --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.cpp @@ -0,0 +1,287 @@ +/****************************************************************************** +* 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 UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstraintSolver* solver) +{ + SCOPED_TIMER_VARNAME(unbuiltGaussSeidelTimer, "ConstraintsUnbuiltGaussSeidel"); + + if(!solver) + return; + + if(!this->dimension) + { + this->currentError = 0.0; + this->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(); + } +} + +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.h new file mode 100644 index 00000000000..75c4d25c421 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.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 UnbuiltGaussSeidelConstraintProblem : public UnbuiltConstraintProblem +{ +public: + SOFA_CLASS(UnbuiltGaussSeidelConstraintProblem, UnbuiltConstraintProblem); + + virtual void solve( SReal timeout = 0.0, GenericConstraintSolver* solver = nullptr); +}; +} \ No newline at end of file From ad5fe25c701e47847d1d3fa4b9ae0458ae614d38 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Tue, 26 Aug 2025 11:26:36 +0200 Subject: [PATCH 02/10] Changed all problems into solvers. It makes more sense to use an inheritance. Now the constraintProblem is only a container --- .../animationloop/FreeMotionAnimationLoop.cpp | 4 +- .../Lagrangian/Solver/CMakeLists.txt | 21 +-- ...tProblem.cpp => BuiltConstraintSolver.cpp} | 19 ++- ...raintProblem.h => BuiltConstraintSolver.h} | 9 +- .../lagrangian/solver/ConstraintSolverImpl.h | 3 +- .../solver/GenericConstraintProblem.cpp | 16 +-- .../solver/GenericConstraintProblem.h | 15 +-- .../solver/GenericConstraintSolver.cpp | 58 ++++----- .../solver/GenericConstraintSolver.h | 11 +- ...ntProblem.cpp => NNCGConstraintSolver.cpp} | 85 ++++++------ ...traintProblem.h => NNCGConstraintSolver.h} | 8 +- ... ProjectedGaussSeidelConstraintSolver.cpp} | 97 +++++++------- ...=> ProjectedGaussSeidelConstraintSolver.h} | 8 +- .../solver/UnbuiltConstraintProblem.h | 18 ++- ...roblem.cpp => UnbuiltConstraintSolver.cpp} | 52 +++++--- .../solver/UnbuiltConstraintSolver.h | 39 ++++++ ...=> UnbuiltGaussSeidelConstraintSolver.cpp} | 123 ++++++++++-------- ...h => UnbuiltGaussSeidelConstraintSolver.h} | 8 +- .../constraint/lagrangian/solver/init.cpp | 8 +- 19 files changed, 344 insertions(+), 258 deletions(-) rename Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/{BuiltConstraintProblem.cpp => BuiltConstraintSolver.cpp} (81%) rename Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/{BuiltConstraintProblem.h => BuiltConstraintSolver.h} (89%) rename Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/{NNCGConstraintProblem.cpp => NNCGConstraintSolver.cpp} (60%) rename Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/{NNCGConstraintProblem.h => NNCGConstraintSolver.h} (88%) rename Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/{ProjectedGaussSeidelConstraintProblem.cpp => ProjectedGaussSeidelConstraintSolver.cpp} (69%) rename Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/{ProjectedGaussSeidelConstraintProblem.h => ProjectedGaussSeidelConstraintSolver.h} (89%) rename Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/{UnbuiltConstraintProblem.cpp => UnbuiltConstraintSolver.cpp} (63%) create mode 100644 Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.h rename Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/{UnbuiltGaussSeidelConstraintProblem.cpp => UnbuiltGaussSeidelConstraintSolver.cpp} (62%) rename Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/{UnbuiltGaussSeidelConstraintProblem.h => UnbuiltGaussSeidelConstraintSolver.h} (88%) diff --git a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp index d1571e8182c..2a9ee148d61 100644 --- a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp +++ b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp @@ -46,6 +46,8 @@ #include #include + +#include "sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h" using sofa::simulation::mechanicalvisitor::MechanicalVInitVisitor; #include @@ -100,7 +102,7 @@ void FreeMotionAnimationLoop::init() l_constraintSolver.set(this->getContext()->get(core::objectmodel::BaseContext::SearchDown)); if (!l_constraintSolver) { - if (const auto constraintSolver = sofa::core::objectmodel::New()) + if (const auto constraintSolver = sofa::core::objectmodel::New()) { getContext()->addObject(constraintSolver); constraintSolver->setName( this->getContext()->getNameHelper().resolveName(constraintSolver->getClassName(), {})); diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt b/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt index de46170f58c..66634a1927f 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt +++ b/Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt @@ -7,14 +7,14 @@ set(HEADER_FILES ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/config.h.in ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/init.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ConstraintSolverImpl.h - ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintProblem.h ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintProblem.h - ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintProblem.h - ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintProblem.h - ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintProblem.h - ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltGaussSeidelConstraintProblem.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 @@ -25,13 +25,14 @@ set(HEADER_FILES set(SOURCE_FILES ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/init.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ConstraintSolverImpl.cpp - ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintProblem.cpp ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintProblem.cpp - ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintProblem.cpp - ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintProblem.cpp - ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintProblem.cpp - ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltGaussSeidelConstraintProblem.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/BuiltConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp similarity index 81% rename from Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.cpp rename to Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp index f3d0100a7b0..306ae5d7480 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp @@ -20,8 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#include -#include +#include #include #include @@ -30,13 +29,13 @@ namespace sofa::component::constraint::lagrangian::solver { - void BuiltConstraintProblem::buildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints, GenericConstraintSolver* solver ) + void BuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) { SOFA_UNUSED(numConstraints); SCOPED_TIMER_VARNAME(getComplianceTimer, "Get Compliance"); - dmsg_info() <<" computeCompliance in " << solver->l_constraintCorrections.size()<< " constraintCorrections" ; + dmsg_info() <<" computeCompliance in " << l_constraintCorrections.size()<< " constraintCorrections" ; - const bool multithreading = solver->d_multithreading.getValue(); + const bool multithreading = d_multithreading.getValue(); const simulation::ForEachExecutionPolicy execution = multithreading ? simulation::ForEachExecutionPolicy::PARALLEL : @@ -50,10 +49,10 @@ namespace sofa::component::constraint::lagrangian::solver //Visits all constraint corrections to compute the compliance matrix projected //in the constraint space. - simulation::forEachRange(execution, *taskScheduler, solver->l_constraintCorrections.begin(), solver->l_constraintCorrections.end(), + simulation::forEachRange(execution, *taskScheduler, l_constraintCorrections.begin(), l_constraintCorrections.end(), [&cParams, this, &multithreading, &mutex](const auto& range) { - ComplianceWrapper compliance(W, multithreading); + ComplianceWrapper compliance(current_cp->W, multithreading); for (auto it = range.start; it != range.end; ++it) { @@ -68,12 +67,12 @@ namespace sofa::component::constraint::lagrangian::solver compliance.assembleMatrix(); }); - addRegularization(W, solver->d_regularizationTerm.getValue()); + addRegularization(current_cp->W, d_regularizationTerm.getValue()); dmsg_info() << " computeCompliance_done " ; } - BuiltConstraintProblem::ComplianceWrapper::ComplianceMatrixType& BuiltConstraintProblem::ComplianceWrapper::matrix() + BuiltConstraintSolver::ComplianceWrapper::ComplianceMatrixType& BuiltConstraintSolver::ComplianceWrapper::matrix() { if (m_isMultiThreaded) { @@ -87,7 +86,7 @@ namespace sofa::component::constraint::lagrangian::solver return m_complianceMatrix; } - void BuiltConstraintProblem::ComplianceWrapper::assembleMatrix() const + void BuiltConstraintSolver::ComplianceWrapper::assembleMatrix() const { if (m_threadMatrix) { diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h similarity index 89% rename from Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.h rename to Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h index 9970305970e..612a7e6cfc7 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintProblem.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h @@ -21,16 +21,17 @@ ******************************************************************************/ #pragma once -#include +#include namespace sofa::component::constraint::lagrangian::solver { -class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintProblem : public GenericConstraintProblem +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintSolver : public GenericConstraintSolver { public: - SOFA_CLASS(BuiltConstraintProblem, GenericConstraintProblem); + SOFA_CLASS(BuiltConstraintSolver, GenericConstraintSolver); - virtual void buildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints, GenericConstraintSolver* solver = nullptr) override; + + virtual void doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) override; private: 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 53ad96716b5..9db0ad19abf 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 @@ -35,7 +35,7 @@ namespace sofa::component::constraint::lagrangian::solver { -class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem { public: // The compliance matrix projected in the constraint space @@ -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(); } 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 692f8126d92..101504f6378 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,12 +74,16 @@ void GenericConstraintProblem::solveTimed(SReal tol, int maxIt, SReal timeout) tolerance = tol; maxIterations = maxIt; - solve(timeout); + m_solver->doSolve(timeout); tolerance = tempTol; maxIterations = tempMaxIt; } +void GenericConstraintProblem::setSolver(GenericConstraintSolver* solver) +{ + m_solver = solver; +} void GenericConstraintProblem::result_output(GenericConstraintSolver *solver, SReal *force, SReal error, int iterCount, bool convergence) { @@ -104,16 +108,6 @@ void GenericConstraintProblem::result_output(GenericConstraintSolver *solver, SR } -void GenericConstraintProblem::addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization) -{ - if (regularization>std::numeric_limits::epsilon()) - { - for (int i=0; i ConstraintCorrections; - GenericConstraintProblem() + GenericConstraintProblem(GenericConstraintSolver* solver) : scaleTolerance(true) , allVerified(false) , sor(1.0) , currentError(0.0) , currentIterations(0) + , m_solver(solver) {} ~GenericConstraintProblem() override @@ -57,10 +57,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintProblem : void result_output(GenericConstraintSolver* solver, SReal *force, SReal error, int iterCount, bool convergence); void solveTimed(SReal tol, int maxIt, SReal timeout) override; - virtual void buildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints, GenericConstraintSolver* solver = nullptr) = 0; - virtual void solve( SReal timeout = 0.0, GenericConstraintSolver* solver = nullptr) = 0; - - static void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization); + void setSolver(GenericConstraintSolver* solver); sofa::linearalgebra::FullVector _d; // std::vector constraintsResolutions; // @@ -69,11 +66,13 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintProblem : 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 c3d4e0ff259..20dd0ea9e92 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 @@ -41,9 +40,6 @@ using sofa::simulation::mechanicalvisitor::MechanicalVOpVisitor; using sofa::simulation::mechanicalvisitor::MechanicalProjectJacobianMatrixVisitor; -#include -#include -#include namespace sofa::component::constraint::lagrangian::solver @@ -117,36 +113,16 @@ GenericConstraintSolver::GenericConstraintSolver() d_maxIt.setRequired(true); d_tolerance.setRequired(true); - - - for (unsigned i=0; i< CP_BUFFER_SIZE; ++i) - { - switch ( d_resolutionMethod.getValue()) - { - case ResolutionMethod("ProjectedGaussSeidel"): { - m_cpBuffer[i] = new ProjectedGaussSeidelConstraintProblem; - break; - } - case ResolutionMethod("UnbuiltGaussSeidel"): { - m_cpBuffer[i] = new UnbuiltGaussSeidelConstraintProblem; - break; - } - case ResolutionMethod("NonsmoothNonlinearConjugateGradient"): { - m_cpBuffer[i] = new NNCGConstraintProblem; - break; - } - } - } - current_cp = m_cpBuffer[0]; - - } GenericConstraintSolver::~GenericConstraintSolver() {} + + void GenericConstraintSolver:: init() { + this->initializeConstraintProblems(); ConstraintSolverImpl::init(); simulation::common::VectorOperations vop(sofa::core::execparams::defaultInstance(), this->getContext()); @@ -166,6 +142,16 @@ void GenericConstraintSolver:: init() simulation::MainTaskSchedulerFactory::createInRegistry()->init(); } + +} + +void GenericConstraintSolver::initializeConstraintProblems() +{ + for (unsigned i=0; i< CP_BUFFER_SIZE; ++i) + { + m_cpBuffer[i] = new GenericConstraintProblem(this); + } + current_cp = m_cpBuffer[0]; } void GenericConstraintSolver::cleanup() @@ -228,7 +214,7 @@ bool GenericConstraintSolver::buildSystem(const core::ConstraintParams *cParams, } - current_cp->buildSystem(cParams, numConstraints, this); + this->doBuildSystem(cParams, numConstraints); return true; } @@ -298,7 +284,7 @@ bool GenericConstraintSolver::solveSystem(const core::ConstraintParams * /*cPara msg_info() << tmp.str() ; } - current_cp->solve(0, this); + this->doSolve(0.0); this->d_currentError.setValue(current_cp->currentError); @@ -457,10 +443,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 +#include #include #include +#include namespace sofa::component::constraint::lagrangian::solver { -void NNCGConstraintProblem::solve( SReal timeout, GenericConstraintSolver* solver) +void NNCGConstraintSolver::doSolve( SReal timeout) { SCOPED_TIMER_VARNAME(unbuiltGaussSeidelTimer, "NonsmoothNonlinearConjugateGradient"); - if(!solver) - return; - const int dimension = getDimension(); + const int dimension = current_cp->getDimension(); if(!dimension) { - currentError = 0.0; - currentIterations = 0; + current_cp->currentError = 0.0; + current_cp->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); + current_cp->m_lam.clear(); + current_cp->m_lam.resize(dimension); + current_cp->m_deltaF.clear(); + current_cp->m_deltaF.resize(dimension); + current_cp->m_deltaF_new.clear(); + current_cp->m_deltaF_new.resize(dimension); + current_cp->m_p.clear(); + current_cp->m_p.resize(dimension); - SReal *dfree = getDfree(); - SReal *force = getF(); - SReal **w = getW(); - SReal tol = tolerance; + SReal *dfree = current_cp->getDfree(); + SReal *force = current_cp->getF(); + SReal **w = current_cp->getW(); + SReal tol = current_cp->tolerance; - SReal *d = _d.ptr(); + SReal *d = current_cp->_d.ptr(); SReal error = 0.0; bool convergence = false; sofa::type::vector tempForces; - if(sor != 1.0) + if(current_cp->sor != 1.0) { tempForces.resize(dimension); } - if(scaleTolerance && !allVerified) + if(current_cp->scaleTolerance && !current_cp->allVerified) { tol *= dimension; } for(int i=0; iconstraintsResolutions[i]) { - msg_error(solver) << "Bad size of constraintsResolutions in GenericConstraintProblem" ; + msg_error() << "Bad size of constraintsResolutions in GenericConstraintSolver" ; break; } - constraintsResolutions[i]->init(i, w, force); - i += constraintsResolutions[i]->getNbLines(); + current_cp->constraintsResolutions[i]->init(i, w, force); + i += current_cp->constraintsResolutions[i]->getNbLines(); } sofa::type::vector tabErrors(dimension); @@ -92,14 +91,14 @@ void NNCGConstraintProblem::solve( SReal timeout, GenericConstraintSolver* solve { // perform one iteration of ProjectedGaussSeidel bool constraintsAreVerified = true; - std::copy_n(force, dimension, std::begin(m_lam)); + std::copy_n(force, dimension, std::begin(current_cp->m_lam)); gaussSeidel_increment(false, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, tabErrors); for(int j=0; jm_deltaF[j] = -(force[j] - current_cp->m_lam[j]); + current_cp->m_p[j] = - current_cp->m_deltaF[j]; } } @@ -107,14 +106,14 @@ void NNCGConstraintProblem::solve( SReal timeout, GenericConstraintSolver* solve int iterCount = 0; - for(int i=1; id_maxIt.getValue(); i++) + for(int i=1; im_lam[j] = force[j]; } @@ -122,7 +121,7 @@ void NNCGConstraintProblem::solve( SReal timeout, GenericConstraintSolver* solve gaussSeidel_increment(true, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, tabErrors); - if(allVerified) + if(current_cp->allVerified) { if(constraintsAreVerified) { @@ -140,28 +139,34 @@ void NNCGConstraintProblem::solve( SReal timeout, GenericConstraintSolver* solve // NNCG update with the correction p for(int j=0; jm_deltaF_new[j] = -(force[j] - current_cp->m_lam[j]); } - const SReal beta = m_deltaF_new.dot(m_deltaF_new) / m_deltaF.dot(m_deltaF); - m_deltaF.eq(m_deltaF_new, 1); + const SReal beta = current_cp->m_deltaF_new.dot(current_cp->m_deltaF_new) / current_cp->m_deltaF.dot(current_cp->m_deltaF); + current_cp->m_deltaF.eq(current_cp->m_deltaF_new, 1); if(beta > 1) { - m_p.clear(); - m_p.resize(dimension); + current_cp->m_p.clear(); + current_cp->m_p.resize(dimension); } else { for(int j=0; jm_p[j]; + current_cp->m_p[j] = beta*current_cp->m_p[j] -current_cp-> m_deltaF[j]; } } } - result_output(solver, force, error, iterCount, convergence); + current_cp->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/NNCGConstraintProblem.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.h similarity index 88% rename from Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintProblem.h rename to Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.h index 1275f2153e5..e97baff1a3f 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintProblem.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/NNCGConstraintSolver.h @@ -21,16 +21,16 @@ ******************************************************************************/ #pragma once -#include +#include #include namespace sofa::component::constraint::lagrangian::solver { -class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API NNCGConstraintProblem : public ProjectedGaussSeidelConstraintProblem +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API NNCGConstraintSolver : public ProjectedGaussSeidelConstraintSolver { public: - SOFA_CLASS(NNCGConstraintProblem, ProjectedGaussSeidelConstraintProblem); + SOFA_CLASS(NNCGConstraintSolver, ProjectedGaussSeidelConstraintSolver); - virtual void solve( SReal timeout = 0.0, GenericConstraintSolver* solver = nullptr); + virtual void doSolve( 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/ProjectedGaussSeidelConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.cpp similarity index 69% rename from Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.cpp rename to Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.cpp index 0d62d7c4141..55fe5b02515 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.cpp @@ -20,79 +20,78 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#include +#include #include #include #include +#include namespace sofa::component::constraint::lagrangian::solver { -void ProjectedGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstraintSolver* solver) +void ProjectedGaussSeidelConstraintSolver::doSolve( SReal timeout) { SCOPED_TIMER_VARNAME(gaussSeidelTimer, "ConstraintsGaussSeidel"); - if(!solver) - return; - const int dimension = getDimension(); + const int dimension = current_cp->getDimension(); if(!dimension) { - currentError = 0.0; - currentIterations = 0; + current_cp->currentError = 0.0; + current_cp->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 *dfree = current_cp->getDfree(); + SReal *force = current_cp->getF(); + SReal **w = current_cp->getW(); + SReal tol = current_cp->tolerance; + SReal *d = current_cp->_d.ptr(); SReal error=0.0; bool convergence = false; sofa::type::vector tempForces; - if(sor != 1.0) + if(current_cp->sor != 1.0) { tempForces.resize(dimension); } - if(scaleTolerance && !allVerified) + if(current_cp->scaleTolerance && !current_cp->allVerified) { tol *= dimension; } for(int i=0; iconstraintsResolutions[i]) { - msg_error(solver) << "Bad size of constraintsResolutions in GenericConstraintProblem" ; + msg_error()<< "Bad size of constraintsResolutions in GenericConstraintSolver" ; break; } - constraintsResolutions[i]->init(i, w, force); - i += constraintsResolutions[i]->getNbLines(); + current_cp->constraintsResolutions[i]->init(i, w, force); + i += current_cp->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(); + showGraphs = d_computeGraphs.getValue(); if(showGraphs) { - graph_forces = solver->d_graphForces.beginEdit(); + graph_forces = d_graphForces.beginEdit(); graph_forces->clear(); - graph_violations = solver->d_graphViolations.beginEdit(); + graph_violations = d_graphViolations.beginEdit(); graph_violations->clear(); - graph_residuals = &(*solver->d_graphErrors.beginEdit())["Error"]; + graph_residuals = &(*d_graphErrors.beginEdit())["Error"]; graph_residuals->clear(); } @@ -100,12 +99,12 @@ void ProjectedGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstra int iterCount = 0; - for(int i=0; imaxIterations; i++) { iterCount ++; bool constraintsAreVerified = true; - if(sor != 1.0) + if(current_cp->sor != 1.0) { std::copy_n(force, dimension, tempForces.begin()); } @@ -130,11 +129,11 @@ void ProjectedGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstra graph_residuals->push_back(error); } - if(sor != 1.0) + if(current_cp->sor != 1.0) { for(int j=0; jsor * force[j] + (1-current_cp->sor) * tempForces[j]; } } @@ -144,13 +143,13 @@ void ProjectedGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstra if(timeout && dt > timeout) { - msg_info_when(solver!=nullptr, solver) << "TimeOut" ; + msg_info() << "TimeOut" ; - currentError = error; - currentIterations = i+1; + current_cp->currentError = error; + current_cp->currentIterations = i+1; return; } - else if(allVerified) + else if(current_cp->allVerified) { if(constraintsAreVerified) { @@ -165,41 +164,41 @@ void ProjectedGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstra } } - sofa::helper::AdvancedTimer::valSet("GS iterations", currentIterations); + sofa::helper::AdvancedTimer::valSet("GS iterations", current_cp->currentIterations); - result_output(solver, force, error, iterCount, convergence); + current_cp->result_output(this, force, error, iterCount, convergence); if(showGraphs) { - solver->d_graphErrors.endEdit(); + d_graphErrors.endEdit(); - sofa::type::vector& graph_constraints = (*solver->d_graphConstraints.beginEdit())["Constraints"]; + sofa::type::vector& graph_constraints = (*d_graphConstraints.beginEdit())["Constraints"]; graph_constraints.clear(); for(int j=0; jgetNbLines(); + const unsigned int nbDofs = current_cp->constraintsResolutions[j]->getNbLines(); if(tabErrors[j]) graph_constraints.push_back(tabErrors[j]); - else if(constraintsResolutions[j]->getTolerance()) - graph_constraints.push_back(constraintsResolutions[j]->getTolerance()); + else if(current_cp->constraintsResolutions[j]->getTolerance()) + graph_constraints.push_back(current_cp->constraintsResolutions[j]->getTolerance()); else graph_constraints.push_back(tol); j += nbDofs; } - solver->d_graphConstraints.endEdit(); + d_graphConstraints.endEdit(); - solver->d_graphForces.endEdit(); + d_graphForces.endEdit(); } } -void ProjectedGaussSeidelConstraintProblem::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 ProjectedGaussSeidelConstraintSolver::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 { for(int j=0; jgetNbLines(); + const unsigned int nb = current_cp->constraintsResolutions[j]->getNbLines(); //2. for each line we compute the actual value of d // (a)d is set to dfree @@ -217,7 +216,7 @@ void ProjectedGaussSeidelConstraintProblem::gaussSeidel_increment(bool measureEr } //3. the specific resolution of the constraint(s) is called - constraintsResolutions[j]->resolution(j, w, d, force, dfree); + 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)) if(measureError) @@ -251,15 +250,15 @@ void ProjectedGaussSeidelConstraintProblem::gaussSeidel_increment(bool measureEr } } - const bool givenTolerance = (bool)constraintsResolutions[j]->getTolerance(); + const bool givenTolerance = (bool)current_cp->constraintsResolutions[j]->getTolerance(); if(givenTolerance) { - if(contraintError > constraintsResolutions[j]->getTolerance()) + if(contraintError > current_cp->constraintsResolutions[j]->getTolerance()) { constraintsAreVerified = false; } - contraintError *= tol / constraintsResolutions[j]->getTolerance(); + contraintError *= tol / current_cp->constraintsResolutions[j]->getTolerance(); } error += contraintError; @@ -274,4 +273,12 @@ void ProjectedGaussSeidelConstraintProblem::gaussSeidel_increment(bool measureEr } } + +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/ProjectedGaussSeidelConstraintProblem.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h similarity index 89% rename from Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.h rename to Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h index ef2f289bb16..52fb4821493 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintProblem.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h @@ -21,17 +21,17 @@ ******************************************************************************/ #pragma once -#include +#include #include namespace sofa::component::constraint::lagrangian::solver { -class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ProjectedGaussSeidelConstraintProblem : public BuiltConstraintProblem +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ProjectedGaussSeidelConstraintSolver : public BuiltConstraintSolver { public: - SOFA_CLASS(ProjectedGaussSeidelConstraintProblem, BuiltConstraintProblem); + SOFA_CLASS(ProjectedGaussSeidelConstraintSolver, BuiltConstraintSolver); - virtual void solve( SReal timeout = 0.0, GenericConstraintSolver* solver = nullptr); + virtual void doSolve( 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, sofa::type::vector& tabErrors) const; 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 index 8438870d316..f075eeb37b7 100644 --- 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 @@ -1,4 +1,4 @@ -/****************************************************************************** +/****************************************************************************** * SOFA, Simulation Open-Framework Architecture * * (c) 2006 INRIA, USTL, UJF, CNRS, MGH * * * @@ -20,22 +20,28 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once +#include #include -#include +#include +#include namespace sofa::component::constraint::lagrangian::solver { + class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltConstraintProblem : public GenericConstraintProblem { public: - SOFA_CLASS(UnbuiltConstraintProblem, GenericConstraintProblem); + typedef std::vector< core::behavior::BaseConstraintCorrection* > ConstraintCorrections; - virtual void buildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints, GenericConstraintSolver* solver = nullptr) override; + UnbuiltConstraintProblem(GenericConstraintSolver* solver) + : GenericConstraintProblem(solver) + {} -protected: linearalgebra::SparseMatrix Wdiag; /** UNBUILT **/ std::list constraints_sequence; /** UNBUILT **/ std::vector< ConstraintCorrections > cclist_elems; /** UNBUILT **/ + + }; -} \ No newline at end of file +} diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp similarity index 63% rename from Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.cpp rename to Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp index d5ece473b2f..c55496d3859 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp @@ -20,50 +20,58 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ +#include #include #include namespace sofa::component::constraint::lagrangian::solver { -void UnbuiltConstraintProblem::buildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints, GenericConstraintSolver* solver ) +void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) { - for (const auto& cc : solver -> l_constraintCorrections) + UnbuiltConstraintProblem* c_current_cp = dynamic_cast(current_cp); + if (c_current_cp == nullptr) + { + msg_error()<<"Constraint problem must derive from UnbuiltConstraintProblem"; + return; + } + + for (const auto& cc : l_constraintCorrections) { if (!cc->isActive()) continue; - constraints_sequence.resize(numConstraints); - std::iota(constraints_sequence.begin(), constraints_sequence.end(), 0); + 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(getF(), constraints_sequence); + cc->resetForUnbuiltResolution(c_current_cp->getF(), c_current_cp->constraints_sequence); } - sofa::linearalgebra::SparseMatrix* localWdiag = &Wdiag; + 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 - cclist_elems.clear(); - cclist_elems.resize(numConstraints); - const int nbCC = solver->l_constraintCorrections.size(); + 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++) - cclist_elems[i].resize(nbCC, nullptr); + 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 = constraintsResolutions[c_id]->getNbLines(); + const unsigned int l = c_current_cp->constraintsResolutions[c_id]->getNbLines(); - for (unsigned int j = 0; j < solver->l_constraintCorrections.size(); j++) + for (unsigned int j = 0; j < l_constraintCorrections.size(); j++) { - core::behavior::BaseConstraintCorrection* cc = solver->l_constraintCorrections[j]; + core::behavior::BaseConstraintCorrection* cc = l_constraintCorrections[j]; if (!cc->isActive()) continue; if (cc->hasConstraintNumber(c_id)) { - cclist_elems[c_id][j] = cc; + c_current_cp->cclist_elems[c_id][j] = cc; cc->getBlockDiagonalCompliance(localWdiag, c_id, c_id + l - 1); foundCC = true; } @@ -71,7 +79,7 @@ void UnbuiltConstraintProblem::buildSystem( const core::ConstraintParams *cParam msg_error_when(!foundCC) << "No constraintCorrection found for constraint" << c_id ; - SReal** w = getW(); + 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); @@ -79,9 +87,19 @@ void UnbuiltConstraintProblem::buildSystem( const core::ConstraintParams *cParam c_id += l; } - addRegularization(W, solver->d_regularizationTerm.getValue()); - addRegularization(Wdiag, solver->d_regularizationTerm.getValue()); + 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..df2d233fb45 --- /dev/null +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.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 UnbuiltConstraintSolver : public GenericConstraintSolver +{ +public: + SOFA_CLASS(UnbuiltConstraintSolver, GenericConstraintSolver); + + virtual void doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) override; + virtual void initializeConstraintProblems() override; +}; +} \ No newline at end of file diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp similarity index 62% rename from Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.cpp rename to Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp index a9499871d0a..e4d5b4da52f 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.cpp @@ -20,38 +20,45 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#include +#include #include +#include #include #include +#include namespace sofa::component::constraint::lagrangian::solver { -void UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstraintSolver* solver) +void UnbuiltGaussSeidelConstraintSolver::doSolve( SReal timeout) { + UnbuiltConstraintProblem* c_current_cp = dynamic_cast(current_cp); + if (c_current_cp == nullptr) + { + msg_error()<<"Constraint problem must derive from UnbuiltConstraintProblem"; + return; + } + SCOPED_TIMER_VARNAME(unbuiltGaussSeidelTimer, "ConstraintsUnbuiltGaussSeidel"); - if(!solver) - return; - if(!this->dimension) + if(!c_current_cp->getDimension()) { - this->currentError = 0.0; - this->currentIterations = 0; + 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 = getDfree(); - SReal *force = getF(); - SReal **w = getW(); - SReal tol = tolerance; + 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 = _d.ptr(); + SReal *d = c_current_cp->_d.ptr(); unsigned int iter = 0, nb = 0; @@ -59,24 +66,24 @@ void UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstrain bool convergence = false; sofa::type::vector tempForces; - if(sor != 1.0) tempForces.resize(dimension); + if(c_current_cp->sor != 1.0) tempForces.resize(c_current_cp->getDimension()); - if(scaleTolerance && !allVerified) - tol *= dimension; + if(c_current_cp->scaleTolerance && !c_current_cp->allVerified) + tol *= c_current_cp->getDimension(); - for(int i=0; igetDimension(); ) { - if(!constraintsResolutions[i]) + if(!c_current_cp->constraintsResolutions[i]) { - msg_warning(solver) << "Bad size of constraintsResolutions in GenericConstraintProblem" ; - dimension = i; + msg_warning() << "Bad size of constraintsResolutions in GenericConstraintSolver" ; + c_current_cp->setDimension(i); break; } - constraintsResolutions[i]->init(i, w, force); - i += constraintsResolutions[i]->getNbLines(); + c_current_cp->constraintsResolutions[i]->init(i, w, force); + i += c_current_cp->constraintsResolutions[i]->getNbLines(); } - memset(force, 0, dimension * sizeof(SReal)); // Erase previous forces for the time being + memset(force, 0, c_current_cp->getDimension() * sizeof(SReal)); // Erase previous forces for the time being bool showGraphs = false; @@ -85,40 +92,40 @@ void UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstrain sofa::type::vector tabErrors; - showGraphs = solver->d_computeGraphs.getValue(); + showGraphs = d_computeGraphs.getValue(); if(showGraphs) { - graph_forces = solver->d_graphForces.beginEdit(); + graph_forces = d_graphForces.beginEdit(); graph_forces->clear(); - graph_violations = solver->d_graphViolations.beginEdit(); + graph_violations = d_graphViolations.beginEdit(); graph_violations->clear(); - graph_residuals = &(*solver->d_graphErrors.beginEdit())["Error"]; + graph_residuals = &(*d_graphErrors.beginEdit())["Error"]; graph_residuals->clear(); } - tabErrors.resize(dimension); + tabErrors.resize(c_current_cp->getDimension()); // temporary buffers std::vector errF; std::vector tempF; - for(iter=0; iter < static_cast(maxIterations); iter++) + for(iter=0; iter < static_cast(c_current_cp->maxIterations); iter++) { bool constraintsAreVerified = true; - if(sor != 1.0) + if(c_current_cp->sor != 1.0) { - std::copy_n(force, dimension, tempForces.begin()); + std::copy_n(force, c_current_cp->getDimension(), 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 + 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 = constraintsResolutions[j]->getNbLines(); + nb = c_current_cp->constraintsResolutions[j]->getNbLines(); //2. for each line we compute the actual value of d // (a)d is set to dfree @@ -130,14 +137,14 @@ void UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstrain std::copy_n(&dfree[j], nb, &d[j]); // (b) contribution of forces are added to d - for (auto* el : cclist_elems[j]) + 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 - constraintsResolutions[j]->resolution(j, w, d, force, dfree); + 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; @@ -165,11 +172,11 @@ void UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstrain constraintsAreVerified = false; } - if(constraintsResolutions[j]->getTolerance()) + if(c_current_cp->constraintsResolutions[j]->getTolerance()) { - if(contraintError > constraintsResolutions[j]->getTolerance()) + if(contraintError > c_current_cp->constraintsResolutions[j]->getTolerance()) constraintsAreVerified = false; - contraintError *= tol / constraintsResolutions[j]->getTolerance(); + contraintError *= tol / c_current_cp->constraintsResolutions[j]->getTolerance(); } error += contraintError; @@ -192,7 +199,7 @@ void UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstrain force[j+l] -= errF[l]; // DForce } - for (auto* el : cclist_elems[j]) + for (auto* el : c_current_cp->cclist_elems[j]) { if (el) el->setConstraintDForce(force, j, j+nb-1, update); @@ -205,7 +212,7 @@ void UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstrain if(showGraphs) { - for(int j=0; jgetDimension(); j++) { std::ostringstream oss; oss << "f" << j; @@ -220,10 +227,10 @@ void UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstrain graph_residuals->push_back(error); } - if(sor != 1.0) + 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) { @@ -232,12 +239,12 @@ void UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstrain if(dt > timeout) { - currentError = error; - currentIterations = iter+1; + c_current_cp->currentError = error; + c_current_cp->currentIterations = iter+1; return; } } - else if(allVerified) + else if(c_current_cp->allVerified) { if(constraintsAreVerified) { @@ -254,34 +261,40 @@ void UnbuiltGaussSeidelConstraintProblem::solve( SReal timeout, GenericConstrain - sofa::helper::AdvancedTimer::valSet("GS iterations", currentIterations); + sofa::helper::AdvancedTimer::valSet("GS iterations", c_current_cp->currentIterations); - result_output(solver, force, error, iter, convergence); + c_current_cp->result_output(this, force, error, iter, convergence); if(showGraphs) { - solver->d_graphErrors.endEdit(); + d_graphErrors.endEdit(); - sofa::type::vector& graph_constraints = (*solver->d_graphConstraints.beginEdit())["Constraints"]; + sofa::type::vector& graph_constraints = (*d_graphConstraints.beginEdit())["Constraints"]; graph_constraints.clear(); - for(int j=0; jgetDimension(); ) { - nb = constraintsResolutions[j]->getNbLines(); + nb = c_current_cp->constraintsResolutions[j]->getNbLines(); if(tabErrors[j]) graph_constraints.push_back(tabErrors[j]); - else if(constraintsResolutions[j]->getTolerance()) - graph_constraints.push_back(constraintsResolutions[j]->getTolerance()); + 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; } - solver->d_graphConstraints.endEdit(); + d_graphConstraints.endEdit(); - solver->d_graphForces.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/UnbuiltGaussSeidelConstraintProblem.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.h similarity index 88% rename from Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.h rename to Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.h index 75c4d25c421..e40509b3f04 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintProblem.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltGaussSeidelConstraintSolver.h @@ -21,16 +21,16 @@ ******************************************************************************/ #pragma once -#include +#include #include namespace sofa::component::constraint::lagrangian::solver { -class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltGaussSeidelConstraintProblem : public UnbuiltConstraintProblem +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltGaussSeidelConstraintSolver : public UnbuiltConstraintSolver { public: - SOFA_CLASS(UnbuiltGaussSeidelConstraintProblem, UnbuiltConstraintProblem); + SOFA_CLASS(UnbuiltGaussSeidelConstraintSolver, UnbuiltConstraintSolver); - virtual void solve( SReal timeout = 0.0, GenericConstraintSolver* solver = nullptr); + virtual void doSolve( 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); } From cbeadf6f7f3a5424ed643cb298dc06953aaecebc Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Tue, 26 Aug 2025 15:18:11 +0200 Subject: [PATCH 03/10] Apply changes to scenes, tests and tutorials --- .../animationloop/FreeMotionAnimationLoop.cpp | 2 +- .../scenes_test/BilateralInteractionConstraint.scn | 2 +- .../lagrangian/solver/GenericConstraintProblem.h | 12 ++++++------ .../lagrangian/solver/GenericConstraintSolver.cpp | 2 -- .../lagrangian/solver/GenericConstraintSolver.h | 9 --------- .../tests/BaseTetrahedronFEMForceField_test.h | 2 +- ...=> ProjectedGaussSeidelConstraintSolver_test.cpp} | 6 +++--- .../examples/ArticulatedArm/header.py | 2 +- .../plugins/PersistentContact/examples/grasping.scn | 2 +- .../plugins/Sensable/examples/SimpleBox-Method2.scn | 2 +- applications/plugins/SofaHAPI/examples/SofaHAPI1.scn | 2 +- .../SofaMatrix/examples/ComplianceMatrixExporter.scn | 2 +- .../SofaMatrix/examples/ComplianceMatrixImage.scn | 2 +- applications/plugins/Xitact/examples/xitactTest.scn | 2 +- .../SceneChecking/SceneCheckCollisionResponse.cpp | 4 ++-- examples/Benchmark/Performance/TorusFall.scn | 2 +- .../Collision/Response/RuleBasedContactManager.scn | 2 +- .../BilateralLagrangianConstraint_NNCG.scn | 2 +- .../Lagrangian/BilateralLagrangianConstraint_PGS.scn | 2 +- .../BilateralLagrangianConstraint_Rigid.scn | 2 +- ...lateralLagrangianConstraint_Soft_Rigid_Bodies.scn | 2 +- .../Lagrangian/BilateralLagrangianConstraint_UGS.scn | 2 +- ...angianConstraint_with_regularization_solvable.scn | 2 +- ...gianConstraint_with_regularization_unsolvable.scn | 2 +- .../Lagrangian/FixedLagrangianConstaint_Rigid3.scn | 2 +- .../Lagrangian/FixedLagrangianConstaint_Vec3.scn | 2 +- .../FrictionContact_VelocityConstraints.scn | 2 +- .../Constraint/Lagrangian/InextensiblePendulum.scn | 2 +- .../Lagrangian/SlidingLagrangianConstraint.scn | 2 +- .../LinearVelocityProjectiveConstraint.scn | 2 +- examples/Component/Engine/Select/NearestPointROI.scn | 2 +- .../Mapping/Linear/DistanceToPlaneMapping.scn | 2 +- examples/Component/Mapping/NonLinear/AreaMapping.scn | 2 +- .../FEM/Heterogeneous-TetrahedronFEMForceField.scn | 2 +- .../Spring/RestShapeSpringsForceField2.scn | 2 +- .../RemovingBilateralInteractionConstraint.scn | 2 +- examples/Demos/SofaScene.scn | 2 +- examples/Demos/SofaWasher.scn | 2 +- .../fallingBeamAugmentedLagrangianCollision.scn | 2 +- examples/Demos/fallingBeamLagrangianCollision.scn | 2 +- examples/Demos/fallingSOFA.scn | 2 +- examples/Demos/sofa_1000PR.scn | 2 +- examples/SimpleAPI/fallingSOFA.cpp | 2 +- 43 files changed, 49 insertions(+), 60 deletions(-) rename Sofa/framework/Simulation/Core/tests/{GenericConstraintSolver_test.cpp => ProjectedGaussSeidelConstraintSolver_test.cpp} (91%) diff --git a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp index 2a9ee148d61..1c2463dd7e0 100644 --- a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp +++ b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp @@ -69,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/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintProblem.h index 9de0e42ffbd..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 @@ -59,12 +59,12 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintProblem : void setSolver(GenericConstraintSolver* solver); - sofa::linearalgebra::FullVector _d; // - std::vector constraintsResolutions; // - bool scaleTolerance, allVerified; // - SReal sor; /** GAUSS-SEIDEL **/ - SReal currentError; // - int currentIterations; // + sofa::linearalgebra::FullVector _d; + std::vector constraintsResolutions; + bool scaleTolerance, allVerified; + SReal sor; + SReal currentError; + int currentIterations; sofa::linearalgebra::FullVector m_lam; sofa::linearalgebra::FullVector m_deltaF; 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 20dd0ea9e92..622dd45493a 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 @@ -62,7 +62,6 @@ void clearMultiVecId(sofa::core::objectmodel::BaseContext* ctx, const sofa::core } -static constexpr GenericConstraintSolver::ResolutionMethod defaultResolutionMethod("ProjectedGaussSeidel"); GenericConstraintSolver::GenericConstraintSolver() : d_maxIt(initData(&d_maxIt, 1000, "maxIterations", "maximal number of iterations of the Gauss-Seidel algorithm")) @@ -81,7 +80,6 @@ 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", diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h index c11eadc4a4d..6ce3b9c0e27 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h @@ -60,14 +60,6 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : ConstraintProblem* getConstraintProblem() override; void lockConstraintProblem(sofa::core::objectmodel::BaseObject* from, ConstraintProblem* p1, ConstraintProblem* p2 = nullptr) override; - - MAKE_SELECTABLE_ITEMS(ResolutionMethod, - sofa::helper::Item{"ProjectedGaussSeidel", "Projected Gauss-Seidel"}, - sofa::helper::Item{"UnbuiltGaussSeidel", "Gauss-Seidel where the matrix is not assembled"}, - sofa::helper::Item{"NonsmoothNonlinearConjugateGradient", "Non-smooth non-linear conjugate gradient"} - ); - Data< ResolutionMethod > 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_tolerance; ///< residual error threshold for termination of the Gauss-Seidel algorithm Data d_sor; ///< Successive Over Relaxation parameter (0-2) @@ -85,7 +77,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. 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/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 9f4d8bf264a..fdc8328b311 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"}}); From ac6be2484989707a3be5198b32e8ea6f9959c3c2 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Tue, 23 Sep 2025 16:26:45 +0200 Subject: [PATCH 04/10] Apply review comments --- .../animationloop/FreeMotionAnimationLoop.cpp | 6 +- .../solver/BuiltConstraintSolver.cpp | 94 +++++++++---------- .../lagrangian/solver/BuiltConstraintSolver.h | 8 ++ .../solver/ConstraintSolverImpl.cpp | 4 +- .../lagrangian/solver/ConstraintSolverImpl.h | 6 +- .../solver/GenericConstraintSolver.cpp | 9 +- .../solver/GenericConstraintSolver.h | 2 +- .../ProjectedGaussSeidelConstraintSolver.h | 1 + .../solver/UnbuiltConstraintProblem.h | 5 + .../solver/UnbuiltConstraintSolver.h | 7 ++ 10 files changed, 84 insertions(+), 58 deletions(-) diff --git a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp index 1c2463dd7e0..dd3111f9609 100644 --- a/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp +++ b/Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp @@ -44,12 +44,12 @@ #include #include #include +#include #include - -#include "sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h" using sofa::simulation::mechanicalvisitor::MechanicalVInitVisitor; + #include using sofa::simulation::mechanicalvisitor::MechanicalBeginIntegrationVisitor; @@ -102,7 +102,7 @@ void FreeMotionAnimationLoop::init() l_constraintSolver.set(this->getContext()->get(core::objectmodel::BaseContext::SearchDown)); if (!l_constraintSolver) { - if (const auto constraintSolver = sofa::core::objectmodel::New()) + if (const auto constraintSolver = sofa::core::objectmodel::New()) { getContext()->addObject(constraintSolver); constraintSolver->setName( this->getContext()->getNameHelper().resolveName(constraintSolver->getClassName(), {})); diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp index 306ae5d7480..f317a423713 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp @@ -29,75 +29,75 @@ namespace sofa::component::constraint::lagrangian::solver { - void BuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) - { - SOFA_UNUSED(numConstraints); - SCOPED_TIMER_VARNAME(getComplianceTimer, "Get Compliance"); - dmsg_info() <<" computeCompliance in " << l_constraintCorrections.size()<< " constraintCorrections" ; +void BuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, 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 bool multithreading = d_multithreading.getValue(); - const simulation::ForEachExecutionPolicy execution = multithreading ? - simulation::ForEachExecutionPolicy::PARALLEL : - simulation::ForEachExecutionPolicy::SEQUENTIAL; + const simulation::ForEachExecutionPolicy execution = multithreading ? + simulation::ForEachExecutionPolicy::PARALLEL : + simulation::ForEachExecutionPolicy::SEQUENTIAL; - simulation::TaskScheduler* taskScheduler = simulation::MainTaskSchedulerFactory::createInRegistry(); - assert(taskScheduler); + simulation::TaskScheduler* taskScheduler = simulation::MainTaskSchedulerFactory::createInRegistry(); + assert(taskScheduler); - //Used to prevent simultaneous accesses to the main compliance matrix - std::mutex mutex; + //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); + //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) + for (auto it = range.start; it != range.end; ++it) + { + core::behavior::BaseConstraintCorrection* cc = *it; + if (cc->isActive()) { - core::behavior::BaseConstraintCorrection* cc = *it; - if (cc->isActive()) - { - cc->addComplianceInConstraintSpace(cParams, &compliance.matrix()); - } + cc->addComplianceInConstraintSpace(cParams, &compliance.matrix()); } + } - std::lock_guard guard(mutex); - compliance.assembleMatrix(); - }); + std::lock_guard guard(mutex); + compliance.assembleMatrix(); + }); - addRegularization(current_cp->W, d_regularizationTerm.getValue()); - dmsg_info() << " computeCompliance_done " ; - } + addRegularization(current_cp->W, d_regularizationTerm.getValue()); + dmsg_info() << " computeCompliance_done " ; +} - BuiltConstraintSolver::ComplianceWrapper::ComplianceMatrixType& BuiltConstraintSolver::ComplianceWrapper::matrix() +BuiltConstraintSolver::ComplianceWrapper::ComplianceMatrixType& BuiltConstraintSolver::ComplianceWrapper::matrix() +{ + if (m_isMultiThreaded) { - if (m_isMultiThreaded) + if (!m_threadMatrix) { - if (!m_threadMatrix) - { - m_threadMatrix = std::make_unique(); - m_threadMatrix->resize(m_complianceMatrix.rowSize(), m_complianceMatrix.colSize()); - } - return *m_threadMatrix; + m_threadMatrix = std::make_unique(); + m_threadMatrix->resize(m_complianceMatrix.rowSize(), m_complianceMatrix.colSize()); } - return m_complianceMatrix; + return *m_threadMatrix; } + return m_complianceMatrix; +} - void BuiltConstraintSolver::ComplianceWrapper::assembleMatrix() const +void BuiltConstraintSolver::ComplianceWrapper::assembleMatrix() const +{ + if (m_threadMatrix) { - if (m_threadMatrix) + for (linearalgebra::BaseMatrix::Index j = 0; j < m_threadMatrix->rowSize(); ++j) { - for (linearalgebra::BaseMatrix::Index j = 0; j < m_threadMatrix->rowSize(); ++j) + for (linearalgebra::BaseMatrix::Index l = 0; l < m_threadMatrix->colSize(); ++l) { - for (linearalgebra::BaseMatrix::Index l = 0; l < m_threadMatrix->colSize(); ++l) - { - m_complianceMatrix.add(j, l, m_threadMatrix->element(j,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 index 612a7e6cfc7..a42763527a3 100644 --- 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 @@ -25,8 +25,16 @@ 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); 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 9db0ad19abf..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 @@ -35,7 +35,7 @@ namespace sofa::component::constraint::lagrangian::solver { -class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem +class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem { public: // The compliance matrix projected in the constraint space @@ -67,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; }; 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 622dd45493a..4aa4f1b73bb 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 @@ -64,7 +64,7 @@ void clearMultiVecId(sofa::core::objectmodel::BaseContext* ctx, const sofa::core GenericConstraintSolver::GenericConstraintSolver() - : 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")) @@ -114,7 +114,12 @@ GenericConstraintSolver::GenericConstraintSolver() } GenericConstraintSolver::~GenericConstraintSolver() -{} +{ + for (unsigned i=0; i< CP_BUFFER_SIZE; ++i) + { + delete m_cpBuffer[i]; + } +} diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h index 6ce3b9c0e27..6065d5e7040 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h @@ -60,7 +60,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : ConstraintProblem* getConstraintProblem() override; void lockConstraintProblem(sofa::core::objectmodel::BaseObject* from, ConstraintProblem* p1, ConstraintProblem* p2 = nullptr) override; - 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 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 index 52fb4821493..a956210a9cd 100644 --- 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 @@ -33,6 +33,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ProjectedGaussSeidelConstr virtual void doSolve( SReal timeout = 0.0) override; +protected: 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; }; 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 index f075eeb37b7..579d94efcb1 100644 --- 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 @@ -29,6 +29,10 @@ 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: @@ -38,6 +42,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltConstraintProblem : : 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.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.h index df2d233fb45..df2f3953c2e 100644 --- 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 @@ -28,6 +28,13 @@ 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: From 956c777126c65abcde1d51a0aa0345fff7c4c62c Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Tue, 23 Sep 2025 16:35:01 +0200 Subject: [PATCH 05/10] change ownership of d_multithreading --- .../lagrangian/solver/BuiltConstraintSolver.cpp | 12 ++++++++++++ .../lagrangian/solver/BuiltConstraintSolver.h | 4 +++- .../lagrangian/solver/GenericConstraintSolver.cpp | 8 -------- .../lagrangian/solver/GenericConstraintSolver.h | 1 - 4 files changed, 15 insertions(+), 10 deletions(-) diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp index f317a423713..50fae7093d6 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp @@ -28,6 +28,18 @@ 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, unsigned int numConstraints) { 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 index a42763527a3..da0ce0d38bc 100644 --- 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 @@ -37,10 +37,12 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintSolver : pu public: SOFA_CLASS(BuiltConstraintSolver, GenericConstraintSolver); + Data d_multithreading; ///< Build compliances concurrently - + BuiltConstraintSolver(); virtual void doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) override; + virtual void init() override; private: struct ComplianceWrapper 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 4aa4f1b73bb..2ae718ed75a 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 @@ -70,7 +70,6 @@ GenericConstraintSolver::GenericConstraintSolver() , 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_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")) @@ -139,13 +138,6 @@ void GenericConstraintSolver:: init() dx.realloc(&vop,false,true, core::VecIdProperties{"constraint_dx", GetClass()->className}); m_dxId = dx.id(); } - - if(d_multithreading.getValue()) - { - simulation::MainTaskSchedulerFactory::createInRegistry()->init(); - } - - } void GenericConstraintSolver::initializeConstraintProblems() diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h index 6065d5e7040..6ce7b08df8f 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h @@ -66,7 +66,6 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : 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_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 From 9a823b2769d6be37557724af3e0912a8c9a7ac7e Mon Sep 17 00:00:00 2001 From: bakpaul Date: Tue, 30 Sep 2025 17:27:03 +0200 Subject: [PATCH 06/10] Add comment on do methods and changed signature to add constraint problem --- .../solver/BuiltConstraintSolver.cpp | 8 +- .../lagrangian/solver/BuiltConstraintSolver.h | 5 +- .../solver/GenericConstraintProblem.cpp | 2 +- .../solver/GenericConstraintSolver.cpp | 4 +- .../solver/GenericConstraintSolver.h | 28 ++++++- .../solver/NNCGConstraintSolver.cpp | 74 +++++++++---------- .../lagrangian/solver/NNCGConstraintSolver.h | 2 +- .../ProjectedGaussSeidelConstraintSolver.cpp | 66 ++++++++--------- .../ProjectedGaussSeidelConstraintSolver.h | 7 +- .../solver/UnbuiltConstraintSolver.cpp | 5 +- .../solver/UnbuiltConstraintSolver.h | 4 +- .../UnbuiltGaussSeidelConstraintSolver.cpp | 4 +- .../UnbuiltGaussSeidelConstraintSolver.h | 2 +- 13 files changed, 120 insertions(+), 91 deletions(-) diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp index 50fae7093d6..a2e36c175a1 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp @@ -41,7 +41,7 @@ void BuiltConstraintSolver::init() } } -void BuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) +void BuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints) { SOFA_UNUSED(numConstraints); SCOPED_TIMER_VARNAME(getComplianceTimer, "Get Compliance"); @@ -62,9 +62,9 @@ void BuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams //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) + [&cParams, this, &multithreading, &mutex, problem](const auto& range) { - ComplianceWrapper compliance(current_cp->W, multithreading); + ComplianceWrapper compliance(problem->W, multithreading); for (auto it = range.start; it != range.end; ++it) { @@ -79,7 +79,7 @@ void BuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams compliance.assembleMatrix(); }); - addRegularization(current_cp->W, d_regularizationTerm.getValue()); + addRegularization(problem->W, d_regularizationTerm.getValue()); dmsg_info() << " computeCompliance_done " ; } 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 index da0ce0d38bc..c9416a574e0 100644 --- 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 @@ -40,9 +40,12 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintSolver : pu Data d_multithreading; ///< Build compliances concurrently BuiltConstraintSolver(); - virtual void doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) override; virtual void init() override; + +protected: + virtual void doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints) override; + private: struct ComplianceWrapper 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 101504f6378..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,7 +74,7 @@ void GenericConstraintProblem::solveTimed(SReal tol, int maxIt, SReal timeout) tolerance = tol; maxIterations = maxIt; - m_solver->doSolve(timeout); + m_solver->doSolve(this, timeout); tolerance = tempTol; maxIterations = tempMaxIt; 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 2ae718ed75a..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 @@ -209,7 +209,7 @@ bool GenericConstraintSolver::buildSystem(const core::ConstraintParams *cParams, } - this->doBuildSystem(cParams, numConstraints); + this->doBuildSystem(cParams, current_cp, numConstraints); return true; } @@ -279,7 +279,7 @@ bool GenericConstraintSolver::solveSystem(const core::ConstraintParams * /*cPara msg_info() << tmp.str() ; } - this->doSolve(0.0); + this->doSolve(current_cp, 0.0); this->d_currentError.setValue(current_cp->currentError); diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h index 6ce7b08df8f..3cd0993a7e1 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h @@ -95,8 +95,32 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : sofa::core::MultiVecDerivId m_dxId; virtual void initializeConstraintProblems(); - virtual void doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) = 0; - virtual void doSolve( SReal timeout = 0.0) = 0; + + /***** + * + * @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); 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 index 047a9018ec5..540daa6e895 100644 --- 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 @@ -29,61 +29,61 @@ namespace sofa::component::constraint::lagrangian::solver { -void NNCGConstraintSolver::doSolve( SReal timeout) +void NNCGConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal timeout) { SCOPED_TIMER_VARNAME(unbuiltGaussSeidelTimer, "NonsmoothNonlinearConjugateGradient"); - const int dimension = current_cp->getDimension(); + const int dimension = problem->getDimension(); if(!dimension) { - current_cp->currentError = 0.0; - current_cp->currentIterations = 0; + problem->currentError = 0.0; + problem->currentIterations = 0; return; } - current_cp->m_lam.clear(); - current_cp->m_lam.resize(dimension); - current_cp->m_deltaF.clear(); - current_cp->m_deltaF.resize(dimension); - current_cp->m_deltaF_new.clear(); - current_cp->m_deltaF_new.resize(dimension); - current_cp->m_p.clear(); - current_cp->m_p.resize(dimension); + 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 = current_cp->getDfree(); - SReal *force = current_cp->getF(); - SReal **w = current_cp->getW(); - SReal tol = current_cp->tolerance; + SReal *dfree = problem->getDfree(); + SReal *force = problem->getF(); + SReal **w = problem->getW(); + SReal tol = problem->tolerance; - SReal *d = current_cp->_d.ptr(); + SReal *d = problem->_d.ptr(); SReal error = 0.0; bool convergence = false; sofa::type::vector tempForces; - if(current_cp->sor != 1.0) + if(problem->sor != 1.0) { tempForces.resize(dimension); } - if(current_cp->scaleTolerance && !current_cp->allVerified) + if(problem->scaleTolerance && !problem->allVerified) { tol *= dimension; } for(int i=0; iconstraintsResolutions[i]) + if(!problem->constraintsResolutions[i]) { msg_error() << "Bad size of constraintsResolutions in GenericConstraintSolver" ; break; } - current_cp->constraintsResolutions[i]->init(i, w, force); - i += current_cp->constraintsResolutions[i]->getNbLines(); + problem->constraintsResolutions[i]->init(i, w, force); + i += problem->constraintsResolutions[i]->getNbLines(); } sofa::type::vector tabErrors(dimension); @@ -91,14 +91,14 @@ void NNCGConstraintSolver::doSolve( SReal timeout) { // perform one iteration of ProjectedGaussSeidel bool constraintsAreVerified = true; - std::copy_n(force, dimension, std::begin(current_cp->m_lam)); + std::copy_n(force, dimension, std::begin(problem->m_lam)); - gaussSeidel_increment(false, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, tabErrors); + gaussSeidel_increment(false, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, problem->constraintsResolutions, tabErrors); for(int j=0; jm_deltaF[j] = -(force[j] - current_cp->m_lam[j]); - current_cp->m_p[j] = - current_cp->m_deltaF[j]; + problem->m_deltaF[j] = -(force[j] - problem->m_lam[j]); + problem->m_p[j] = - problem->m_deltaF[j]; } } @@ -113,15 +113,15 @@ void NNCGConstraintSolver::doSolve( SReal timeout) for(int j=0; jm_lam[j] = force[j]; + problem->m_lam[j] = force[j]; } error=0.0; - gaussSeidel_increment(true, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, tabErrors); + gaussSeidel_increment(true, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, problem->constraintsResolutions, tabErrors); - if(current_cp->allVerified) + if(problem->allVerified) { if(constraintsAreVerified) { @@ -139,28 +139,28 @@ void NNCGConstraintSolver::doSolve( SReal timeout) // NNCG update with the correction p for(int j=0; jm_deltaF_new[j] = -(force[j] - current_cp->m_lam[j]); + problem->m_deltaF_new[j] = -(force[j] - problem->m_lam[j]); } - const SReal beta = current_cp->m_deltaF_new.dot(current_cp->m_deltaF_new) / current_cp->m_deltaF.dot(current_cp->m_deltaF); - current_cp->m_deltaF.eq(current_cp->m_deltaF_new, 1); + 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) { - current_cp->m_p.clear(); - current_cp->m_p.resize(dimension); + problem->m_p.clear(); + problem->m_p.resize(dimension); } else { for(int j=0; jm_p[j]; - current_cp->m_p[j] = beta*current_cp->m_p[j] -current_cp-> m_deltaF[j]; + force[j] += beta*problem->m_p[j]; + problem->m_p[j] = beta*problem->m_p[j] -problem-> m_deltaF[j]; } } } - current_cp->result_output(this, force, error, iterCount, convergence); + problem->result_output(this, force, error, iterCount, convergence); } void registerNNCGConstraintSolver(sofa::core::ObjectFactory* factory) 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 index e97baff1a3f..26bd9e95eed 100644 --- 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 @@ -31,6 +31,6 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API NNCGConstraintSolver : pub public: SOFA_CLASS(NNCGConstraintSolver, ProjectedGaussSeidelConstraintSolver); - virtual void doSolve( SReal timeout = 0.0) override; + 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 index 55fe5b02515..efaeb6ed2d9 100644 --- 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 @@ -29,52 +29,52 @@ namespace sofa::component::constraint::lagrangian::solver { -void ProjectedGaussSeidelConstraintSolver::doSolve( SReal timeout) +void ProjectedGaussSeidelConstraintSolver::doSolve( GenericConstraintProblem * problem ,SReal timeout) { SCOPED_TIMER_VARNAME(gaussSeidelTimer, "ConstraintsGaussSeidel"); - const int dimension = current_cp->getDimension(); + const int dimension = problem->getDimension(); if(!dimension) { - current_cp->currentError = 0.0; - current_cp->currentIterations = 0; + 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 = current_cp->getDfree(); - SReal *force = current_cp->getF(); - SReal **w = current_cp->getW(); - SReal tol = current_cp->tolerance; - SReal *d = current_cp->_d.ptr(); + 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(current_cp->sor != 1.0) + if(problem->sor != 1.0) { tempForces.resize(dimension); } - if(current_cp->scaleTolerance && !current_cp->allVerified) + if(problem->scaleTolerance && !problem->allVerified) { tol *= dimension; } for(int i=0; iconstraintsResolutions[i]) + if(!problem->constraintsResolutions[i]) { msg_error()<< "Bad size of constraintsResolutions in GenericConstraintSolver" ; break; } - current_cp->constraintsResolutions[i]->init(i, w, force); - i += current_cp->constraintsResolutions[i]->getNbLines(); + problem->constraintsResolutions[i]->init(i, w, force); + i += problem->constraintsResolutions[i]->getNbLines(); } bool showGraphs = false; @@ -99,18 +99,18 @@ void ProjectedGaussSeidelConstraintSolver::doSolve( SReal timeout) int iterCount = 0; - for(int i=0; imaxIterations; i++) + for(int i=0; imaxIterations; i++) { iterCount ++; bool constraintsAreVerified = true; - if(current_cp->sor != 1.0) + 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, tabErrors); + gaussSeidel_increment(true, dfree, force, w, tol, d, dimension, constraintsAreVerified, error, problem->constraintsResolutions, tabErrors); if(showGraphs) { @@ -129,11 +129,11 @@ void ProjectedGaussSeidelConstraintSolver::doSolve( SReal timeout) graph_residuals->push_back(error); } - if(current_cp->sor != 1.0) + if(problem->sor != 1.0) { for(int j=0; jsor * force[j] + (1-current_cp->sor) * tempForces[j]; + force[j] = problem->sor * force[j] + (1-problem->sor) * tempForces[j]; } } @@ -145,11 +145,11 @@ void ProjectedGaussSeidelConstraintSolver::doSolve( SReal timeout) msg_info() << "TimeOut" ; - current_cp->currentError = error; - current_cp->currentIterations = i+1; + problem->currentError = error; + problem->currentIterations = i+1; return; } - else if(current_cp->allVerified) + else if(problem->allVerified) { if(constraintsAreVerified) { @@ -164,9 +164,9 @@ void ProjectedGaussSeidelConstraintSolver::doSolve( SReal timeout) } } - sofa::helper::AdvancedTimer::valSet("GS iterations", current_cp->currentIterations); + sofa::helper::AdvancedTimer::valSet("GS iterations", problem->currentIterations); - current_cp->result_output(this, force, error, iterCount, convergence); + problem->result_output(this, force, error, iterCount, convergence); if(showGraphs) { @@ -177,12 +177,12 @@ void ProjectedGaussSeidelConstraintSolver::doSolve( SReal timeout) for(int j=0; jconstraintsResolutions[j]->getNbLines(); + const unsigned int nbDofs = problem->constraintsResolutions[j]->getNbLines(); if(tabErrors[j]) graph_constraints.push_back(tabErrors[j]); - else if(current_cp->constraintsResolutions[j]->getTolerance()) - graph_constraints.push_back(current_cp->constraintsResolutions[j]->getTolerance()); + else if(problem->constraintsResolutions[j]->getTolerance()) + graph_constraints.push_back(problem->constraintsResolutions[j]->getTolerance()); else graph_constraints.push_back(tol); @@ -193,12 +193,12 @@ void ProjectedGaussSeidelConstraintSolver::doSolve( SReal timeout) 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, sofa::type::vector& tabErrors) const +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; jconstraintsResolutions[j]->getNbLines(); + const unsigned int nb = constraintCorrections[j]->getNbLines(); //2. for each line we compute the actual value of d // (a)d is set to dfree @@ -216,7 +216,7 @@ void ProjectedGaussSeidelConstraintSolver::gaussSeidel_increment(bool measureErr } //3. the specific resolution of the constraint(s) is called - current_cp->constraintsResolutions[j]->resolution(j, w, d, force, dfree); + constraintCorrections[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)) if(measureError) @@ -250,15 +250,15 @@ void ProjectedGaussSeidelConstraintSolver::gaussSeidel_increment(bool measureErr } } - const bool givenTolerance = (bool)current_cp->constraintsResolutions[j]->getTolerance(); + const bool givenTolerance = (bool)constraintCorrections[j]->getTolerance(); if(givenTolerance) { - if(contraintError > current_cp->constraintsResolutions[j]->getTolerance()) + if(contraintError > constraintCorrections[j]->getTolerance()) { constraintsAreVerified = false; } - contraintError *= tol / current_cp->constraintsResolutions[j]->getTolerance(); + contraintError *= tol / constraintCorrections[j]->getTolerance(); } error += contraintError; 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 index a956210a9cd..35fd4a903c4 100644 --- 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 @@ -30,11 +30,10 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ProjectedGaussSeidelConstr { public: SOFA_CLASS(ProjectedGaussSeidelConstraintSolver, BuiltConstraintSolver); - - virtual void doSolve( SReal timeout = 0.0) override; - protected: - 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; + 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/UnbuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/UnbuiltConstraintSolver.cpp index c55496d3859..830dfda7f9a 100644 --- 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 @@ -27,9 +27,10 @@ namespace sofa::component::constraint::lagrangian::solver { -void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) +void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem, unsigned int numConstraints) { - UnbuiltConstraintProblem* c_current_cp = dynamic_cast(current_cp); + SOFA_UNUSED(cParams); + UnbuiltConstraintProblem* c_current_cp = dynamic_cast(problem); if (c_current_cp == nullptr) { msg_error()<<"Constraint problem must derive from UnbuiltConstraintProblem"; 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 index df2f3953c2e..fe6ff57c60d 100644 --- 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 @@ -40,7 +40,9 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltConstraintSolver : public: SOFA_CLASS(UnbuiltConstraintSolver, GenericConstraintSolver); - virtual void doBuildSystem( const core::ConstraintParams *cParams, unsigned int numConstraints) override; 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 index e4d5b4da52f..7f447554367 100644 --- 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 @@ -31,9 +31,9 @@ namespace sofa::component::constraint::lagrangian::solver { -void UnbuiltGaussSeidelConstraintSolver::doSolve( SReal timeout) +void UnbuiltGaussSeidelConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal timeout) { - UnbuiltConstraintProblem* c_current_cp = dynamic_cast(current_cp); + UnbuiltConstraintProblem* c_current_cp = dynamic_cast(problem); if (c_current_cp == nullptr) { msg_error()<<"Constraint problem must derive from UnbuiltConstraintProblem"; 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 index e40509b3f04..8daabb81e3c 100644 --- 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 @@ -31,6 +31,6 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API UnbuiltGaussSeidelConstrai public: SOFA_CLASS(UnbuiltGaussSeidelConstraintSolver, UnbuiltConstraintSolver); - virtual void doSolve( SReal timeout = 0.0) override; + virtual void doSolve(GenericConstraintProblem * problem , SReal timeout = 0.0) override; }; } \ No newline at end of file From dd1c03131e546c59403644876633d654bc8fcdf6 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Thu, 2 Oct 2025 16:35:16 +0200 Subject: [PATCH 07/10] Add scene check to fix and display info if GenericConstraintSolver is still being used --- .../Core/src/sofa/core/ObjectFactory.cpp | 3 + .../Core/src/sofa/core/ObjectFactory.h | 6 +- .../projects/SceneChecking/CMakeLists.txt | 2 + .../SceneCheckConstraintSolver.cpp | 137 ++++++++++++++++++ .../SceneCheckConstraintSolver.h | 57 ++++++++ .../SceneChecking/SceneCheckUsingAlias.cpp | 2 +- 6 files changed, 205 insertions(+), 2 deletions(-) create mode 100644 applications/projects/SceneChecking/src/SceneChecking/SceneCheckConstraintSolver.cpp create mode 100644 applications/projects/SceneChecking/src/SceneChecking/SceneCheckConstraintSolver.h diff --git a/Sofa/framework/Core/src/sofa/core/ObjectFactory.cpp b/Sofa/framework/Core/src/sofa/core/ObjectFactory.cpp index c46c9e4cd7a..a2ba9e7a41b 100644 --- a/Sofa/framework/Core/src/sofa/core/ObjectFactory.cpp +++ b/Sofa/framework/Core/src/sofa/core/ObjectFactory.cpp @@ -141,6 +141,9 @@ void findTemplatedCreator( objectmodel::BaseObject::SPtr ObjectFactory::createObject(objectmodel::BaseContext* context, objectmodel::BaseObjectDescription* arg) { + if (m_callbackBeforeCreate) + m_callbackBeforeCreate(context, arg); + objectmodel::BaseObject::SPtr object = nullptr; std::vector< std::pair > creators; std::string classname = arg->getAttribute( "type", ""); diff --git a/Sofa/framework/Core/src/sofa/core/ObjectFactory.h b/Sofa/framework/Core/src/sofa/core/ObjectFactory.h index 40ca0fed60e..6d2c520ef4e 100644 --- a/Sofa/framework/Core/src/sofa/core/ObjectFactory.h +++ b/Sofa/framework/Core/src/sofa/core/ObjectFactory.h @@ -54,6 +54,8 @@ namespace sofa::core class ObjectRegistrationData; typedef std::function OnCreateCallback ; +typedef std::function BeforeCreateCallback ; + class SOFA_CORE_API ObjectFactory { public: @@ -109,6 +111,7 @@ class SOFA_CORE_API ObjectFactory protected: /// Main class registry ClassEntryMap registry; + BeforeCreateCallback m_callbackBeforeCreate ; OnCreateCallback m_callbackOnCreate ; /// Keep track of plugins who already registered @@ -206,7 +209,8 @@ class SOFA_CORE_API ObjectFactory /// Dump the content of the factory to a HTML stream. void dumpHTML(std::ostream& out = std::cout); - void setCallback(OnCreateCallback cb) { m_callbackOnCreate = cb ; } + void setOnCreateCallback(OnCreateCallback cb) { m_callbackOnCreate = cb ; } + void setBeforeCreateCallback(BeforeCreateCallback cb) { m_callbackBeforeCreate = cb ; } bool registerObjectsFromPlugin(const std::string& pluginName); bool registerObjects(ObjectRegistrationData& ro); diff --git a/applications/projects/SceneChecking/CMakeLists.txt b/applications/projects/SceneChecking/CMakeLists.txt index e0832b0ad65..2a1ac69d816 100644 --- a/applications/projects/SceneChecking/CMakeLists.txt +++ b/applications/projects/SceneChecking/CMakeLists.txt @@ -13,6 +13,7 @@ set(HEADER_FILES ${SCENECHECK_SRC_DIR}/init.h ${SCENECHECK_SRC_DIR}/SceneCheckAPIChange.h ${SCENECHECK_SRC_DIR}/SceneCheckCollisionResponse.h + ${SCENECHECK_SRC_DIR}/SceneCheckConstraintSolver.h ${SCENECHECK_SRC_DIR}/SceneCheckDeprecatedComponents.h ${SCENECHECK_SRC_DIR}/SceneCheckDuplicatedName.h ${SCENECHECK_SRC_DIR}/SceneCheckEmptyNodeName.h @@ -27,6 +28,7 @@ set(SOURCE_FILES ${SCENECHECK_SRC_DIR}/init.cpp ${SCENECHECK_SRC_DIR}/SceneCheckAPIChange.cpp ${SCENECHECK_SRC_DIR}/SceneCheckCollisionResponse.cpp + ${SCENECHECK_SRC_DIR}/SceneCheckConstraintSolver.cpp ${SCENECHECK_SRC_DIR}/SceneCheckDeprecatedComponents.cpp ${SCENECHECK_SRC_DIR}/SceneCheckDuplicatedName.cpp ${SCENECHECK_SRC_DIR}/SceneCheckEmptyNodeName.cpp diff --git a/applications/projects/SceneChecking/src/SceneChecking/SceneCheckConstraintSolver.cpp b/applications/projects/SceneChecking/src/SceneChecking/SceneCheckConstraintSolver.cpp new file mode 100644 index 00000000000..b1bff8c6c9f --- /dev/null +++ b/applications/projects/SceneChecking/src/SceneChecking/SceneCheckConstraintSolver.cpp @@ -0,0 +1,137 @@ +/****************************************************************************** +* 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 "SceneCheckConstraintSolver.h" + +#include +#include +#include +#include +#include + + +namespace sofa::_scenechecking_ +{ + +const bool SceneCheckConstraintSolverRegistered = sofa::simulation::SceneCheckMainRegistry::addToRegistry(SceneCheckConstraintSolver::newSPtr()); + +using sofa::core::objectmodel::BaseContext; +using sofa::core::objectmodel::BaseObjectDescription; +using sofa::core::ObjectFactory; + + +SceneCheckConstraintSolver::SceneCheckConstraintSolver() +{ + /// Add a callback to be n + ObjectFactory::getInstance()->setBeforeCreateCallback([this](BaseContext* o, BaseObjectDescription *arg) { + const std::string typeNameInScene = arg->getAttribute("type", ""); + if ( typeNameInScene == "GenericConstraintSolver" ) + { + const std::string solverType = arg->getAttribute("resolutionMethod", "ProjectedGaussSeidel"); + std::string newName = "ConstraintSolver"; + if (solverType == "ProjectedGaussSeidel") + newName = std::string("ProjectedGaussSeidel") + newName; + else if (solverType == "UnbuiltGaussSeidel") + newName = std::string("UnbuiltGaussSeidel") + newName; + else if (solverType == "NonsmoothNonlinearConjugateGradient") + newName = std::string("NNCG") + newName; + else + newName = std::string("ProjectedGaussSeidel") + newName; + m_targetDescription["type"] = newName; + std::vector attributes; + arg->getAttributeList(attributes); + for (const auto & attr : attributes) + { + if (attr == "resolutionMethod" || attr == "type") + continue; + + const std::string attrValue = arg->getAttribute(attr,""); + + if (attr == "newtonIterations") + m_targetDescription["maxIterations"] = attrValue; + else if (! attrValue.empty()) + m_targetDescription[attr] = attrValue; + } + + //Now correct args to run simulation anyway + const std::string newtonValue = arg->getAttribute("newtonIterations",""); + if (!newtonValue.empty()) + arg->setAttribute("maxIterations", newtonValue); + + arg->removeAttribute("resolutionMethod"); + arg->removeAttribute("newtonIterations"); + arg->setAttribute("type",newName); + } + + }); +} + +SceneCheckConstraintSolver::~SceneCheckConstraintSolver() +{ + +} + +const std::string SceneCheckConstraintSolver::getName() +{ + return "SceneCheckConstraintSolver"; +} + +const std::string SceneCheckConstraintSolver::getDesc() +{ + return "Check if a Component has been created using an Alias."; +} + +void SceneCheckConstraintSolver::doPrintSummary() +{ + if ( this->m_targetDescription.empty() ) + { + return; + } + + std::stringstream usingAliasesWarning; + usingAliasesWarning << "This scene is using a GenericConstraintSolver which is now an abstract class."<< msgendl; + usingAliasesWarning << "The real types are now specific to the solver. The object has been automatically replaced for you." <" << msgendl; + + msg_warning(this->getName()) << usingAliasesWarning.str(); + +} + +} // namespace sofa::_scenechecking_ diff --git a/applications/projects/SceneChecking/src/SceneChecking/SceneCheckConstraintSolver.h b/applications/projects/SceneChecking/src/SceneChecking/SceneCheckConstraintSolver.h new file mode 100644 index 00000000000..3d42acef8cf --- /dev/null +++ b/applications/projects/SceneChecking/src/SceneChecking/SceneCheckConstraintSolver.h @@ -0,0 +1,57 @@ +/****************************************************************************** +* 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::_scenechecking_ +{ + +class SOFA_SCENECHECKING_API SceneCheckConstraintSolver : public sofa::simulation::SceneCheck +{ +public: + SceneCheckConstraintSolver(); + virtual ~SceneCheckConstraintSolver(); + + typedef std::shared_ptr SPtr; + static SPtr newSPtr() { return SPtr(new SceneCheckConstraintSolver()); } + virtual const std::string getName() override; + virtual const std::string getDesc() override; + void doInit(sofa::simulation::Node* node) override { SOFA_UNUSED(node); } + void doCheckOn(sofa::simulation::Node* node) override { SOFA_UNUSED(node); } + void doPrintSummary() override; + +private: + std::map m_targetDescription; +}; + +} // namespace sofa::_scenechecking_ + +namespace sofa::scenechecking +{ + using _scenechecking_::SceneCheckConstraintSolver; +} // namespace sofa::scenechecking + diff --git a/applications/projects/SceneChecking/src/SceneChecking/SceneCheckUsingAlias.cpp b/applications/projects/SceneChecking/src/SceneChecking/SceneCheckUsingAlias.cpp index cde238845e9..e86db1765cb 100644 --- a/applications/projects/SceneChecking/src/SceneChecking/SceneCheckUsingAlias.cpp +++ b/applications/projects/SceneChecking/src/SceneChecking/SceneCheckUsingAlias.cpp @@ -40,7 +40,7 @@ using sofa::core::ObjectFactory; SceneCheckUsingAlias::SceneCheckUsingAlias() { /// Add a callback to be n - ObjectFactory::getInstance()->setCallback([this](Base* o, BaseObjectDescription *arg) { + ObjectFactory::getInstance()->setOnCreateCallback([this](Base* o, BaseObjectDescription *arg) { const std::string typeNameInScene = arg->getAttribute("type", ""); if ( typeNameInScene != o->getClassName() ) { From 30f81e4766e746ab09093a0ff63cdffac4ffac4d Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Wed, 8 Oct 2025 11:43:49 +0200 Subject: [PATCH 08/10] Add SVD based null space regularization --- .../solver/BuiltConstraintSolver.cpp | 78 +++++++++++++++++++ .../lagrangian/solver/BuiltConstraintSolver.h | 2 + .../solver/GenericConstraintSolver.h | 2 +- ...raint_with_svd_regularization_solvable.scn | 78 +++++++++++++++++++ 4 files changed, 159 insertions(+), 1 deletion(-) create mode 100644 examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_svd_regularization_solvable.scn diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp index a2e36c175a1..e8424e5af63 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp @@ -25,11 +25,13 @@ #include #include #include +#include namespace sofa::component::constraint::lagrangian::solver { BuiltConstraintSolver::BuiltConstraintSolver() : d_multithreading(initData(&d_multithreading, false, "multithreading", "Build compliances concurrently")) +, d_useSVDForRegularization(initData(&d_useSVDForRegularization, false, "useSVDForRegularization", "Use SVD decomposiiton of the compliance matrix to project singular values smaller than regularization to the regularization term. Only works with built")) {} void BuiltConstraintSolver::init() @@ -41,6 +43,82 @@ void BuiltConstraintSolver::init() } } +void BuiltConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization) +{ + + if (d_useSVDForRegularization.getValue()) + { + if (regularization>std::numeric_limits::epsilon()) + { + auto * FullW = dynamic_cast * >(&W); + const size_t problemSize = FullW->rowSize(); + + Eigen::Map> EigenW(FullW->ptr(),problemSize, problemSize) ; + Eigen::JacobiSVD svd( EigenW, Eigen::ComputeFullV | Eigen::ComputeFullU ); + + + + //Given the SVD, loop over all singular values, and those that are smaller than 1% of the highest one are considered to be the null space + std::vector nullSpaceIndicator(problemSize, false); + int nullSpaceBegin = -1; + for(size_t i=0; i indic; + } + + if (f_printLog.getValue()) + { + std::stringstream msg ; + msg <<"Unregularized diagonal : "; + for(size_t i=0; i d_multithreading; ///< Build compliances concurrently + Data d_useSVDForRegularization; ///< Use SVD decomposiiton of the compliance matrix to project singular values smaller than regularization to the regularization term. Only works with built BuiltConstraintSolver(); @@ -45,6 +46,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintSolver : pu protected: virtual void doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints) override; + virtual void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization) override; private: diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h index 3cd0993a7e1..fe86e0ac972 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h @@ -123,7 +123,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver : virtual void doSolve( GenericConstraintProblem * problem, SReal timeout = 0.0) = 0; - static void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization); + virtual void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization); private: diff --git a/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_svd_regularization_solvable.scn b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_svd_regularization_solvable.scn new file mode 100644 index 00000000000..059c065fca2 --- /dev/null +++ b/examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_svd_regularization_solvable.scn @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 2c1ac452981e7187d3b9956a47bbeeb2b748510c Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Thu, 9 Oct 2025 14:53:59 +0200 Subject: [PATCH 09/10] Change null space criteria to have one homogenized for all vectors --- .../constraint/lagrangian/solver/BuiltConstraintSolver.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp index e8424e5af63..a0b58f493f8 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp @@ -71,13 +71,12 @@ void BuiltConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W, cons } //Now for all vector of the null space basis, we look at the indices where the coefficient - //is greater than 1% of the highest value of the vector, this is the constraints that + //is greater than 1% of the norm of the vector, this is the constraints that //belong to the null space and thus have other one that are antagonists for(int i=nullSpaceBegin; (i != -1) && (i indic; + nullSpaceIndicator[j] = nullSpaceIndicator[j] || fabs(svd.matrixV().col(i)(j)) > 0.001; } if (f_printLog.getValue()) From 35a3159c0f09d5cdd57bcbeeab2f7f066e0e39d3 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Wed, 22 Oct 2025 17:08:59 +0200 Subject: [PATCH 10/10] Add check on dynamic cast --- .../constraint/lagrangian/solver/BuiltConstraintSolver.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp index a0b58f493f8..92e1c66640e 100644 --- a/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp +++ b/Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.cpp @@ -51,6 +51,11 @@ void BuiltConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W, cons if (regularization>std::numeric_limits::epsilon()) { auto * FullW = dynamic_cast * >(&W); + if (FullW == nullptr) + { + msg_error()<<"BuiltConstraintSolver expect a LPtrFullMatrix for W but didn't receive one. The constraint problem is wrong. Please fix the code or just deactivate SVD regularization."; + return; + } const size_t problemSize = FullW->rowSize(); Eigen::Map> EigenW(FullW->ptr(),problemSize, problemSize) ;