clean root finding

README.md

@@ -152,7 +152,7 @@ Moreover, a range of additional parameter about the duration of the trajectory a
 
 ## Tests and Numerical Stability
 
-The current test suite validates over 1.008.000 (random) trajectories. The numerical exactness is tested for the position, velocity, acceleration, and time target to be within `1e-8`, for the velocity and acceleration limit to be withing `1e-9`, and for the jerk limit to be within a numerical error of `1e-12`. Ruckig presumes that the input values are within 5 orders of magnitude. Note that Ruckig will also output values outside of this range, there is however no guarantee for correctness.
+The current test suite validates over 1.008.000 (random) trajectories. The numerical exactness is tested for the position, velocity, acceleration, and time target to be within `1e-8`, for the velocity and acceleration limit to be withing `1e-9`, and for the jerk limit to be within a numerical error of `1e-12`. Ruckig presumes that the input values are within 5 orders of magnitude and that `jMax > 5e-2`. Note that Ruckig will also output values outside of this range, there is however no guarantee for correctness.
 
 
 ## Development

include/ruckig/roots.hpp

@@ -274,13 +274,21 @@ inline double polyEval(std::array<double, N> p, double x) {
 template<size_t N>
 inline std::array<double, N-1> polyDeri(const std::array<double, N>& coeffs) {
     std::array<double, N-1> deriv;
-    int horder = N - 1;
-    for (int i = 0; i < horder; i++) {
-        deriv[i] = (horder - i) * coeffs[i];
+    for (int i = 0; i < N - 1; i++) {
+        deriv[i] = (N - 1 - i) * coeffs[i];
     }
     return deriv;
 }
 
+// template<size_t N>
+// inline std::array<double, N-1> polyMonicDeri(const std::array<double, N>& monic_coeffs) {
+//     std::array<double, N-1> deriv;
+//     for (int i = 0; i < N - 1; i++) {
+//         deriv[i] = (N - 1 - i) * monic_coeffs[i] / (N - 1);
+//     }
+//     return deriv;
+// }
+
 // Safe Newton Method
 // Requirements: f(l)*f(h) <= 0
 template <typename F, typename DF>
@@ -344,12 +352,15 @@ inline double safeNewton(const F& func, const DF& dfunc, const double &l, const
 
 // Calculate a single zero of polynom p(x) inside [lbound, ubound]
 // Requirements: p(lbound)*p(ubound) < 0, lbound < ubound
+
+constexpr double tolerance {1e-14};
+
 template<size_t N, size_t maxDblIts = 128>
-inline double shrinkInterval(const std::array<double, N>& p, double lbound, double ubound, double tol = 1e-14) {
+inline double shrinkInterval(const std::array<double, N>& p, double lbound, double ubound) {
     auto deriv = polyDeri(p);
     auto func = [&p](double x) { return polyEval(p, x); };
     auto dfunc = [&deriv](double x) { return polyEval(deriv, x); };
-    return safeNewton(func, dfunc, lbound, ubound, tol, maxDblIts);
+    return safeNewton(func, dfunc, lbound, ubound, tolerance, maxDblIts);
 }
 
 } // namespace Roots

include/ruckig/ruckig.hpp

@@ -84,7 +84,6 @@ class Ruckig {
 
             t_sync = possible_t_sync;
             limiting_dof = std::ceil((i + 1.0) / 3) - 1;
-            // std::cout << "sync: " << limiting_dof << " " << i % 3 << " " << t_sync << std::endl;
             switch (i % 3) {
                 case 0: {
                     profiles[limiting_dof] = blocks[limiting_dof].p_min;

include/ruckig/steps.hpp

@@ -81,6 +81,16 @@ class Step1 {
 
     template<size_t N, size_t left, size_t right, bool same_direction = true>
     inline void add_block(double t_brake) {
+        if constexpr (N == 0) {
+            if (block.a) {
+                return;
+            }
+        } else {
+            if (block.b) {
+                return;
+            }
+        }
+
         double left_duration = valid_profiles[left].t_sum[6] + t_brake;
         double right_duraction = valid_profiles[right].t_sum[6] + t_brake;
         if constexpr (same_direction) {