delta: Rework delta math to avoid using inv_movexy_r

Taking the inverse of the XY move distance can lead to extremely large
values when the XY distance is very small.  This can lead to
saturation of the double precision variables and incorrect results.

Rework the delta kinematic math to avoid using this inverse.  Pass the
closestxy_d value directly to the C functions so that the C code can
calculate its intermediate constants.

After this change the move_z special case is no longer necessary as
the regular delta functions now work with movexy_r=0 and movez_r=1.

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

4 files changed, 83 insertions, 126 deletions

diff --git a/klippy/chelper.py b/klippy/chelper.py
index 58049582..4384fe28 100644
--- a/klippy/chelper.py
+++ b/klippy/chelper.py
@@ -24,14 +24,14 @@ defs_stepcompress = """
     int32_t stepcompress_push_sqrt(struct stepcompress *sc
         , double steps, double step_offset
         , double clock_offset, double sqrt_offset, double factor);
-    int32_t stepcompress_push_delta_const(
-        struct stepcompress *sc, double clock_offset, double dist
-        , double step_dist, double start_pos, double closest_height2
-        , double height, double movez_r, double inv_velocity);
-    int32_t stepcompress_push_delta_accel(
-        struct stepcompress *sc, double clock_offset, double dist
-        , double step_dist, double start_pos, double closest_height2
-        , double height, double movez_r, double accel_multiplier);
+    int32_t stepcompress_push_delta_const(struct stepcompress *sc
+        , double clock_offset, double dist, double start_pos
+        , double inv_velocity, double step_dist, double height
+        , double closestxy_d, double closest_height2, double movez_r);
+    int32_t stepcompress_push_delta_accel(struct stepcompress *sc
+        , double clock_offset, double dist, double start_pos
+        , double accel_multiplier, double step_dist, double height
+        , double closestxy_d, double closest_height2, double movez_r);
     void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
     void stepcompress_queue_msg(struct stepcompress *sc
         , uint32_t *data, int len);
diff --git a/klippy/delta.py b/klippy/delta.py
index 19984496..7d7b063f 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -125,72 +125,25 @@ class DeltaKinematics:
                 raise homing.EndstopMoveError(end_pos)
         if move.axes_d[2]:
             move.limit_speed(self.max_z_velocity, 9999999.9)
-    def move_z(self, move_time, move):
-        if not move.axes_d[2]:
-            return
-        if self.need_motor_enable:
-            self.check_motor_enable(move_time)
-        inv_accel = 1. / move.accel
-        inv_cruise_v = 1. / move.cruise_v
-        for i in StepList:
-            towerx_d = self.towers[i][0] - move.start_pos[0]
-            towery_d = self.towers[i][1] - move.start_pos[1]
-            tower_d2 = towerx_d**2 + towery_d**2
-            height = math.sqrt(self.arm_length2 - tower_d2) + move.start_pos[2]
-
-            mcu_stepper = self.steppers[i].mcu_stepper
-            mcu_time = mcu_stepper.print_to_mcu_time(move_time)
-            step_pos = mcu_stepper.commanded_position
-            inv_step_dist = self.steppers[i].inv_step_dist
-            step_offset = step_pos - height * inv_step_dist
-            step_dist = self.steppers[i].step_dist
-            steps = move.axes_d[2] * inv_step_dist
-
-            # Acceleration steps
-            accel_multiplier = 2.0 * step_dist * inv_accel
-            if move.accel_r:
-                #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
-                accel_time_offset = move.start_v * inv_accel
-                accel_sqrt_offset = accel_time_offset**2
-                accel_steps = move.accel_r * steps
-                count = mcu_stepper.step_sqrt(
-                    mcu_time - accel_time_offset, accel_steps, step_offset
-                    , accel_sqrt_offset, accel_multiplier)
-                step_offset += count - accel_steps
-                mcu_time += move.accel_t
-            # Cruising steps
-            if move.cruise_r:
-                #t = pos/cruise_v
-                cruise_multiplier = step_dist * inv_cruise_v
-                cruise_steps = move.cruise_r * steps
-                count = mcu_stepper.step_factor(
-                    mcu_time, cruise_steps, step_offset, cruise_multiplier)
-                step_offset += count - cruise_steps
-                mcu_time += move.cruise_t
-            # Deceleration steps
-            if move.decel_r:
-                #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
-                decel_time_offset = move.cruise_v * inv_accel
-                decel_sqrt_offset = decel_time_offset**2
-                decel_steps = move.decel_r * steps
-                count = mcu_stepper.step_sqrt(
-                    mcu_time + decel_time_offset, decel_steps, step_offset
-                    , decel_sqrt_offset, -accel_multiplier)
     def move(self, move_time, move):
         axes_d = move.axes_d
+        move_d = movexy_d = move.move_d
+        movexy_r = 1.
+        movez_r = 0.
+        inv_movexy_d = 1. / movexy_d
         if not axes_d[0] and not axes_d[1]:
-            self.move_z(move_time, move)
-            return
+            if not axes_d[2]:
+                return
+            movez_r = axes_d[2] * inv_movexy_d
+            movexy_d = movexy_r = inv_movexy_d = 0.
+        elif axes_d[2]:
+            movexy_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2)
+            movexy_r = movexy_d * inv_movexy_d
+            movez_r = axes_d[2] * inv_movexy_d
+            inv_movexy_d = 1. / movexy_d
+
         if self.need_motor_enable:
             self.check_motor_enable(move_time)
-        move_d = move.move_d
-        movez_r = 0.
-        inv_movexy_d = 1. / move_d
-        inv_movexy_r = 1.
-        if axes_d[2]:
-            movez_r = axes_d[2] * inv_movexy_d
-            inv_movexy_d = 1. / math.sqrt(axes_d[0]**2 + axes_d[1]**2)
-            inv_movexy_r = move_d * inv_movexy_d
 
         origx, origy, origz = move.start_pos[:3]
 
@@ -214,80 +167,76 @@ class DeltaKinematics:
             closestxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d
             tangentxy_d2 = towerx_d**2 + towery_d**2 - closestxy_d**2
             closest_height2 = self.arm_length2 - tangentxy_d2
-            closest_height = math.sqrt(closest_height2)
-            closest_d = closestxy_d * inv_movexy_r
-            closestz = origz + closest_d*movez_r
 
             # Calculate accel/cruise/decel portions of move
-            reverse_d = closest_d + closest_height*movez_r*inv_movexy_r
+            reversexy_d = closestxy_d + math.sqrt(closest_height2)*movez_r
             accel_up_d = cruise_up_d = decel_up_d = 0.
             accel_down_d = cruise_down_d = decel_down_d = 0.
-            if reverse_d <= 0.:
+            if reversexy_d <= 0.:
                 accel_down_d = accel_d
                 cruise_down_d = cruise_end_d
                 decel_down_d = move_d
-            elif reverse_d >= move_d:
+            elif reversexy_d >= movexy_d:
                 accel_up_d = accel_d
                 cruise_up_d = cruise_end_d
                 decel_up_d = move_d
-            elif reverse_d < accel_d:
-                accel_up_d = reverse_d
+            elif reversexy_d < accel_d * movexy_r:
+                accel_up_d = reversexy_d * move_d * inv_movexy_d
                 accel_down_d = accel_d
                 cruise_down_d = cruise_end_d
                 decel_down_d = move_d
-            elif reverse_d < cruise_end_d:
+            elif reversexy_d < cruise_end_d * movexy_r:
                 accel_up_d = accel_d
-                cruise_up_d = reverse_d
+                cruise_up_d = reversexy_d * move_d * inv_movexy_d
                 cruise_down_d = cruise_end_d
                 decel_down_d = move_d
             else:
                 accel_up_d = accel_d
                 cruise_up_d = cruise_end_d
-                decel_up_d = reverse_d
+                decel_up_d = reversexy_d * move_d * inv_movexy_d
                 decel_down_d = move_d
 
             # Generate steps
             mcu_stepper = self.steppers[i].mcu_stepper
             mcu_time = mcu_stepper.print_to_mcu_time(move_time)
             step_pos = mcu_stepper.commanded_position
-            inv_step_dist = self.steppers[i].inv_step_dist
             step_dist = self.steppers[i].step_dist
-            height = step_pos*step_dist - closestz
+            height = step_pos*step_dist - origz
             if accel_up_d > 0.:
                 count = mcu_stepper.step_delta_accel(
-                    mcu_time - accel_time_offset, closest_d - accel_up_d,
-                    step_dist, closest_d + accel_offset,
-                    closest_height2, height, movez_r, accel_multiplier)
+                    mcu_time - accel_time_offset, accel_up_d,
+                    accel_offset, accel_multiplier, step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
                 height += count * step_dist
             if cruise_up_d > 0.:
                 count = mcu_stepper.step_delta_const(
-                    mcu_time + accel_t, closest_d - cruise_up_d,
-                    step_dist, closest_d - accel_d,
-                    closest_height2, height, movez_r, inv_cruise_v)
+                    mcu_time + accel_t, cruise_up_d,
+                    -accel_d, inv_cruise_v, step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
                 height += count * step_dist
             if decel_up_d > 0.:
                 count = mcu_stepper.step_delta_accel(
-                    mcu_time + decel_time_offset, closest_d - decel_up_d,
-                    step_dist, closest_d - decel_offset,
-                    closest_height2, height, movez_r, -accel_multiplier)
+                    mcu_time + decel_time_offset, decel_up_d,
+                    -decel_offset, -accel_multiplier, step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
                 height += count * step_dist
             if accel_down_d > 0.:
                 count = mcu_stepper.step_delta_accel(
-                    mcu_time - accel_time_offset, closest_d - accel_down_d,
-                    -step_dist, closest_d + accel_offset,
-                    closest_height2, height, movez_r, accel_multiplier)
+                    mcu_time - accel_time_offset, accel_down_d,
+                    accel_offset, accel_multiplier, -step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
                 height += count * step_dist
             if cruise_down_d > 0.:
                 count = mcu_stepper.step_delta_const(
-                    mcu_time + accel_t, closest_d - cruise_down_d,
-                    -step_dist, closest_d - accel_d,
-                    closest_height2, height, movez_r, inv_cruise_v)
+                    mcu_time + accel_t, cruise_down_d,
+                    -accel_d, inv_cruise_v, -step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
                 height += count * step_dist
             if decel_down_d > 0.:
                 count = mcu_stepper.step_delta_accel(
-                    mcu_time + decel_time_offset, closest_d - decel_down_d,
-                    -step_dist, closest_d - decel_offset,
-                    closest_height2, height, movez_r, -accel_multiplier)
+                    mcu_time + decel_time_offset, decel_down_d,
+                    -decel_offset, -accel_multiplier, -step_dist,
+                    height, closestxy_d, closest_height2, movez_r)
 
 
 ######################################################################
diff --git a/klippy/mcu.py b/klippy/mcu.py
index c1abb959..4c2bbc68 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -86,21 +86,25 @@ class MCU_stepper:
             self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq)
         self.commanded_position += count
         return count
-    def step_delta_const(self, mcu_time, dist, step_dist, start_pos
-                         , closest_height2, height, movez_r, inv_velocity):
+    def step_delta_const(self, mcu_time, dist, start_pos
+                         , inv_velocity, step_dist
+                         , height, closestxy_d, closest_height2, movez_r):
         clock = mcu_time * self._mcu_freq
         count = self.ffi_lib.stepcompress_push_delta_const(
-            self._stepqueue, clock, dist, step_dist, start_pos
-            , closest_height2, height, movez_r, inv_velocity * self._mcu_freq)
+            self._stepqueue, clock, dist, start_pos
+            , inv_velocity * self._mcu_freq, step_dist
+            , height, closestxy_d, closest_height2, movez_r)
         self.commanded_position += count
         return count
-    def step_delta_accel(self, mcu_time, dist, step_dist, start_pos
-                         , closest_height2, height, movez_r, accel_multiplier):
+    def step_delta_accel(self, mcu_time, dist, start_pos
+                         , accel_multiplier, step_dist
+                         , height, closestxy_d, closest_height2, movez_r):
         clock = mcu_time * self._mcu_freq
         mcu_freq2 = self._mcu_freq**2
         count = self.ffi_lib.stepcompress_push_delta_accel(
-            self._stepqueue, clock, dist, step_dist, start_pos
-            , closest_height2, height, movez_r, accel_multiplier * mcu_freq2)
+            self._stepqueue, clock, dist, start_pos
+            , accel_multiplier * mcu_freq2, step_dist
+            , height, closestxy_d, closest_height2, movez_r)
         self.commanded_position += count
         return count
     def get_errors(self):
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
index 4f41cfee..0af0672e 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -442,14 +442,15 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
 // Schedule 'count' number of steps using the delta kinematic const speed
 int32_t
 stepcompress_push_delta_const(
-    struct stepcompress *sc, double clock_offset, double dist, double step_dist
-    , double start_pos, double closest_height2, double height, double movez_r
-    , double inv_velocity)
+    struct stepcompress *sc, double clock_offset, double dist, double start_pos
+    , double inv_velocity, double step_dist
+    , double height, double closestxy_d, double closest_height2, double movez_r)
 {
     // Calculate number of steps to take
-    double zdist = dist * movez_r;
-    int count = (safe_sqrt(closest_height2 - dist*dist + zdist*zdist)
-                 - height - zdist) / step_dist + .5;
+    double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
+    double reldist = closestxy_d - movexy_r*dist;
+    double end_height = safe_sqrt(closest_height2 - reldist*reldist);
+    int count = (end_height - height + movez_r*dist) / step_dist + .5;
     if (count <= 0 || count > 1000000) {
         if (count)
             errorf("push_delta_const invalid count %d %d %f %f %f %f %f %f %f %f"
@@ -462,11 +463,12 @@ stepcompress_push_delta_const(
     // Calculate each step time
     uint64_t *qn = sc->queue_next, *end = &qn[count];
     clock_offset += 0.5;
+    start_pos += movexy_r*closestxy_d;
     height += .5 * step_dist;
     while (qn < end) {
-        double zh = height*movez_r;
-        double v = safe_sqrt(closest_height2 - height*height + zh*zh);
-        double pos = start_pos + zh + (step_dist > 0. ? -v : v);
+        double relheight = movexy_r*height - movez_r*closestxy_d;
+        double v = safe_sqrt(closest_height2 - relheight*relheight);
+        double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
         *qn++ = clock_offset + pos * inv_velocity;
         height += step_dist;
     }
@@ -477,14 +479,15 @@ stepcompress_push_delta_const(
 // Schedule 'count' number of steps using delta kinematic acceleration
 int32_t
 stepcompress_push_delta_accel(
-    struct stepcompress *sc, double clock_offset, double dist, double step_dist
-    , double start_pos, double closest_height2, double height, double movez_r
-    , double accel_multiplier)
+    struct stepcompress *sc, double clock_offset, double dist, double start_pos
+    , double accel_multiplier, double step_dist
+    , double height, double closestxy_d, double closest_height2, double movez_r)
 {
     // Calculate number of steps to take
-    double zdist = dist * movez_r;
-    int count = (safe_sqrt(closest_height2 - dist*dist + zdist*zdist)
-                 - height - zdist) / step_dist + .5;
+    double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
+    double reldist = closestxy_d - movexy_r*dist;
+    double end_height = safe_sqrt(closest_height2 - reldist*reldist);
+    int count = (end_height - height + movez_r*dist) / step_dist + .5;
     if (count <= 0 || count > 1000000) {
         if (count)
             errorf("push_delta_accel invalid count %d %d %f %f %f %f %f %f %f %f"
@@ -497,11 +500,12 @@ stepcompress_push_delta_accel(
     // Calculate each step time
     uint64_t *qn = sc->queue_next, *end = &qn[count];
     clock_offset += 0.5;
+    start_pos += movexy_r*closestxy_d;
     height += .5 * step_dist;
     while (qn < end) {
-        double zh = height*movez_r;
-        double v = safe_sqrt(closest_height2 - height*height + zh*zh);
-        double pos = start_pos + zh + (step_dist > 0. ? -v : v);
+        double relheight = movexy_r*height - movez_r*closestxy_d;
+        double v = safe_sqrt(closest_height2 - relheight*relheight);
+        double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
         v = safe_sqrt(pos * accel_multiplier);
         *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
         height += step_dist;