mathutil: Move trilateration code from delta.py to mathutil.py

Move the trilateration algorithm to mathutil.py.  It may be useful
outside of delta kinematics, and it complicates the delta code.

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

2 files changed, 67 insertions, 58 deletions

diff --git a/klippy/delta.py b/klippy/delta.py
index 08fc91f6..3efed7db 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -4,7 +4,7 @@
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import math, logging
-import stepper, homing, chelper
+import stepper, homing, chelper, mathutil
 
 # Slow moves once the ratio of tower to XY movement exceeds SLOW_RATIO
 SLOW_RATIO = 3.
@@ -85,8 +85,9 @@ class DeltaKinematics:
         self.set_position([0., 0., 0.], ())
     def get_rails(self, flags=""):
         return list(self.rails)
-    def _actuator_to_cartesian(self, pos):
-        return actuator_to_cartesian(self.towers, self.arm2, pos)
+    def _actuator_to_cartesian(self, spos):
+        sphere_coords = [(t[0], t[1], sp) for t, sp in zip(self.towers, spos)]
+        return mathutil.trilateration(sphere_coords, self.arm2)
     def calc_position(self):
         spos = [rail.get_commanded_position() for rail in self.rails]
         return self._actuator_to_cartesian(spos)
@@ -183,57 +184,6 @@ class DeltaKinematics:
             'arm_a': self.arm_lengths[0], 'arm_b': self.arm_lengths[1],
             'arm_c': self.arm_lengths[2] }
 
-
-######################################################################
-# Matrix helper functions for 3x1 matrices
-######################################################################
-
-def matrix_cross(m1, m2):
-    return [m1[1] * m2[2] - m1[2] * m2[1],
-            m1[2] * m2[0] - m1[0] * m2[2],
-            m1[0] * m2[1] - m1[1] * m2[0]]
-
-def matrix_dot(m1, m2):
-    return m1[0] * m2[0] + m1[1] * m2[1] + m1[2] * m2[2]
-
-def matrix_magsq(m1):
-    return m1[0]**2 + m1[1]**2 + m1[2]**2
-
-def matrix_add(m1, m2):
-    return [m1[0] + m2[0], m1[1] + m2[1], m1[2] + m2[2]]
-
-def matrix_sub(m1, m2):
-    return [m1[0] - m2[0], m1[1] - m2[1], m1[2] - m2[2]]
-
-def matrix_mul(m1, s):
-    return [m1[0]*s, m1[1]*s, m1[2]*s]
-
-def actuator_to_cartesian(towers, arm2, pos):
-    # Find nozzle position using trilateration (see wikipedia)
-    carriage1 = list(towers[0]) + [pos[0]]
-    carriage2 = list(towers[1]) + [pos[1]]
-    carriage3 = list(towers[2]) + [pos[2]]
-
-    s21 = matrix_sub(carriage2, carriage1)
-    s31 = matrix_sub(carriage3, carriage1)
-
-    d = math.sqrt(matrix_magsq(s21))
-    ex = matrix_mul(s21, 1. / d)
-    i = matrix_dot(ex, s31)
-    vect_ey = matrix_sub(s31, matrix_mul(ex, i))
-    ey = matrix_mul(vect_ey, 1. / math.sqrt(matrix_magsq(vect_ey)))
-    ez = matrix_cross(ex, ey)
-    j = matrix_dot(ey, s31)
-
-    x = (arm2[0] - arm2[1] + d**2) / (2. * d)
-    y = (arm2[0] - arm2[2] - x**2 + (x-i)**2 + j**2) / (2. * j)
-    z = -math.sqrt(arm2[0] - x**2 - y**2)
-
-    ex_x = matrix_mul(ex, x)
-    ey_y = matrix_mul(ey, y)
-    ez_z = matrix_mul(ez, z)
-    return matrix_add(carriage1, matrix_add(ex_x, matrix_add(ey_y, ez_z)))
-
 def get_position_from_stable(spos, params):
     angles = [params['angle_a'], params['angle_b'], params['angle_c']]
     radius = params['radius']
