WInverse iterations debugging

console_output.txt

[----------] 3 tests from MalidriveRoadCurveLineWInverseTest
[ RUN      ] MalidriveRoadCurveLineWInverseTest.AtTheCenterline
WInverse
  i  dp
  0  0
WInverse
  i  dp
  0  0
WInverse
  i  dp
  0  0
WInverse
  i  dp
  0  0
[       OK ] MalidriveRoadCurveLineWInverseTest.AtTheCenterline (0 ms)
[ RUN      ] MalidriveRoadCurveLineWInverseTest.WithALateralOffset
WInverse
  i  dp
  0  1.80411e-14
  1  -1.66533e-14
  2  1.38778e-14
  3  -1.66533e-14
  4  1.38778e-14
  5  -1.66533e-14
  6  1.38778e-14
  7  -1.66533e-14
  8  1.38778e-14
  9  -1.66533e-14
  10  1.38778e-14
  11  -1.66533e-14
  12  1.38778e-14
  13  -1.66533e-14
  14  1.38778e-14
  15  -1.66533e-14
WInverse
  i  dp
  0  1.80411e-14
  1  -1.66533e-14
  2  1.38778e-14
  3  -1.66533e-14
  4  1.38778e-14
  5  -1.66533e-14
  6  1.38778e-14
  7  -1.66533e-14
  8  1.38778e-14
  9  -1.66533e-14
  10  1.38778e-14
  11  -1.66533e-14
  12  1.38778e-14
  13  -1.66533e-14
  14  1.38778e-14
  15  -1.66533e-14
WInverse
  i  dp
  0  9.02056e-15
WInverse
  i  dp
  0  1.44329e-14
  1  -1.11022e-15
[       OK ] MalidriveRoadCurveLineWInverseTest.WithALateralOffset (1 ms)
[ RUN      ] MalidriveRoadCurveLineWInverseTest.WithAVerticalOffset
WInverse
  i  dp
  0  0
WInverse
  i  dp
  0  0
WInverse
  i  dp
  0  3.53553
  1  1.04083e-15
WInverse
  i  dp
  0  2.30437
  1  -0.070758
  2  0.00254216
  3  -9.09373e-05
  4  3.25349e-06
  5  -1.164e-07
  6  4.16446e-09
  7  -1.4899e-10
  8  5.32463e-12
  9  -1.84852e-13
  10  1.41553e-14
  11  -2.498e-15
[       OK ] MalidriveRoadCurveLineWInverseTest.WithAVerticalOffset (0 ms)
[----------] 3 tests from MalidriveRoadCurveLineWInverseTest (1 ms total)

winverse_debug.diff

diff --git a/src/malidrive/road_curve/malidrive_road_curve.cc b/src/malidrive/road_curve/malidrive_road_curve.cc
index ae71536..81ad98b 100644
--- a/src/malidrive/road_curve/malidrive_road_curve.cc
+++ b/src/malidrive/road_curve/malidrive_road_curve.cc
@@ -150,6 +150,8 @@ maliput::math::Vector3 MalidriveRoadCurve::DoWInverse(const maliput::math::Vecto
   // Correction in p computed iteratively.
   double dp{2 * linear_tolerance_};
 
+  std::cout << "WInverse" << std::endl
+            << "  i  dp" << std::endl;
   const int max_iterations{16};
   // Start of iterations.
   for (int i = 0; i < max_iterations && std::abs(dp) > linear_tolerance_; ++i)
@@ -165,6 +167,7 @@ maliput::math::Vector3 MalidriveRoadCurve::DoWInverse(const maliput::math::Vecto
     //   dp = (w_delta / w_dot.norm()).dot(s_hat);
     // which is equivalent to the following:
     dp = w_delta.dot(w_dot) / w_dot.dot(w_dot);
+    std::cout << "  " << i << "  " << dp << std::endl;
 
     p = maliput::math::saturate(p + dp, ground_curve_->p0(), ground_curve_->p1());
   }