diff --git a/Sofa/Component/ODESolver/Backward/CMakeLists.txt b/Sofa/Component/ODESolver/Backward/CMakeLists.txt index 7721bac6c11..f7e28cd92d6 100644 --- a/Sofa/Component/ODESolver/Backward/CMakeLists.txt +++ b/Sofa/Component/ODESolver/Backward/CMakeLists.txt @@ -10,6 +10,7 @@ set(HEADER_FILES ${SOFACOMPONENTODESOLVERBACKWARD_SOURCE_DIR}/StaticSolver.h ${SOFACOMPONENTODESOLVERBACKWARD_SOURCE_DIR}/VariationalSymplecticSolver.h ${SOFACOMPONENTODESOLVERBACKWARD_SOURCE_DIR}/NewmarkImplicitSolver.h + ${SOFACOMPONENTODESOLVERBACKWARD_SOURCE_DIR}/NewtonRaphsonSolver.h ) set(SOURCE_FILES @@ -18,6 +19,7 @@ set(SOURCE_FILES ${SOFACOMPONENTODESOLVERBACKWARD_SOURCE_DIR}/StaticSolver.cpp ${SOFACOMPONENTODESOLVERBACKWARD_SOURCE_DIR}/VariationalSymplecticSolver.cpp ${SOFACOMPONENTODESOLVERBACKWARD_SOURCE_DIR}/NewmarkImplicitSolver.cpp + ${SOFACOMPONENTODESOLVERBACKWARD_SOURCE_DIR}/NewtonRaphsonSolver.cpp ) sofa_find_package(Sofa.Simulation.Core REQUIRED) diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewtonRaphsonSolver.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewtonRaphsonSolver.cpp new file mode 100644 index 00000000000..bbd09ddcac5 --- /dev/null +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewtonRaphsonSolver.cpp @@ -0,0 +1,471 @@ +/****************************************************************************** +* 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::odesolver::backward +{ + +static constexpr NewtonRaphsonSolver::NewtonStatus defaultStatus("Undefined"); + +NewtonRaphsonSolver::NewtonRaphsonSolver() + : l_integrationMethod(initLink("integrationMethod", "The integration method to use in a Newton iteration")) + , d_maxNbIterationsNewton(initData(&d_maxNbIterationsNewton, 1u, "maxNbIterationsNewton", + "Maximum number of iterations of the Newton's method if it has not converged.")) + , d_relativeSuccessiveStoppingThreshold(initData(&d_relativeSuccessiveStoppingThreshold, 1e-5_sreal, + "relativeSuccessiveStoppingThreshold", + "Threshold for the relative successive progress criterion. The Newton " + "iterations will stop when the ratio between the norm of the residual " + "at iteration k over the norm of the residual at iteration k-1 is lower" + " than this threshold.")) + , d_relativeInitialStoppingThreshold(initData(&d_relativeInitialStoppingThreshold, 1e-5_sreal, + "relativeInitialStoppingThreshold", + "Threshold for the relative initial progress criterion. The Newton" + " iterations will stop when the ratio between the norm of the residual " + "at iteration k over the norm of the residual at iteration 0 is" + " lower than this threshold. This criterion tracks the overall progress " + "made since the beginning of the iteration process. If the ratio is " + "significantly smaller than 1, it indicates that the iterative process " + "is making substantial progress, and the method is converging toward the" + " root.")) + , d_absoluteResidualStoppingThreshold(initData(&d_absoluteResidualStoppingThreshold, 1e-5_sreal, + "absoluteResidualStoppingThreshold", + "Threshold for the absolute function value stopping criterion. The " + "Newton iterations will stop when the norm of the residual at iteration " + "k is lower than this threshold. This criterion indicates the current " + "iteration found an estimate close to the root.")) + , d_relativeEstimateDifferenceThreshold(initData(&d_relativeEstimateDifferenceThreshold, 0_sreal, + "relativeEstimateDifferenceThreshold", + "Threshold for the relative change in root estimate criterion. The " + "Newton iterations will stop when the difference between two successive " + "estimates divided by the previous estimate is smaller than this threshold")) + , d_absoluteEstimateDifferenceThreshold(initData(&d_absoluteEstimateDifferenceThreshold, 0_sreal, + "absoluteEstimateDifferenceThreshold", + "Threshold for the absolute change in root estimate criterion. The " + "Newton iterations will stop when the difference between two successive " + "estimates is smaller than this threshold.")) + , d_maxNbIterationsLineSearch(initData(&d_maxNbIterationsLineSearch, 5u, "maxNbIterationsLineSearch", + "Maximum number of iterations of the line search method if it has not converged.")) + , d_lineSearchCoefficient(initData(&d_lineSearchCoefficient, 0.5_sreal, "lineSearchCoefficient", "Line search coefficient")) + , d_status(initData(&d_status, defaultStatus, "status", ("status\n" + NewtonStatus::dataDescription()).c_str())) +{ + d_status.setReadOnly(true); +} + +void NewtonRaphsonSolver::init() +{ + OdeSolver::init(); + LinearSolverAccessor::init(); + + if (!l_integrationMethod.get()) + { + l_integrationMethod.set(getContext()->get(getContext()->getTags(), core::objectmodel::BaseContext::SearchDown)); + + if (!l_integrationMethod) + { + msg_error() << "An integration method is required by this component but has not been found."; + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } + } + + d_status.setValue(defaultStatus); + + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); +} + +void NewtonRaphsonSolver::reset() +{ + d_status.setValue(defaultStatus); +} + +void NewtonRaphsonSolver::computeRightHandSide(const core::ExecParams* params, SReal dt, + core::MultiVecDerivId force, + core::MultiVecDerivId b, + core::MultiVecDerivId velocity_i, + core::MultiVecCoordId position_i) const +{ + core::behavior::RHSInput input; + input.intermediateVelocity = velocity_i; + input.intermediatePosition = position_i; + + l_integrationMethod->computeRightHandSide(params, input, force, b, dt); +} + +SReal NewtonRaphsonSolver::computeResidual(const core::ExecParams* params, sofa::simulation::common::MechanicalOperations& mop, + SReal dt, core::MultiVecDerivId force, + core::MultiVecDerivId oldVelocity, + core::MultiVecDerivId newVelocity) +{ + sofa::simulation::common::VectorOperations vop( params, this->getContext() ); + + core::behavior::MultiVecDeriv residual(&vop, true, core::VecIdProperties{"residual", GetClass()->className}); + + core::behavior::MultiVecDeriv tmp(&vop); + + vop.v_eq(tmp, newVelocity); + vop.v_peq(tmp, oldVelocity, -1); + mop.addMdx(residual, tmp); + + vop.v_peq(residual, force, -dt); + + vop.v_dot(residual, residual); + return vop.finish(); +} + +void NewtonRaphsonSolver::solve( + const core::ExecParams* params, SReal dt, + sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) +{ + if (!isComponentStateValid()) + { + return; + } + + const bool printLog = f_printLog.getValue(); + + static constexpr auto running = NewtonStatus("Running"); + d_status.setValue(running); + + // Create the vector and mechanical operations tools. These are used to execute special operations (multiplication, + // additions, etc.) on multi-vectors (a vector that is stored in different buffers inside the mechanical objects) + sofa::simulation::common::VectorOperations vop( params, this->getContext() ); + sofa::simulation::common::MechanicalOperations mop( params, this->getContext() ); + mop->setImplicit(true); + + //force vector that will be computed in this solve. The content of the + //previous time step will be erased. + core::behavior::MultiVecDeriv force(&vop, core::vec_id::write_access::force ); + core::behavior::MultiVecDeriv b(&vop, true, core::VecIdProperties{"RHS", GetClass()->className}); + + core::behavior::MultiVecDeriv velocityPrevious(&vop); + velocityPrevious.eq(vResult); + + //the intermediate position and velocity required by the Newton's algorithm + core::behavior::MultiVecCoord position_i(&vop); + core::behavior::MultiVecDeriv velocity_i(&vop); + + //initial guess: the new states are initialized with states from the previous time step + position_i.eq(xResult); + velocity_i.eq(vResult); + + //the position and velocity that will be computed at the end of this algorithm + core::behavior::MultiVecCoord newPosition(&vop, xResult ); + core::behavior::MultiVecDeriv newVelocity(&vop, vResult ); + + //dx vector is required by some operations of the algorithm, even if it is + //not explicit + core::behavior::MultiVecDeriv dx(&vop, sofa::core::vec_id::write_access::dx); + dx.realloc(&vop, false, true); + + m_linearSystemSolution.realloc(&vop, false, true, + core::VecIdProperties{"solution", GetClass()->className}); + + // inform the constraint parameters about the position and velocity id + mop.cparams.setX(xResult); + mop.cparams.setV(vResult); + + l_integrationMethod->initializeVectors(params, + core::vec_id::read_access::position, + core::vec_id::read_access::velocity); + + { + SCOPED_TIMER("ComputeRHS"); + computeRightHandSide(params, dt, force, b, velocity_i, position_i); + } + + SReal squaredResidualNorm{}; + { + SCOPED_TIMER("ComputeError"); + squaredResidualNorm = this->computeResidual(params, mop, dt, force, velocityPrevious, velocity_i); + } + + const auto absoluteStoppingThreshold = d_absoluteResidualStoppingThreshold.getValue(); + const auto squaredAbsoluteStoppingThreshold = std::pow(absoluteStoppingThreshold, 2); + + if (absoluteStoppingThreshold > 0 && squaredResidualNorm <= squaredAbsoluteStoppingThreshold) + { + msg_info() << "The ODE has already reached an equilibrium state. " + << "The residual squared norm is " << squaredResidualNorm << ". " + << "The threshold for convergence is " << squaredAbsoluteStoppingThreshold; + static constexpr auto convergedEquilibrium = NewtonStatus("ConvergedEquilibrium"); + d_status.setValue(convergedEquilibrium); + } + else + { + SCOPED_TIMER("NewtonsIterations"); + + const auto relativeSuccessiveStoppingThreshold = d_relativeSuccessiveStoppingThreshold.getValue(); + const auto relativeInitialStoppingThreshold = d_relativeInitialStoppingThreshold.getValue(); + const auto relativeEstimateDifferenceThreshold = d_relativeEstimateDifferenceThreshold.getValue(); + const auto absoluteEstimateDifferenceThreshold = d_absoluteEstimateDifferenceThreshold.getValue(); + + const auto squaredRelativeSuccessiveStoppingThreshold = std::pow(relativeSuccessiveStoppingThreshold, 2); + const auto squaredRelativeInitialStoppingThreshold = std::pow(relativeInitialStoppingThreshold, 2); + const auto squaredRelativeEstimateDifferenceThreshold = std::pow(relativeEstimateDifferenceThreshold, 2); + const auto squaredAbsoluteEstimateDifferenceThreshold = std::pow(absoluteEstimateDifferenceThreshold, 2); + + const auto maxNbIterationsNewton = d_maxNbIterationsNewton.getValue(); + const auto maxNbIterationsLineSearch = d_maxNbIterationsLineSearch.getValue(); + const auto [mFact, bFact, kFact] = l_integrationMethod->getMatricesFactors(dt); + bool hasConverged = false; + bool hasLineSearchFailed = false; + const auto lineSearchCoefficient = d_lineSearchCoefficient.getValue(); + const auto firstSquaredResidualNorm = squaredResidualNorm; + + unsigned int newtonIterationCount = 0; + for (; newtonIterationCount < maxNbIterationsNewton; ++newtonIterationCount) + { + //assemble the system matrix + { + SCOPED_TIMER("setSystemMBKMatrix"); + mop.setSystemMBKMatrix(mFact, bFact, kFact, l_linearSolver.get()); + } + + //solve the system + { + SCOPED_TIMER("MBKSolve"); + + l_linearSolver->setSystemLHVector(m_linearSystemSolution); + l_linearSolver->setSystemRHVector(b); + l_linearSolver->solveSystem(); + } + + bool lineSearchSuccess = false; + SReal squaredResidualNormLineSearch {}; + SReal totalLineSearchCoefficient { 1_sreal }; + SReal minTotalLineSearchCoefficient { 1_sreal }; + SReal minSquaredResidualNormLineSearch { std::numeric_limits::max() }; + + const auto lineSearch = [&](bool applyCoefficient) + { + if (applyCoefficient) + { + // solution *= lineSearchCoefficient + vop.v_teq(m_linearSystemSolution, lineSearchCoefficient); + totalLineSearchCoefficient *= lineSearchCoefficient; + } + + l_integrationMethod->updateStates(params, dt, + position_i, velocity_i, + newPosition, newVelocity, + m_linearSystemSolution); + + mop.projectPositionAndVelocity(newPosition, newVelocity); + mop.propagateXAndV(newPosition, newVelocity); + + computeRightHandSide(params, dt, force, b, newVelocity, newPosition); + + squaredResidualNormLineSearch = this->computeResidual(params, mop, dt, force, velocityPrevious, newVelocity); + if (squaredResidualNormLineSearch < minSquaredResidualNormLineSearch) + { + minSquaredResidualNormLineSearch = squaredResidualNormLineSearch; + minTotalLineSearchCoefficient = totalLineSearchCoefficient; + } + return squaredResidualNormLineSearch < squaredResidualNorm; + }; + + for (unsigned int lineSearchIterationCount = 0; lineSearchIterationCount < maxNbIterationsLineSearch; ++lineSearchIterationCount) + { + if (lineSearch(lineSearchIterationCount > 0)) + { + lineSearchSuccess = true; + break; + } + } + + if (!lineSearchSuccess) + { + hasLineSearchFailed = true; + msg_warning_when(maxNbIterationsLineSearch > 0) << "Line search failed at Newton iteration " + << newtonIterationCount << ". Using the coefficient " + << minTotalLineSearchCoefficient << " resulting to the minimal residual norm (" << minSquaredResidualNormLineSearch << "). Stopping the iterative process."; + + vop.v_teq(m_linearSystemSolution, minTotalLineSearchCoefficient / totalLineSearchCoefficient); + lineSearch(false); + } + + const auto previousSquaredResidualNorm = squaredResidualNorm; + squaredResidualNorm = squaredResidualNormLineSearch; + + std::stringstream iterationResults; + + if (printLog) + { + iterationResults << "Iteration results:"; + iterationResults << "\n* Current iteration = " << newtonIterationCount; + iterationResults << "\n* Residual = " << std::sqrt(squaredResidualNorm) << " (threshold = " << absoluteStoppingThreshold << ")"; + iterationResults << "\n* Successive relative ratio = " << std::sqrt(squaredResidualNorm / previousSquaredResidualNorm) << " (threshold = " << relativeSuccessiveStoppingThreshold << ", previous residual = " << std::sqrt(previousSquaredResidualNorm) << ")"; + iterationResults << "\n* Initial relative ratio = " << std::sqrt(squaredResidualNorm / firstSquaredResidualNorm) << " (threshold = " << relativeInitialStoppingThreshold << ", initial residual = " << std::sqrt(firstSquaredResidualNorm) << ")"; + } + + if (!lineSearchSuccess) + { + msg_info() << iterationResults.str(); + + static constexpr auto divergedLineSearch = NewtonStatus("DivergedLineSearch"); + d_status.setValue(divergedLineSearch); + + break; + } + + // relative successive convergence + if (relativeSuccessiveStoppingThreshold > 0 && + squaredResidualNorm < squaredRelativeSuccessiveStoppingThreshold * previousSquaredResidualNorm) + { + msg_info() << iterationResults.str(); + msg_info() << "[CONVERGED] residual successive ratio is smaller than " + "the threshold (" << relativeInitialStoppingThreshold + << ") after " << (newtonIterationCount+1) << " Newton iterations."; + hasConverged = true; + + static constexpr auto convergedResidualSuccessiveRatio = NewtonStatus("ConvergedResidualSuccessiveRatio"); + d_status.setValue(convergedResidualSuccessiveRatio); + + break; + } + + // relative initial convergence + if (relativeInitialStoppingThreshold > 0 && + squaredResidualNorm < squaredRelativeInitialStoppingThreshold * firstSquaredResidualNorm) + { + msg_info() << iterationResults.str(); + msg_info() << "[CONVERGED] residual initial ratio is smaller than " + "the threshold (" << relativeInitialStoppingThreshold + << ") after " << (newtonIterationCount+1) << " Newton iterations."; + hasConverged = true; + + static constexpr auto convergedResidualInitialRatio = NewtonStatus("ConvergedResidualInitialRatio"); + d_status.setValue(convergedResidualInitialRatio); + + break; + } + + // absolute convergence + if (absoluteStoppingThreshold > 0 && + squaredResidualNorm <= squaredAbsoluteStoppingThreshold) + { + msg_info() << iterationResults.str(); + msg_info() << "[CONVERGED] residual squared norm (" << + squaredResidualNorm << ") is smaller than the threshold (" + << squaredAbsoluteStoppingThreshold << ") after " + << (newtonIterationCount+1) << " Newton iterations."; + hasConverged = true; + + static constexpr auto convergedAbsoluteResidual = NewtonStatus("ConvergedAbsoluteResidual"); + d_status.setValue(convergedAbsoluteResidual); + + break; + } + + if (absoluteEstimateDifferenceThreshold > 0 || squaredRelativeEstimateDifferenceThreshold > 0) + { + core::behavior::MultiVecDeriv tmp(&vop); + + vop.v_eq(tmp, newVelocity); + vop.v_peq(tmp, velocity_i, -1); + + vop.v_dot(tmp, tmp); + const auto squaredAbsoluteDifference = vop.finish(); + + if (printLog) + { + iterationResults << "\n* Successive estimate difference = " << std::sqrt(squaredAbsoluteDifference); + } + + if (squaredRelativeEstimateDifferenceThreshold > 0) + { + vop.v_dot(velocity_i, velocity_i); + const auto squaredPreviousVelocity = vop.finish(); + + if (squaredPreviousVelocity != 0) + { + if (printLog) + { + iterationResults << "\n* Relative estimate difference = " << std::sqrt(squaredAbsoluteDifference / squaredPreviousVelocity); + } + + if (squaredAbsoluteDifference < squaredPreviousVelocity * squaredRelativeEstimateDifferenceThreshold) + { + msg_info() << iterationResults.str(); + msg_info() << "[CONVERGED] relative successive estimate difference (" << + std::sqrt(squaredAbsoluteDifference / squaredPreviousVelocity) + << ") is smaller than the threshold (" + << squaredRelativeEstimateDifferenceThreshold << ") after " + << (newtonIterationCount+1) << " Newton iterations."; + hasConverged = true; + + static constexpr auto convergedRelativeEstimateDifference = NewtonStatus("ConvergedRelativeEstimateDifference"); + d_status.setValue(convergedRelativeEstimateDifference); + + break; + } + } + } + + if (squaredAbsoluteDifference < squaredAbsoluteEstimateDifferenceThreshold) + { + msg_info() << iterationResults.str(); + msg_info() << "[CONVERGED] absolute successive estimate difference (" << + std::sqrt(squaredAbsoluteDifference) << ") is smaller than the threshold (" + << squaredAbsoluteEstimateDifferenceThreshold << ") after " + << (newtonIterationCount+1) << " Newton iterations."; + hasConverged = true; + + static constexpr auto convergedAbsoluteEstimateDifference = NewtonStatus("ConvergedAbsoluteEstimateDifference"); + d_status.setValue(convergedAbsoluteEstimateDifference); + + break; + } + } + + vop.v_eq(position_i, newPosition); + vop.v_eq(velocity_i, newVelocity); + + msg_info() << iterationResults.str(); + } + + if (!hasConverged) + { + msg_warning() << "Newton's method failed to converge after " << newtonIterationCount + << " iterations with residual squared norm = " << squaredResidualNorm << ". "; + + if (!hasLineSearchFailed) + { + static constexpr auto divergedMaxIterations = NewtonStatus("DivergedMaxIterations"); + d_status.setValue(divergedMaxIterations); + } + } + } +} + +void registerNewtonRaphsonSolver(sofa::core::ObjectFactory* factory) +{ + factory->registerObjects(core::ObjectRegistrationData("Generic Newton-Raphson algorithm.") + .add< NewtonRaphsonSolver >()); +} + +} diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewtonRaphsonSolver.h b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewtonRaphsonSolver.h new file mode 100644 index 00000000000..61f0be23e49 --- /dev/null +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewtonRaphsonSolver.h @@ -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 * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include + + +namespace sofa::component::odesolver::backward +{ + +class SOFA_COMPONENT_ODESOLVER_BACKWARD_API NewtonRaphsonSolver + : public sofa::core::behavior::OdeSolver + , public sofa::core::behavior::LinearSolverAccessor +{ +public: + SOFA_CLASS2(NewtonRaphsonSolver, sofa::core::behavior::OdeSolver, sofa::core::behavior::LinearSolverAccessor); + + void init() override; + void reset() override; + void solve(const core::ExecParams* params, SReal dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) override; + + SingleLink l_integrationMethod; + + Data d_maxNbIterationsNewton; + Data d_relativeSuccessiveStoppingThreshold; + Data d_relativeInitialStoppingThreshold; + Data d_absoluteResidualStoppingThreshold; + Data d_relativeEstimateDifferenceThreshold; + Data d_absoluteEstimateDifferenceThreshold; + Data d_maxNbIterationsLineSearch; + Data d_lineSearchCoefficient; + + MAKE_SELECTABLE_ITEMS(NewtonStatus, + sofa::helper::Item{"Undefined", "The solver has not been called yet"}, + sofa::helper::Item{"Running", "The solver is still running and/or did not finish"}, + sofa::helper::Item{"ConvergedEquilibrium", "Converged: the iterations did not start because the system is already at equilibrium"}, + sofa::helper::Item{"DivergedLineSearch", "Diverged: line search failed"}, + sofa::helper::Item{"DivergedMaxIterations", "Diverged: Reached the maximum number of iterations"}, + sofa::helper::Item{"ConvergedResidualSuccessiveRatio", "Converged: Residual successive ratio is smaller than the threshold"}, + sofa::helper::Item{"ConvergedResidualInitialRatio", "Converged: Residual initial ratio is smaller than the threshold"}, + sofa::helper::Item{"ConvergedAbsoluteResidual", "Converged: Absolute residual is smaller than the threshold"}, + sofa::helper::Item{"ConvergedRelativeEstimateDifference", "Converged: Relative estimate difference is smaller than the threshold"}, + sofa::helper::Item{"ConvergedAbsoluteEstimateDifference", "Converged: Absolute estimate difference is smaller than the threshold"}); + + Data d_status; + +protected: + NewtonRaphsonSolver(); + + core::behavior::MultiVecDeriv m_linearSystemSolution; + + void computeRightHandSide(const core::ExecParams* params, SReal dt, + core::MultiVecDerivId force, + core::MultiVecDerivId b, + core::MultiVecDerivId velocity_i, + core::MultiVecCoordId position_i) const; + + + SReal computeResidual(const core::ExecParams* params, sofa::simulation::common::MechanicalOperations& mop, SReal dt, + core::MultiVecDerivId force, + core::MultiVecDerivId oldVelocity, core::MultiVecDerivId newVelocity); +}; + +} diff --git a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/init.cpp b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/init.cpp index 8856a388a25..8aa55426071 100644 --- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/init.cpp +++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/init.cpp @@ -30,6 +30,7 @@ extern void registerEulerImplicitSolver(sofa::core::ObjectFactory* factory); extern void registerNewmarkImplicitSolver(sofa::core::ObjectFactory* factory); extern void registerStaticSolver(sofa::core::ObjectFactory* factory); extern void registerVariationalSymplecticSolver(sofa::core::ObjectFactory* factory); +extern void registerNewtonRaphsonSolver(sofa::core::ObjectFactory* factory); extern "C" { SOFA_EXPORT_DYNAMIC_LIBRARY void initExternalModule(); @@ -59,6 +60,7 @@ void registerObjects(sofa::core::ObjectFactory* factory) registerNewmarkImplicitSolver(factory); registerStaticSolver(factory); registerVariationalSymplecticSolver(factory); + registerNewtonRaphsonSolver(factory); } void init() diff --git a/Sofa/Component/ODESolver/Backward/tests/CMakeLists.txt b/Sofa/Component/ODESolver/Backward/tests/CMakeLists.txt index b382bdf8d4e..b5226a7a40f 100644 --- a/Sofa/Component/ODESolver/Backward/tests/CMakeLists.txt +++ b/Sofa/Component/ODESolver/Backward/tests/CMakeLists.txt @@ -7,6 +7,7 @@ set(SOURCE_FILES EulerImplicitSolverStatic_test.cpp EulerImplicitSolver_withDamping_test.cpp NewmarkImplicitSolverDynamic_test.cpp + NewtonRaphsonSolver_test.cpp StaticSolver_test.cpp SpringSolverDynamic_test.cpp VariationalSymplecticExplicitSolverDynamic_test.cpp diff --git a/Sofa/Component/ODESolver/Backward/tests/NewtonRaphsonSolver_test.cpp b/Sofa/Component/ODESolver/Backward/tests/NewtonRaphsonSolver_test.cpp new file mode 100644 index 00000000000..9da8e510a0e --- /dev/null +++ b/Sofa/Component/ODESolver/Backward/tests/NewtonRaphsonSolver_test.cpp @@ -0,0 +1,212 @@ +/****************************************************************************** +* 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 +{ + +struct NewtonRaphsonParameters +{ + SReal relativeSuccessiveStoppingThreshold {}; + SReal relativeInitialStoppingThreshold {}; + SReal absoluteResidualStoppingThreshold {}; + SReal relativeEstimateDifferenceThreshold {}; + SReal absoluteEstimateDifferenceThreshold {}; + unsigned int maxNbIterationsLineSearch = 1; + unsigned int maxNbIterationsNewton = 1; + + void apply(component::odesolver::backward::NewtonRaphsonSolver* solver) const + { + solver->d_relativeSuccessiveStoppingThreshold.setValue(relativeSuccessiveStoppingThreshold); + solver->d_relativeInitialStoppingThreshold.setValue(relativeInitialStoppingThreshold); + solver->d_absoluteResidualStoppingThreshold.setValue(absoluteResidualStoppingThreshold); + solver->d_relativeEstimateDifferenceThreshold.setValue(relativeEstimateDifferenceThreshold); + solver->d_absoluteEstimateDifferenceThreshold.setValue(absoluteEstimateDifferenceThreshold); + solver->d_maxNbIterationsLineSearch.setValue(maxNbIterationsLineSearch); + solver->d_maxNbIterationsNewton.setValue(maxNbIterationsNewton); + } +}; + +struct NewtonRaphsonTest : public testing::BaseSimulationTest +{ + SceneInstance m_scene{}; + + component::odesolver::backward::NewtonRaphsonSolver* m_solver { nullptr }; + component::statecontainer::MechanicalObject* m_state { nullptr }; + + void onSetUp() override + { + sofa::simpleapi::importPlugin("Sofa.Component.LinearSolver.Direct"); + sofa::simpleapi::importPlugin("Sofa.Component.ODESolver.Backward"); + sofa::simpleapi::importPlugin("Sofa.Component.ODESolver.Integration"); + sofa::simpleapi::importPlugin("Sofa.Component.StateContainer"); + sofa::simpleapi::importPlugin("Sofa.Component.Topology.Container.Dynamic"); + + sofa::simpleapi::createObject(m_scene.root, "DefaultAnimationLoop"); + sofa::simpleapi::createObject(m_scene.root, "BDF1"); + auto solverObject = sofa::simpleapi::createObject(m_scene.root, "NewtonRaphsonSolver", {{"printLog", "true"}}); + m_solver = dynamic_cast(solverObject.get()); + sofa::simpleapi::createObject(m_scene.root, "EigenSimplicialLDLT", {{"template", "CompressedRowSparseMatrix"}}); + auto stateObject = sofa::simpleapi::createObject(m_scene.root, "MechanicalObject", {{"template", "Vec1"}, {"position", "1"}, {"rest_position", "0"}}); + m_state = dynamic_cast*>(stateObject.get()); + sofa::simpleapi::createObject(m_scene.root, "PointSetTopologyContainer", {{"name", "topology"}, {"position", "1"}}); + + ASSERT_NE(nullptr, m_solver); + ASSERT_NE(nullptr, m_state); + } + + void noForce() + { + m_scene.initScene(); + EXPECT_EQ(m_solver->d_status.getValue(), component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus("Undefined")); + + m_scene.simulate(0.01_sreal); + EXPECT_EQ(m_solver->d_status.getValue(), component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus("ConvergedEquilibrium")); + } + + void gravity(const NewtonRaphsonParameters& params, const component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus& expectedStatus) + { + static constexpr SReal gravity = -1; + m_scene.root->setGravity({gravity, 0, 0}); + + sofa::simpleapi::importPlugin("Sofa.Component.Mass"); + sofa::simpleapi::createObject(m_scene.root, "UniformMass", {{"totalMass", "1"}, {"topology", "@topology"}, {"printLog", "true"}}); + params.apply(m_solver); + + m_scene.initScene(); + EXPECT_EQ(m_solver->d_status.getValue(), component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus("Undefined")); + + auto previousVelocity = m_state->read(sofa::core::vec_id::read_access::velocity)->getValue(); + auto previousPosition = m_state->read(sofa::core::vec_id::read_access::position)->getValue(); + + static constexpr SReal dt = 0.1_sreal; + m_scene.simulate(dt); + EXPECT_EQ(m_solver->d_status.getValue(), expectedStatus); + + const auto& velocity = m_state->read(sofa::core::vec_id::read_access::velocity)->getValue(); + const auto& position = m_state->read(sofa::core::vec_id::read_access::position)->getValue(); + + EXPECT_FLOATINGPOINT_EQ(velocity[0].x(), gravity * dt) + EXPECT_FLOATINGPOINT_EQ(position[0].x(), previousPosition[0].x() + velocity[0].x() * dt); + + for (unsigned int i = 1; i < 10; ++i) + { + previousVelocity = m_state->read(sofa::core::vec_id::read_access::velocity)->getValue(); + previousPosition = m_state->read(sofa::core::vec_id::read_access::position)->getValue(); + + m_scene.simulate(dt); + EXPECT_FLOATINGPOINT_EQ(velocity[0].x(), previousVelocity[0].x() + gravity * dt); + + EXPECT_FLOATINGPOINT_EQ(position[0].x(), previousPosition[0].x() + velocity[0].x() * dt); + } + } + + void spring() + { + sofa::simpleapi::importPlugin("Sofa.Component.SolidMechanics.Spring"); + sofa::simpleapi::createObject(m_scene.root, "RestShapeSpringsForceField", {{"points", "0"}}); + + m_scene.initScene(); + EXPECT_EQ(m_solver->d_status.getValue(), component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus("Undefined")); + + m_scene.simulate(0.01_sreal); + } + +}; + +TEST_F(NewtonRaphsonTest, noForce) +{ + this->noForce(); +} + +TEST_F(NewtonRaphsonTest, gravity_noStopping) +{ + // no stopping criteria is defined so it cannot converge + this->gravity( + NewtonRaphsonParameters{}, + component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus("DivergedMaxIterations")); +} + +TEST_F(NewtonRaphsonTest, gravity_relativeSuccessiveStopping) +{ + NewtonRaphsonParameters params; + params.relativeSuccessiveStoppingThreshold = 1e-15_sreal; + + this->gravity( + params, + component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus("ConvergedResidualSuccessiveRatio")); +} + +TEST_F(NewtonRaphsonTest, gravity_relativeInitialStopping) +{ + NewtonRaphsonParameters params; + params.relativeInitialStoppingThreshold = 1e-15_sreal; + + this->gravity( + params, + component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus("ConvergedResidualInitialRatio")); +} + +TEST_F(NewtonRaphsonTest, gravity_absoluteResidualStopping) +{ + NewtonRaphsonParameters params; + params.absoluteResidualStoppingThreshold = 1e-15_sreal; + + this->gravity( + params, + component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus("ConvergedAbsoluteResidual")); +} + +TEST_F(NewtonRaphsonTest, gravity_relativeEstimateDifferenceStopping) +{ + NewtonRaphsonParameters params; + params.relativeEstimateDifferenceThreshold = 1e-15_sreal; + + // considered as diverged because this threshold requires more than one iteration + this->gravity( + params, + component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus("DivergedMaxIterations")); +} + +TEST_F(NewtonRaphsonTest, gravity_absoluteEstimateDifferenceStopping) +{ + NewtonRaphsonParameters params; + params.absoluteEstimateDifferenceThreshold = 1e-15_sreal; + + // considered as diverged because this threshold requires more than one iteration + this->gravity( + params, + component::odesolver::backward::NewtonRaphsonSolver::NewtonStatus("DivergedMaxIterations")); +} + +TEST_F(NewtonRaphsonTest, spring) +{ + this->spring(); +} + + +} diff --git a/Sofa/Component/ODESolver/CMakeLists.txt b/Sofa/Component/ODESolver/CMakeLists.txt index de2fd4732a2..588f45e258e 100644 --- a/Sofa/Component/ODESolver/CMakeLists.txt +++ b/Sofa/Component/ODESolver/CMakeLists.txt @@ -4,7 +4,7 @@ project(Sofa.Component.ODESolver LANGUAGES CXX) set(SOFACOMPONENTODESOLVER_SOURCE_DIR "src/sofa/component/odesolver") sofa_add_subdirectory_modules(SOFACOMPONENTODESOLVER_TARGETS - DIRECTORIES Forward Backward + DIRECTORIES Forward Backward Integration ) set(HEADER_FILES diff --git a/Sofa/Component/ODESolver/Integration/CMakeLists.txt b/Sofa/Component/ODESolver/Integration/CMakeLists.txt new file mode 100644 index 00000000000..ecc3e79e1c4 --- /dev/null +++ b/Sofa/Component/ODESolver/Integration/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.22) +project(Sofa.Component.ODESolver.Integration LANGUAGES CXX) + +set(SOFACOMPONENTODESOLVERINTEGRATION_SOURCE_DIR "src/sofa/component/odesolver/integration") + +set(HEADER_FILES + ${SOFACOMPONENTODESOLVERINTEGRATION_SOURCE_DIR}/config.h.in + ${SOFACOMPONENTODESOLVERINTEGRATION_SOURCE_DIR}/init.h + + ${SOFACOMPONENTODESOLVERINTEGRATION_SOURCE_DIR}/BDF1.h +) + +set(SOURCE_FILES + ${SOFACOMPONENTODESOLVERINTEGRATION_SOURCE_DIR}/init.cpp + + ${SOFACOMPONENTODESOLVERINTEGRATION_SOURCE_DIR}/BDF1.cpp +) + +sofa_find_package(Sofa.Simulation.Core REQUIRED) + +add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES}) +target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Simulation.Core) + +sofa_create_package_with_targets( + PACKAGE_NAME ${PROJECT_NAME} + PACKAGE_VERSION ${Sofa_VERSION} + TARGETS ${PROJECT_NAME} AUTO_SET_TARGET_PROPERTIES + INCLUDE_SOURCE_DIR "src" + INCLUDE_INSTALL_DIR "${PROJECT_NAME}" +) + +#cmake_dependent_option(SOFA_COMPONENT_ODESOLVER_INTEGRATION_BUILD_TESTS "Compile the automatic tests" ON "SOFA_BUILD_TESTS OR NOT DEFINED SOFA_BUILD_TESTS" OFF) +#if(SOFA_COMPONENT_ODESOLVER_INTEGRATION_BUILD_TESTS) +# add_subdirectory(tests) +#endif() diff --git a/Sofa/Component/ODESolver/Integration/Sofa.Component.ODESolver.IntegrationConfig.cmake.in b/Sofa/Component/ODESolver/Integration/Sofa.Component.ODESolver.IntegrationConfig.cmake.in new file mode 100644 index 00000000000..680c31106ed --- /dev/null +++ b/Sofa/Component/ODESolver/Integration/Sofa.Component.ODESolver.IntegrationConfig.cmake.in @@ -0,0 +1,12 @@ +# CMake package configuration file for the @PROJECT_NAME@ module + +@PACKAGE_GUARD@ +@PACKAGE_INIT@ + +find_package(Sofa.Simulation.Core QUIET REQUIRED) + +if(NOT TARGET @PROJECT_NAME@) + include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") +endif() + +check_required_components(@PROJECT_NAME@) diff --git a/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/BDF1.cpp b/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/BDF1.cpp new file mode 100644 index 00000000000..76838137203 --- /dev/null +++ b/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/BDF1.cpp @@ -0,0 +1,146 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#include +#include +#include +#include +#include +#include + + +namespace sofa::component::odesolver::integration +{ + +void registerBDF1(sofa::core::ObjectFactory* factory) +{ + factory->registerObjects(core::ObjectRegistrationData("Velocity-based Backward Euler integration method.") + .add< BDF1 >()); +} + + +BDF1::BDF1() + : d_rayleighStiffness(initData(&d_rayleighStiffness, 0_sreal, "rayleighStiffness", "Rayleigh damping coefficient related to stiffness, > 0") ) + , d_rayleighMass(initData(&d_rayleighMass, 0_sreal, "rayleighMass", "Rayleigh damping coefficient related to mass, > 0")) +{} + +void BDF1::initializeVectors(const core::ExecParams* params, core::ConstMultiVecCoordId x, core::ConstMultiVecDerivId v) +{ + m_vop = std::make_unique(params, this->getContext()); + + m_oldPosition.realloc(m_vop.get(), false, true, core::VecIdProperties{"oldPosition", GetClass()->className}); + m_oldVelocity.realloc(m_vop.get(), false, true, core::VecIdProperties{"oldVelocity", GetClass()->className}); + + m_vop->v_eq(m_oldPosition, x); + m_vop->v_eq(m_oldVelocity, v); +} + +core::behavior::BaseIntegrationMethod::Factors BDF1::getMatricesFactors(SReal dt) const +{ + return { + core::MatricesFactors::M{1 + d_rayleighMass.getValue() * dt}, + core::MatricesFactors::B{-dt}, + core::MatricesFactors::K{-dt * (d_rayleighStiffness.getValue() + dt)} + }; +} + +void BDF1::computeRightHandSide( + const core::ExecParams* params, + core::behavior::RHSInput input, + core::MultiVecDerivId force, + core::MultiVecDerivId rightHandSide, + SReal dt) +{ + sofa::simulation::common::MechanicalOperations mop( params, this->getContext() ); + mop->setImplicit(true); + + const auto alpha = d_rayleighMass.getValue(); + const auto beta = d_rayleighStiffness.getValue(); + + { + SCOPED_TIMER("ComputeForce"); + static constexpr bool clearForcesBeforeComputingThem = true; + static constexpr bool applyBottomUpMappings = true; + + mop.mparams.setX(input.intermediatePosition); + mop.mparams.setV(input.intermediateVelocity); + mop.computeForce(force, + clearForcesBeforeComputingThem, applyBottomUpMappings); + } + + // b += dt (-alpha M + beta K) v_i + { + const auto backupV = mop.mparams.v(); + { + mop.mparams.setV(input.intermediateVelocity); + mop.addMBKv(force, + core::MatricesFactors::M(-alpha * dt), + core::MatricesFactors::B(0), + core::MatricesFactors::K(beta * dt)); + } + mop.mparams.setV(backupV); + } + + // b = dt * f + m_vop->v_eq(rightHandSide, force, dt); + + // b += M * (v_n - v_i) + { + core::behavior::MultiVecDeriv tmp(m_vop.get()); + tmp.eq(m_oldVelocity); + m_vop->v_peq(tmp, input.intermediateVelocity, -1); //(v_n - v_i) + + mop.addMdx(rightHandSide, tmp, core::MatricesFactors::M(1).get()); + } + + // b += (dt^2 K) * v + { + const auto backupV = mop.mparams.v(); + { + mop.mparams.setV(m_oldVelocity); + mop.addMBKv(rightHandSide, + core::MatricesFactors::M(0), + core::MatricesFactors::B(0), + core::MatricesFactors::K(dt * dt)); + } + mop.mparams.setV(backupV); + } + + // Apply projective constraints + mop.projectResponse(rightHandSide); + +} + +void BDF1::updateStates(const core::ExecParams* params, SReal dt, + core::MultiVecCoordId x, core::MultiVecDerivId v, + core::MultiVecCoordId newX, core::MultiVecDerivId newV, + core::MultiVecDerivId linearSystemSolution) +{ + // v_(i+1) = v_i + x + m_vop->v_eq(newV, v); + m_vop->v_peq(newV, linearSystemSolution); + + // x_(i+1) = dt * v_(i+1) + x_i + m_vop->v_eq(newX, x); + m_vop->v_peq(newX, newV, dt); +} + +} diff --git a/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/BDF1.h b/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/BDF1.h new file mode 100644 index 00000000000..b41e28bc0a4 --- /dev/null +++ b/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/BDF1.h @@ -0,0 +1,73 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include +#include +#include + + +namespace sofa::component::odesolver::integration +{ + +/** + * Velocity-based Backward Euler integration method + * 1-step Backward Differentiation Formula + */ +class SOFA_COMPONENT_ODESOLVER_INTEGRATION_API BDF1 : public sofa::core::behavior::BaseIntegrationMethod +{ +public: + SOFA_CLASS(BDF1, sofa::core::behavior::BaseIntegrationMethod); + + void initializeVectors(const core::ExecParams* params, core::ConstMultiVecCoordId x, core::ConstMultiVecDerivId v) override; + Factors getMatricesFactors(SReal dt) const override; + void computeRightHandSide(const core::ExecParams* params, + core::behavior::RHSInput input, + core::MultiVecDerivId force, + core::MultiVecDerivId rightHandSide, + SReal dt) override; + + void updateStates(const core::ExecParams* params, SReal dt, + core::MultiVecCoordId x, + core::MultiVecDerivId v, + core::MultiVecCoordId newX, + core::MultiVecDerivId newV, + core::MultiVecDerivId linearSystemSolution) override; + + Data d_rayleighStiffness; + Data d_rayleighMass; + +protected: + BDF1(); + + +private: + + //copy of the states of the previous time step + core::behavior::MultiVecCoord m_oldPosition; + core::behavior::MultiVecDeriv m_oldVelocity; + + std::unique_ptr m_vop; + +}; + +} diff --git a/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/config.h.in b/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/config.h.in new file mode 100644 index 00000000000..82b6e3d1495 --- /dev/null +++ b/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/config.h.in @@ -0,0 +1,38 @@ +/****************************************************************************** +* 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 + +#ifdef SOFA_BUILD_SOFA_COMPONENT_ODESOLVER_INTEGRATION +# define SOFA_TARGET @PROJECT_NAME@ +# define SOFA_COMPONENT_ODESOLVER_INTEGRATION_API SOFA_EXPORT_DYNAMIC_LIBRARY +#else +# define SOFA_COMPONENT_ODESOLVER_INTEGRATION_API SOFA_IMPORT_DYNAMIC_LIBRARY +#endif + + +namespace sofa::component::odesolver::integration +{ + constexpr const char* MODULE_NAME = "@PROJECT_NAME@"; + constexpr const char* MODULE_VERSION = "@PROJECT_VERSION@"; +} // namespace sofa::component::odesolver::integration diff --git a/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/init.cpp b/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/init.cpp new file mode 100644 index 00000000000..e3f43dd9713 --- /dev/null +++ b/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/init.cpp @@ -0,0 +1,70 @@ +/****************************************************************************** +* 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::odesolver::integration +{ + +extern void registerBDF1(core::ObjectFactory* factory); + +extern "C" { + SOFA_EXPORT_DYNAMIC_LIBRARY void initExternalModule(); + SOFA_EXPORT_DYNAMIC_LIBRARY const char* getModuleName(); + SOFA_EXPORT_DYNAMIC_LIBRARY const char* getModuleVersion(); + SOFA_EXPORT_DYNAMIC_LIBRARY void registerObjects(sofa::core::ObjectFactory* factory); +} + +void initExternalModule() +{ + init(); +} + +const char* getModuleName() +{ + return MODULE_NAME; +} + +const char* getModuleVersion() +{ + return MODULE_VERSION; +} + +void registerObjects(sofa::core::ObjectFactory* factory) +{ + registerBDF1(factory); +} + +void init() +{ + static bool first = true; + if (first) + { + // make sure that this plugin is registered into the PluginManager + sofa::helper::system::PluginManager::getInstance().registerPlugin(MODULE_NAME); + + first = false; + } +} + +} diff --git a/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/init.h b/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/init.h new file mode 100644 index 00000000000..07278621ea9 --- /dev/null +++ b/Sofa/Component/ODESolver/Integration/src/sofa/component/odesolver/integration/init.h @@ -0,0 +1,28 @@ +/****************************************************************************** +* 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::odesolver::integration +{ + SOFA_COMPONENT_ODESOLVER_INTEGRATION_API void init(); +} diff --git a/Sofa/Component/ODESolver/src/sofa/component/odesolver/init.cpp b/Sofa/Component/ODESolver/src/sofa/component/odesolver/init.cpp index 7f06baa6206..a17b9f8553b 100644 --- a/Sofa/Component/ODESolver/src/sofa/component/odesolver/init.cpp +++ b/Sofa/Component/ODESolver/src/sofa/component/odesolver/init.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -56,6 +57,7 @@ void registerObjects(sofa::core::ObjectFactory* factory) { factory->registerObjectsFromPlugin("Sofa.Component.ODESolver.Backward"); factory->registerObjectsFromPlugin("Sofa.Component.ODESolver.Forward"); + factory->registerObjectsFromPlugin("Sofa.Component.ODESolver.Integration"); } void init() @@ -66,6 +68,7 @@ void init() // force dependencies at compile-time sofa::component::odesolver::backward::init(); sofa::component::odesolver::forward::init(); + sofa::component::odesolver::integration::init(); // make sure that this plugin is registered into the PluginManager sofa::helper::system::PluginManager::getInstance().registerPlugin(MODULE_NAME); diff --git a/Sofa/framework/Core/CMakeLists.txt b/Sofa/framework/Core/CMakeLists.txt index 4910d364c28..127eb4a01e9 100644 --- a/Sofa/framework/Core/CMakeLists.txt +++ b/Sofa/framework/Core/CMakeLists.txt @@ -54,6 +54,7 @@ set(HEADER_FILES ${SRC_ROOT}/behavior/BaseConstraintSet.h ${SRC_ROOT}/behavior/BaseController.h ${SRC_ROOT}/behavior/BaseForceField.h + ${SRC_ROOT}/behavior/BaseIntegrationMethod.h ${SRC_ROOT}/behavior/BaseInteractionConstraint.h ${SRC_ROOT}/behavior/BaseInteractionForceField.h ${SRC_ROOT}/behavior/BaseInteractionProjectiveConstraintSet.h @@ -234,6 +235,7 @@ set(SOURCE_FILES ${SRC_ROOT}/behavior/BaseConstraintCorrection.cpp ${SRC_ROOT}/behavior/BaseConstraintSet.cpp ${SRC_ROOT}/behavior/BaseForceField.cpp + ${SRC_ROOT}/behavior/BaseIntegrationMethod.cpp ${SRC_ROOT}/behavior/BaseInteractionForceField.cpp ${SRC_ROOT}/behavior/BaseLinearSolver.cpp ${SRC_ROOT}/behavior/BaseMass.cpp diff --git a/Sofa/framework/Core/src/sofa/core/behavior/BaseIntegrationMethod.cpp b/Sofa/framework/Core/src/sofa/core/behavior/BaseIntegrationMethod.cpp new file mode 100644 index 00000000000..fe746555fba --- /dev/null +++ b/Sofa/framework/Core/src/sofa/core/behavior/BaseIntegrationMethod.cpp @@ -0,0 +1,22 @@ +/****************************************************************************** +* 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 diff --git a/Sofa/framework/Core/src/sofa/core/behavior/BaseIntegrationMethod.h b/Sofa/framework/Core/src/sofa/core/behavior/BaseIntegrationMethod.h new file mode 100644 index 00000000000..4a605e5ba86 --- /dev/null +++ b/Sofa/framework/Core/src/sofa/core/behavior/BaseIntegrationMethod.h @@ -0,0 +1,66 @@ +/****************************************************************************** +* 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::core::behavior +{ + +/** + * The vectors required to be able to compute the right-hand side + */ +struct SOFA_CORE_API RHSInput +{ + MultiVecDerivId intermediateVelocity; + MultiVecCoordId intermediatePosition; +}; + +class SOFA_CORE_API BaseIntegrationMethod : public sofa::core::objectmodel::BaseObject +{ +public: + SOFA_CLASS(BaseIntegrationMethod, sofa::core::objectmodel::BaseObject); + + using Factors = std::tuple; + + virtual void initializeVectors(const core::ExecParams* params, ConstMultiVecCoordId x, ConstMultiVecDerivId v) {} + + virtual Factors getMatricesFactors(SReal dt) const = 0; + + virtual void computeRightHandSide(const core::ExecParams* params, + RHSInput input, + MultiVecDerivId force, + MultiVecDerivId rightHandSide, + SReal dt) = 0; + + virtual void updateStates(const core::ExecParams* params, SReal dt, + MultiVecCoordId x, + MultiVecDerivId v, + MultiVecCoordId newX, + MultiVecDerivId newV, + MultiVecDerivId linearSystemSolution) = 0; +}; + +} diff --git a/Sofa/framework/Type/src/sofa/type/StrongType.h b/Sofa/framework/Type/src/sofa/type/StrongType.h index eff9c92711f..5fe5f63f58d 100644 --- a/Sofa/framework/Type/src/sofa/type/StrongType.h +++ b/Sofa/framework/Type/src/sofa/type/StrongType.h @@ -21,6 +21,8 @@ ******************************************************************************/ #pragma once +#include + namespace sofa::type { diff --git a/examples/Component/ODESolver/Backward/NewtonRaphsonSolver.scn b/examples/Component/ODESolver/Backward/NewtonRaphsonSolver.scn new file mode 100644 index 00000000000..a724ef6cc97 --- /dev/null +++ b/examples/Component/ODESolver/Backward/NewtonRaphsonSolver.scn @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/Component/ODESolver/Backward/NewtonRaphsonSolver_spring.scn b/examples/Component/ODESolver/Backward/NewtonRaphsonSolver_spring.scn new file mode 100644 index 00000000000..7188a212d0f --- /dev/null +++ b/examples/Component/ODESolver/Backward/NewtonRaphsonSolver_spring.scn @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/Component/ODESolver/Backward/NewtonRaphsonSolver_spring1d.scn b/examples/Component/ODESolver/Backward/NewtonRaphsonSolver_spring1d.scn new file mode 100644 index 00000000000..5fd18851682 --- /dev/null +++ b/examples/Component/ODESolver/Backward/NewtonRaphsonSolver_spring1d.scn @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +