toolhead: Calculate and store axes_r in move class

Calculate the ratio of axis distance to total move distance (axis_d /
move_d) and store in a new member variable axes_r.  This avoids
needing to recalculate the value in other code.

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

klippy/chelper/__init__.py

@@ -61,7 +61,7 @@ defs_trapq = """
     void trapq_append(struct trapq *tq, double print_time
         , double accel_t, double cruise_t, double decel_t
         , double start_pos_x, double start_pos_y, double start_pos_z
-        , double axes_d_x, double axes_d_y, double axes_d_z
+        , double axes_r_x, double axes_r_y, double axes_r_z
         , double start_v, double cruise_v, double accel);
     struct trapq *trapq_alloc(void);
     void trapq_free(struct trapq *tq);

klippy/chelper/trapq.c

@@ -25,19 +25,11 @@ void __visible
 trapq_append(struct trapq *tq, double print_time
              , double accel_t, double cruise_t, double decel_t
              , double start_pos_x, double start_pos_y, double start_pos_z
-             , double axes_d_x, double axes_d_y, double axes_d_z
+             , double axes_r_x, double axes_r_y, double axes_r_z
              , double start_v, double cruise_v, double accel)
 {
-    struct coord axes_r, start_pos;
+    struct coord start_pos = { x: start_pos_x, y: start_pos_y, z: start_pos_z };
-    double inv_move_d = 1. / sqrt(axes_d_x*axes_d_x + axes_d_y*axes_d_y
+    struct coord axes_r = { x: axes_r_x, y: axes_r_y, z: axes_r_z };
-                                  + axes_d_z*axes_d_z);
-    axes_r.x = axes_d_x * inv_move_d;
-    axes_r.y = axes_d_y * inv_move_d;
-    axes_r.z = axes_d_z * inv_move_d;
-    start_pos.x = start_pos_x;
-    start_pos.y = start_pos_y;
-    start_pos.z = start_pos_z;
-
     if (accel_t) {
         struct move *m = move_alloc();
         m->print_time = print_time;

klippy/chelper/trapq.h

@@ -23,7 +23,7 @@ struct move *move_alloc(void);
 void trapq_append(struct trapq *tq, double print_time
                   , double accel_t, double cruise_t, double decel_t
                   , double start_pos_x, double start_pos_y, double start_pos_z
-                  , double axes_d_x, double axes_d_y, double axes_d_z
+                  , double axes_r_x, double axes_r_y, double axes_r_z
                   , double start_v, double cruise_v, double accel);
 double move_get_distance(struct move *m, double move_time);
 struct coord move_get_coord(struct move *m, double move_time);

klippy/extras/force_move.py

@@ -11,16 +11,19 @@ STALL_TIME = 0.100
 
 # Calculate a move's accel_t, cruise_t, and cruise_v
 def calc_move_time(dist, speed, accel):
-    dist = abs(dist)
+    axis_r = 1.
+    if dist < 0.:
+        axis_r = -1.
+        dist = -dist
     if not accel or not dist:
-        return 0., dist / speed, speed
+        return axis_r, 0., dist / speed, speed
     max_cruise_v2 = dist * accel
     if max_cruise_v2 < speed**2:
         speed = math.sqrt(max_cruise_v2)
     accel_t = speed / accel
     accel_decel_d = accel_t * speed
     cruise_t = (dist - accel_decel_d) / speed
-    return accel_t, cruise_t, speed
+    return axis_r, accel_t, cruise_t, speed
 
 class ForceMove:
     def __init__(self, config):
@@ -67,9 +70,9 @@ class ForceMove:
         print_time = toolhead.get_last_move_time()
         prev_sk = stepper.set_stepper_kinematics(self.stepper_kinematics)
         stepper.set_position((0., 0., 0.))
-        accel_t, cruise_t, cruise_v = calc_move_time(dist, speed, accel)
+        axis_r, accel_t, cruise_t, cruise_v = calc_move_time(dist, speed, accel)
         self.trapq_append(self.trapq, print_time, accel_t, cruise_t, accel_t,
-                          0., 0., 0., dist, 0., 0., 0., cruise_v, accel)
+                          0., 0., 0., axis_r, 0., 0., 0., cruise_v, accel)
         print_time += accel_t + cruise_t + accel_t
         stepper.generate_steps(print_time)
         self.trapq_free_moves(self.trapq, print_time)

klippy/extras/manual_stepper.py

@@ -54,11 +54,11 @@ class ManualStepper:
         self.sync_print_time()
         cp = self.stepper.get_commanded_position()
         dist = movepos - cp
-        accel_t, cruise_t, cruise_v = force_move.calc_move_time(
+        axis_r, accel_t, cruise_t, cruise_v = force_move.calc_move_time(
             dist, speed, accel)
         self.trapq_append(self.trapq, self.next_cmd_time,
                           accel_t, cruise_t, accel_t,
-                          cp, 0., 0., dist, 0., 0.,
+                          cp, 0., 0., axis_r, 0., 0.,
                           0., cruise_v, accel)
         self.next_cmd_time += accel_t + cruise_t + accel_t
         self.stepper.generate_steps(self.next_cmd_time)

klippy/kinematics/extruder.py

@@ -90,7 +90,7 @@ class PrinterExtruder:
     def motor_off(self, print_time):
         self.stepper.motor_enable(print_time, 0)
     def check_move(self, move):
-        move.extrude_r = move.axes_d[3] / move.move_d
+        move.extrude_r = move.axes_r[3]
         move.extrude_max_corner_v = 0.
         if not self.heater.can_extrude:
             raise homing.EndstopError(
@@ -111,7 +111,7 @@ class PrinterExtruder:
                 # Permit extrusion if amount extruded is tiny
                 move.extrude_r = self.max_extrude_ratio
                 return
-            area = move.axes_d[3] * self.filament_area / move.move_d
+            area = move.axes_r[3] * self.filament_area
             logging.debug("Overextrude: %s vs %s (area=%.3f dist=%.3f)",
                           move.extrude_r, self.max_extrude_ratio,
                           area, move.move_d)
@@ -172,7 +172,7 @@ class PrinterExtruder:
         return flush_count
     def move(self, print_time, move):
         axis_d = move.axes_d[3]
-        axis_r = axis_d / move.move_d
+        axis_r = move.axes_r[3]
         accel = move.accel * axis_r
         start_v = move.start_v * axis_r
         cruise_v = move.cruise_v * axis_r

klippy/toolhead.py

@@ -27,9 +27,15 @@ class Move:
                             end_pos[3])
             axes_d[0] = axes_d[1] = axes_d[2] = 0.
             self.move_d = move_d = abs(axes_d[3])
+            inv_move_d = 0.
+            if move_d:
+                inv_move_d = 1. / move_d
             self.accel = 99999999.9
             velocity = speed
             self.is_kinematic_move = False
+        else:
+            inv_move_d = 1. / move_d
+        self.axes_r = [d * inv_move_d for d in axes_d]
         self.min_move_t = move_d / velocity
         # Junction speeds are tracked in velocity squared.  The
         # delta_v2 is the maximum amount of this squared-velocity that
@@ -53,12 +59,11 @@ class Move:
         # Allow extruder to calculate its maximum junction
         extruder_v2 = self.toolhead.extruder.calc_junction(prev_move, self)
         # Find max velocity using "approximated centripetal velocity"
-        axes_d = self.axes_d
+        axes_r = self.axes_r
-        prev_axes_d = prev_move.axes_d
+        prev_axes_r = prev_move.axes_r
-        junction_cos_theta = -((axes_d[0] * prev_axes_d[0]
+        junction_cos_theta = -(axes_r[0] * prev_axes_r[0]
-                                + axes_d[1] * prev_axes_d[1]
+                               + axes_r[1] * prev_axes_r[1]
-                                + axes_d[2] * prev_axes_d[2])
+                               + axes_r[2] * prev_axes_r[2])
-                               / (self.move_d * prev_move.move_d))
         if junction_cos_theta > 0.999999:
             return
         junction_cos_theta = max(junction_cos_theta, -0.999999)
@@ -302,7 +307,7 @@ class ToolHead:
                     self.trapq, next_move_time,
                     move.accel_t, move.cruise_t, move.decel_t,
                     move.start_pos[0], move.start_pos[1], move.start_pos[2],
-                    move.axes_d[0], move.axes_d[1], move.axes_d[2],
+                    move.axes_r[0], move.axes_r[1], move.axes_r[2],
                     move.start_v, move.cruise_v, move.accel)
             if move.axes_d[3]:
                 self.extruder.move(next_move_time, move)