From 2992112da038e1ec57a1a378a5dcdb7076aa3453 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Wed, 4 Jul 2018 22:28:34 -0500 Subject: [PATCH] Optimize delta kinematics Co-Authored-By: ejtagle --- Marlin/src/module/delta.cpp | 34 +++++++++++++++++----------------- Marlin/src/module/delta.h | 8 ++++---- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/Marlin/src/module/delta.cpp b/Marlin/src/module/delta.cpp index 2fe924a42..f005d6298 100644 --- a/Marlin/src/module/delta.cpp +++ b/Marlin/src/module/delta.cpp @@ -155,38 +155,38 @@ float delta_safe_distance_from_top() { * * The result is stored in the cartes[] array. */ -void forward_kinematics_DELTA(float z1, float z2, float z3) { +void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3) { // Create a vector in old coordinates along x axis of new coordinate - float p12[3] = { delta_tower[B_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[B_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z2 - z1 }; + const float p12[3] = { delta_tower[B_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[B_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z2 - z1 }; - // Get the Magnitude of vector. - float d = SQRT( sq(p12[0]) + sq(p12[1]) + sq(p12[2]) ); + // Get the reciprocal of Magnitude of vector. + const float d2 = sq(p12[0]) + sq(p12[1]) + sq(p12[2]), inv_d = RSQRT(d2); - // Create unit vector by dividing by magnitude. - float ex[3] = { p12[0] / d, p12[1] / d, p12[2] / d }; + // Create unit vector by multiplying by the inverse of the magnitude. + const float ex[3] = { p12[0] * inv_d, p12[1] * inv_d, p12[2] * inv_d }; // Get the vector from the origin of the new system to the third point. - float p13[3] = { delta_tower[C_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[C_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z3 - z1 }; + const float p13[3] = { delta_tower[C_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[C_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z3 - z1 }; // Use the 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]; + const float i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2]; // 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 }; + const float iex[3] = { ex[0] * i, ex[1] * i, ex[2] * i }; // Subtract the X component from the original vector leaving only Y. 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]) ); + // The magnitude and the inverse of the magnitude of Y component + const float j2 = sq(ey[0]) + sq(ey[1]) + sq(ey[2]), inv_j = RSQRT(j2); // Convert to a unit vector - ey[0] /= j; ey[1] /= j; ey[2] /= j; + ey[0] *= inv_j; ey[1] *= inv_j; ey[2] *= inv_j; // The cross product of the unit x and y is the unit z // float[] ez = vectorCrossProd(ex, ey); - float ez[3] = { + const 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] @@ -194,16 +194,16 @@ void forward_kinematics_DELTA(float z1, float z2, float z3) { // We now have the d, i and j values defined in Wikipedia. // Plug them into the equations defined in Wikipedia for Xnew, Ynew and Znew - float Xnew = (delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[B_AXIS] + sq(d)) / (d * 2), - Ynew = ((delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[C_AXIS] + HYPOT2(i, j)) / 2 - i * Xnew) / j, - Znew = SQRT(delta_diagonal_rod_2_tower[A_AXIS] - HYPOT2(Xnew, Ynew)); + const float Xnew = (delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[B_AXIS] + d2) * inv_d * 0.5, + Ynew = ((delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[C_AXIS] + sq(i) + j2) * 0.5 - i * Xnew) * inv_j, + Znew = SQRT(delta_diagonal_rod_2_tower[A_AXIS] - HYPOT2(Xnew, Ynew)); // Start from the origin of the old coordinates and add vectors in the // old coords that represent the Xnew, Ynew and Znew to find the point // in the old system. cartes[X_AXIS] = delta_tower[A_AXIS][X_AXIS] + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew; cartes[Y_AXIS] = delta_tower[A_AXIS][Y_AXIS] + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew; - cartes[Z_AXIS] = z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew; + cartes[Z_AXIS] = z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew; } #if ENABLED(SENSORLESS_HOMING) diff --git a/Marlin/src/module/delta.h b/Marlin/src/module/delta.h index cf62b5e62..2dde72f80 100644 --- a/Marlin/src/module/delta.h +++ b/Marlin/src/module/delta.h @@ -65,14 +65,14 @@ void recalc_delta_settings(); */ // Macro to obtain the Z position of an individual tower -#define DELTA_Z(V,T) V[Z_AXIS] + SQRT( \ +#define DELTA_Z(V,T) V[Z_AXIS] + SQRT( \ delta_diagonal_rod_2_tower[T] - HYPOT2( \ delta_tower[T][X_AXIS] - V[X_AXIS], \ delta_tower[T][Y_AXIS] - V[Y_AXIS] \ ) \ ) -#define DELTA_IK(V) do { \ +#define DELTA_IK(V) do { \ delta[A_AXIS] = DELTA_Z(V, A_AXIS); \ delta[B_AXIS] = DELTA_Z(V, B_AXIS); \ delta[C_AXIS] = DELTA_Z(V, C_AXIS); \ @@ -111,9 +111,9 @@ float delta_safe_distance_from_top(); * * The result is stored in the cartes[] array. */ -void forward_kinematics_DELTA(float z1, float z2, float z3); +void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3); -FORCE_INLINE void forward_kinematics_DELTA(float point[ABC]) { +FORCE_INLINE void forward_kinematics_DELTA(const float (&point)[ABC]) { forward_kinematics_DELTA(point[A_AXIS], point[B_AXIS], point[C_AXIS]); }