@@ -242,6 +192,6 @@ def get_position_from_stable(spos, params):
               for angle in map(math.radians, angles)]
     arm2 = [a**2 for a in [params['arm_a'], params['arm_b'], params['arm_c']]]
     endstops = [params['endstop_a'], params['endstop_b'], params['endstop_c']]
-    pos = [es + math.sqrt(a2 - radius2) - p
-           for es, a2, p in zip(endstops, arm2, spos)]
-    return actuator_to_cartesian(towers, arm2, pos)
+    sphere_coords = [(t[0], t[1], es + math.sqrt(a2 - radius2) - p)
+                     for t, es, a2, p in zip(towers, endstops, arm2, spos)]
+    return mathutil.trilateration(sphere_coords, arm2)
diff --git a/klippy/mathutil.py b/klippy/mathutil.py
index d8df3539..2e39c5d1 100644
--- a/klippy/mathutil.py
+++ b/klippy/mathutil.py
@@ -3,7 +3,12 @@
 # Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
-import logging
+import math, logging
+
+
+######################################################################
+# Coordinate descent
+######################################################################
 
 # Helper code that implements coordinate descent
 def coordinate_descent(adj_params, params, error_func):
@@ -38,3 +43,57 @@ def coordinate_descent(adj_params, params, error_func):
             dp[param_name] *= 0.9
     logging.info("Coordinate descent best_err: %s  rounds: %d", best_err, rounds)
     return params
+
+
+######################################################################
+# Trilateration
+######################################################################
+
+# Trilateration finds the intersection of three spheres.  See the
+# wikipedia article for the details of the algorithm.
+def trilateration(sphere_coords, radius2):
+    sphere_coord1, sphere_coord2, sphere_coord3 = sphere_coords
+    s21 = matrix_sub(sphere_coord2, sphere_coord1)
+    s31 = matrix_sub(sphere_coord3, sphere_coord1)
+
+    d = math.sqrt(matrix_magsq(s21))
+    ex = matrix_mul(s21, 1. / d)
+    i = matrix_dot(ex, s31)
+    vect_ey = matrix_sub(s31, matrix_mul(ex, i))
+    ey = matrix_mul(vect_ey, 1. / math.sqrt(matrix_magsq(vect_ey)))
+    ez = matrix_cross(ex, ey)
+    j = matrix_dot(ey, s31)
+
+    x = (radius2[0] - radius2[1] + d**2) / (2. * d)
+    y = (radius2[0] - radius2[2] - x**2 + (x-i)**2 + j**2) / (2. * j)
+    z = -math.sqrt(radius2[0] - x**2 - y**2)
+
+    ex_x = matrix_mul(ex, x)
+    ey_y = matrix_mul(ey, y)
+    ez_z = matrix_mul(ez, z)
+    return matrix_add(sphere_coord1, matrix_add(ex_x, matrix_add(ey_y, ez_z)))
+
+
+######################################################################
+# Matrix helper functions for 3x1 matrices
+######################################################################
+
+def matrix_cross(m1, m2):
+    return [m1[1] * m2[2] - m1[2] * m2[1],
+            m1[2] * m2[0] - m1[0] * m2[2],
+            m1[0] * m2[1] - m1[1] * m2[0]]
+
+def matrix_dot(m1, m2):
+    return m1[0] * m2[0] + m1[1] * m2[1] + m1[2] * m2[2]
+
+def matrix_magsq(m1):
+    return m1[0]**2 + m1[1]**2 + m1[2]**2
+
+def matrix_add(m1, m2):
+    return [m1[0] + m2[0], m1[1] + m2[1], m1[2] + m2[2]]
+
+def matrix_sub(m1, m2):
+    return [m1[0] - m2[0], m1[1] - m2[1], m1[2] - m2[2]]
+
+def matrix_mul(m1, s):
+    return [m1[0]*s, m1[1]*s, m1[2]*s]