From 8c6731effc2c7b7ca2143d769105c66020ff00a3 Mon Sep 17 00:00:00 2001
From: Alex Bilger <alexbilger0@gmail.com>
Date: Fri, 20 Dec 2024 10:53:52 +0100
Subject: [PATCH] implement relative estimate difference criterion

---
 .../backward/NewtonRaphsonSolver.cpp          | 39 +++++++++++++++++--
 .../odesolver/backward/NewtonRaphsonSolver.h  |  1 +
 2 files changed, 37 insertions(+), 3 deletions(-)

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
index 0f868e0f263..73c01500844 100644
--- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewtonRaphsonSolver.cpp
+++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewtonRaphsonSolver.cpp
@@ -55,6 +55,11 @@ NewtonRaphsonSolver::NewtonRaphsonSolver()
         "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 "
@@ -200,10 +205,12 @@ void NewtonRaphsonSolver::solve(
 
         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();
@@ -343,7 +350,7 @@ void NewtonRaphsonSolver::solve(
                 break;
             }
 
-            if (absoluteEstimateDifferenceThreshold > 0)
+            if (absoluteEstimateDifferenceThreshold > 0 || squaredRelativeEstimateDifferenceThreshold > 0)
             {
                 core::behavior::MultiVecDeriv tmp(&vop);
 
@@ -355,14 +362,40 @@ void NewtonRaphsonSolver::solve(
 
                 if (printLog)
                 {
-                    iterationResults << "\n* Successive estimate difference = " << squaredAbsoluteDifference << '\n';
+                    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;
+                            break;
+                        }
+                    }
                 }
 
                 if (squaredAbsoluteDifference < squaredAbsoluteEstimateDifferenceThreshold)
                 {
                     msg_info() << iterationResults.str();
                     msg_info() << "[CONVERGED] absolute successive estimate difference (" <<
-                            squaredAbsoluteDifference << ") is smaller than the threshold ("
+                            std::sqrt(squaredAbsoluteDifference) << ") is smaller than the threshold ("
                             << squaredAbsoluteEstimateDifferenceThreshold << ") after "
                             << (newtonIterationCount+1) << " Newton iterations.";
                     hasConverged = true;
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
index 97463f1d414..495ae7a741e 100644
--- a/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewtonRaphsonSolver.h
+++ b/Sofa/Component/ODESolver/Backward/src/sofa/component/odesolver/backward/NewtonRaphsonSolver.h
@@ -47,6 +47,7 @@ class SOFA_COMPONENT_ODESOLVER_BACKWARD_API NewtonRaphsonSolver
     Data<SReal> d_relativeSuccessiveStoppingThreshold;
     Data<SReal> d_relativeInitialStoppingThreshold;
     Data<SReal> d_absoluteResidualStoppingThreshold;
+    Data<SReal> d_relativeEstimateDifferenceThreshold;
     Data<SReal> d_absoluteEstimateDifferenceThreshold;
     Data<unsigned int> d_maxNbIterationsLineSearch;
     Data<SReal> d_lineSearchCoefficient;