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>

1 files changed, 60 insertions, 1 deletions

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]