From 499e404fbfd23acc7c1ef007ebff960991a880cf Mon Sep 17 00:00:00 2001
From: AnHardt <github@kitelab.de>
Date: Tue, 7 Jun 2016 01:44:14 +0200
Subject: [PATCH] forwardKinematics for Delta printers

---
 Marlin/Marlin_main.cpp | 70 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 70 insertions(+)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 3e2c641acf..50ce093718 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -7780,6 +7780,76 @@ void clamp_to_software_endstops(float target[3]) {
     return abs(distance - delta[TOWER_3]);
   }
 
+  float cartesian[3]; // result
+  void forwardKinematics(float z1, float z2, float z3) {
+    //As discussed in Wikipedia "Trilateration"
+    //we are establishing a new coordinate
+    //system in the plane of the three carriage points.
+    //This system will have the origin at tower1 and
+    //tower2 is on the x axis. tower3 is in the X-Y
+    //plane with a Z component of zero. We will define unit
+    //vectors in this coordinate system in our original
+    //coordinate system. Then when we calculate the
+    //Xnew, Ynew and Znew values, we can translate back into
+    //the original system by moving along those unit vectors
+    //by the corresponding values.
+    // https://en.wikipedia.org/wiki/Trilateration
+
+    // Variable names matched to Marlin, c-version
+    // and avoiding a vector library
+    // by Andreas Hardtung 2016-06-7
+    // based on a Java function from
+    // "Delta Robot Kinematics by Steve Graves" V3
+
+    // Result is in cartesian[].
+
+    //Create a vector in old coords along x axis of new coord
+    float p12[3] = { delta_tower2_x - delta_tower1_x, delta_tower2_y - delta_tower1_y, z2 - z1 };
+
+    //Get the Magnitude of vector.
+    float d = sqrt( p12[0]*p12[0] + p12[1]*p12[1] + p12[2]*p12[2] );
+
+    //Create unit vector by dividing by magnitude.
+    float ex[3] = { p12[0]/d, p12[1]/d, p12[2]/d };
+
+    //Now find vector from the origin of the new system to the third point.
+    float p13[3] = { delta_tower3_x - delta_tower1_x, delta_tower3_y - delta_tower1_y, z3 - z1 };
+
+    //Now use dot product to find the component of this vector on the X axis.
+    float i = ex[0]*p13[0] + ex[1]*p13[1] + ex[2]*p13[2];
+
+    //Now create a vector along the x axis that represents the x component of p13.
+    float iex[3] = { ex[0]*i,  ex[1]*i,  ex[2]*i  };
+
+    //Now subtract the X component away from the original vector leaving only the Y component. We use the
+    //variable that will be the unit vector after we scale it.
+    float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2]};
+
+    //The magnitude of Y component
+    float j = sqrt(sq(ey[0]) + sq(ey[1]) + sq(ey[2]));
+
+    //Now make vector a unit vector
+    ey[0] /= j; ey[1] /= j;  ey[2] /= j;
+
+    //The cross product of the unit x and y is the unit z
+    //float[] ez = vectorCrossProd(ex, ey);
+    float ez[3] = { ex[1]*ey[2] - ex[2]*ey[1], ex[2]*ey[0] - ex[0]*ey[2], ex[0]*ey[1] - ex[1]*ey[0] };
+
+    //Now we have the d, i and j values defined in Wikipedia.
+    //We can plug them into the equations defined in
+    //Wikipedia for Xnew, Ynew and Znew
+    float Xnew = (delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_2 + d*d)/(d*2);
+    float Ynew = ((delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_3 + i*i + j*j)/2 - i*Xnew) /j;
+    float Znew = sqrt(delta_diagonal_rod_2_tower_1 - Xnew*Xnew - Ynew*Ynew);
+
+    //Now we can start from the origin in the old coords and
+    //add vectors in the old coords that represent the
+    //Xnew, Ynew and Znew to find the point in the old system
+    cartesian[X_AXIS] = delta_tower1_x + ex[0]*Xnew + ey[0]*Ynew - ez[0]*Znew;
+    cartesian[Y_AXIS] = delta_tower1_y + ex[1]*Xnew + ey[1]*Ynew - ez[1]*Znew;
+    cartesian[Z_AXIS] = z1             + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
+  };
+
   #if ENABLED(AUTO_BED_LEVELING_FEATURE)
 
     // Adjust print surface height by linear interpolation over the bed_level array.