trapq: Add code to calculate definitive integral

Support calculating the definitive integral of a cartesian axis
position over a time range of the movement queue.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 44 insertions, 0 deletions

diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c
index 603ce9e2..5d5073c1 100644
--- a/klippy/chelper/trapq.c
+++ b/klippy/chelper/trapq.c
@@ -86,6 +86,50 @@ move_get_coord(struct move *m, double move_time)
         .z = m->start_pos.z + m->axes_r.z * move_dist };
 }
 
+// Helper code for integrating acceleration
+static double
+integrate_accel(struct move *m, double start, double end)
+{
+    double half_v = .5 * m->start_v, sixth_a = (1. / 3.) * m->half_accel;
+    double si = start * start * (half_v + sixth_a * start);
+    double ei = end * end * (half_v + sixth_a * end);
+    return ei - si;
+}
+
+// Calculate the definitive integral on part of a move
+static double
+move_integrate(struct move *m, int axis, double start, double end)
+{
+    if (start < 0.)
+        start = 0.;
+    if (end > m->move_t)
+        end = m->move_t;
+    double base = m->start_pos.axis[axis - 'x'] * (end - start);
+    double integral = integrate_accel(m, start, end);
+    return base + integral * m->axes_r.axis[axis - 'x'];
+}
+
+// Calculate the definitive integral for a cartesian axis
+double
+trapq_integrate(struct move *m, int axis, double start, double end)
+{
+    double res = move_integrate(m, axis, start, end);
+    // Integrate over previous moves
+    struct move *prev = m;
+    while (unlikely(start < 0.)) {
+        prev = list_prev_entry(prev, node);
+        start += prev->move_t;
+        res += move_integrate(prev, axis, start, prev->move_t);
+    }
+    // Integrate over future moves
+    while (unlikely(end > m->move_t)) {
+        end -= m->move_t;
+        m = list_next_entry(m, node);
+        res += move_integrate(m, axis, 0., end);
+    }
+    return res;
+}
+
 #define NEVER_TIME 9999999999999999.9
 
 // Allocate a new 'trapq' object