gcode_arcs: Minor cleanup of planArc()

Fix linear_travel optimization.  Use "2*pi" instead of "radians(360)".
Always emit at least one segment and always end at the final target
coordinates.

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

1 files changed, 27 insertions, 42 deletions

diff --git a/klippy/extras/gcode_arcs.py b/klippy/extras/gcode_arcs.py
index 64016133..3b0876ab 100644
--- a/klippy/extras/gcode_arcs.py
+++ b/klippy/extras/gcode_arcs.py
@@ -44,10 +44,6 @@ class ArcSupport:
         # Build list of linear coordinates to move to
         coords = self.planArc(currentPos, [asX, asY, asZ], [asI, asJ],
                               clockwise)
-        if not coords:
-            self.gcode.respond_info("G2/G3 could not translate '%s'"
-                                    % (params['#original'],))
-            return
         e_per_move = e_base = 0.
         if asE is not None:
             if gcodestatus['absolute_extrude']:
@@ -71,69 +67,58 @@ class ArcSupport:
     # Arcs smaller then this value, will be a Line only
     def planArc(self, currentPos, targetPos, offset, clockwise):
         # todo: sometimes produces full circles
-        coords = []
-        MM_PER_ARC_SEGMENT = self.mm_per_arc_segment
-
         X_AXIS = 0
         Y_AXIS = 1
         Z_AXIS = 2
 
         # Radius vector from center to current location
-        r_P = offset[0]*-1
-        r_Q = offset[1]*-1
+        r_P = -offset[0]
+        r_Q = -offset[1]
 
-        radius = math.hypot(r_P, r_Q)
+        # Determine angular travel
         center_P = currentPos[X_AXIS] - r_P
         center_Q = currentPos[Y_AXIS] - r_Q
         rt_X = targetPos[X_AXIS] - center_P
         rt_Y = targetPos[Y_AXIS] - center_Q
-        linear_travel = targetPos[Z_AXIS] - currentPos[Z_AXIS]
-
         angular_travel = math.atan2(r_P * rt_Y - r_Q * rt_X,
-            r_P * rt_X + r_Q * rt_Y)
-        if (angular_travel < 0): angular_travel+= math.radians(360)
-        if (clockwise): angular_travel-= math.radians(360)
+                                    r_P * rt_X + r_Q * rt_Y)
+        if angular_travel < 0.:
+            angular_travel += 2. * math.pi
+        if clockwise:
+            angular_travel -= 2. * math.pi
 
-        # Make a circle if the angular rotation is 0
-        # and the target is current position
-        if (angular_travel == 0
+        if (angular_travel == 0.
             and currentPos[X_AXIS] == targetPos[X_AXIS]
             and currentPos[Y_AXIS] == targetPos[Y_AXIS]):
-            angular_travel = math.radians(360)
+            # Make a circle if the angular rotation is 0 and the
+            # target is current position
+            angular_travel = 2. * math.pi
 
+        # Determine number of segments
+        linear_travel = targetPos[Z_AXIS] - currentPos[Z_AXIS]
+        radius = math.hypot(r_P, r_Q)
         flat_mm = radius * angular_travel
-        mm_of_travel = linear_travel
-        if(mm_of_travel == linear_travel):
+        if linear_travel:
             mm_of_travel = math.hypot(flat_mm, linear_travel)
         else:
-            mm_of_travel = math.abs(flat_mm)
+            mm_of_travel = math.fabs(flat_mm)
+        segments = max(1., math.floor(mm_of_travel / self.mm_per_arc_segment))
 
-        if (mm_of_travel < 0.001):
-            return coords
-
-        segments = int(math.floor(mm_of_travel / (MM_PER_ARC_SEGMENT)))
-        if(segments<1):
-            segments=1
-
-        raw = [0.,0.,0.,0.]
-        theta_per_segment = float(angular_travel / segments)
-        linear_per_segment = float(linear_travel / segments)
-
-        # Initialize the linear axis
-        raw[Z_AXIS] = currentPos[Z_AXIS];
-
-        for i in range(1,segments+1):
+        # Generate coordinates
+        theta_per_segment = angular_travel / segments
+        linear_per_segment = linear_travel / segments
+        coords = []
+        for i in range(1, int(segments)):
+            dist_Z = i * linear_per_segment
             cos_Ti = math.cos(i * theta_per_segment)
             sin_Ti = math.sin(i * theta_per_segment)
             r_P = -offset[0] * cos_Ti + offset[1] * sin_Ti
             r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti
 
-            raw[X_AXIS] = center_P + r_P
-            raw[Y_AXIS] = center_Q + r_Q
-            raw[Z_AXIS] += linear_per_segment
-
-            coords.append([raw[X_AXIS],  raw[Y_AXIS], raw[Z_AXIS] ])
+            c = [center_P + r_P, center_Q + r_Q, currentPos[Z_AXIS] + dist_Z]
+            coords.append(c)
 
+        coords.append(targetPos)
         return coords
 
 def load_config(config):