Clean up least_squares_fit

Marlin/least_squares_fit.cpp

@@ -21,13 +21,13 @@
  */
 
 /**
- * Least Squares Best Fit  By Roxy and Ed Williams
+ * Least Squares Best Fit by Roxy and Ed Williams
  *
  * This algorithm is high speed and has a very small code footprint.
  * Its results are identical to both the Iterative Least-Squares published
  * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE
- * it saves roughly 10K of program memory.   It also does not require all of 
+ * it saves roughly 10K of program memory. It also does not require all of
- * coordinates to be present during the calculations.  Each point can be 
+ * coordinates to be present during the calculations. Each point can be
  * probed and then discarded.
  *
  */
@@ -41,56 +41,44 @@
 
 #include "least_squares_fit.h"
 
-void incremental_LSF_reset(struct linear_fit_data *lsf) {
+void incremental_LSF_reset(struct linear_fit_data *lsf) { ZERO(lsf); }
-	lsf->n = 0;
-	lsf->A = 0.0;					// probably a memset() can be done to zero 
-	lsf->B = 0.0;                                   // this whole structure
-	lsf->D = 0.0;
-	lsf->xbar = lsf->ybar = lsf->zbar = 0.0;
-	lsf->x2bar = lsf->y2bar = lsf->z2bar = 0.0;
-	lsf->xybar = lsf->xzbar = lsf->yzbar = 0.0;
-	lsf->max_absx = lsf->max_absy = 0.0;
-    }
 
 void incremental_LSF(struct linear_fit_data *lsf, float x, float y, float z) {
-	lsf->xbar += x;
+  lsf->xbar += x;
-	lsf->ybar += y;
+  lsf->ybar += y;
-	lsf->zbar += z;
+  lsf->zbar += z;
-	lsf->x2bar += x*x;
+  lsf->x2bar += sq(x);
-	lsf->y2bar += y*y;
+  lsf->y2bar += sq(y);
-	lsf->z2bar += z*z;
+  lsf->z2bar += sq(z);
-	lsf->xybar += x*y;
+  lsf->xybar += sq(x);
-	lsf->xzbar += x*z;
+  lsf->xzbar += sq(x);
-	lsf->yzbar += y*z;
+  lsf->yzbar += sq(y);
-	lsf->max_absx = (fabs(x) > lsf->max_absx) ? fabs(x) : lsf->max_absx;
+  lsf->max_absx = max(fabs(x), lsf->max_absx);
-	lsf->max_absy = (fabs(y) > lsf->max_absy) ? fabs(y) : lsf->max_absy;
+  lsf->max_absy = max(fabs(y), lsf->max_absy);
-	lsf->n++;
+  lsf->n++;
-	return;
+}
-  }
 
 int finish_incremental_LSF(struct linear_fit_data *lsf) {
-	float DD, N;
+  const float N = (float)lsf->n;
 
-	N = (float) lsf->n;
+  lsf->xbar /= N;
-	lsf->xbar /= N;
+  lsf->ybar /= N;
-	lsf->ybar /= N;
+  lsf->zbar /= N;
-	lsf->zbar /= N;
+  lsf->x2bar = lsf->x2bar / N - lsf->xbar * lsf->xbar;
-	lsf->x2bar = lsf->x2bar/N - lsf->xbar*lsf->xbar;
+  lsf->y2bar = lsf->y2bar / N - lsf->ybar * lsf->ybar;
-	lsf->y2bar = lsf->y2bar/N - lsf->ybar*lsf->ybar;
+  lsf->z2bar = lsf->z2bar / N - lsf->zbar * lsf->zbar;
-	lsf->z2bar = lsf->z2bar/N - lsf->zbar*lsf->zbar;
+  lsf->xybar = lsf->xybar / N - lsf->xbar * lsf->ybar;
-	lsf->xybar = lsf->xybar/N - lsf->xbar*lsf->ybar;
+  lsf->yzbar = lsf->yzbar / N - lsf->ybar * lsf->zbar;
-	lsf->yzbar = lsf->yzbar/N - lsf->ybar*lsf->zbar;
+  lsf->xzbar = lsf->xzbar / N - lsf->xbar * lsf->zbar;
-	lsf->xzbar = lsf->xzbar/N - lsf->xbar*lsf->zbar;
 
-	DD = lsf->x2bar*lsf->y2bar - lsf->xybar*lsf->xybar;
+  const float DD = lsf->x2bar * lsf->y2bar - sq(lsf->xybar);
-	if (fabs(DD) <= 1e-10*(lsf->max_absx+lsf->max_absy)) 
+  if (fabs(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy))
-	  return -1;
+    return -1;
-	
-	lsf->A = (lsf->yzbar*lsf->xybar - lsf->xzbar*lsf->y2bar) / DD;
-	lsf->B = (lsf->xzbar*lsf->xybar - lsf->yzbar*lsf->x2bar) / DD;
-	lsf->D = -(lsf->zbar + lsf->A*lsf->xbar + lsf->B*lsf->ybar);
-	return 0;
-}
-#endif
 
+  lsf->A = (lsf->yzbar * lsf->xybar - lsf->xzbar * lsf->y2bar) / DD;
+  lsf->B = (lsf->xzbar * lsf->xybar - lsf->yzbar * lsf->x2bar) / DD;
+  lsf->D = -(lsf->zbar + lsf->A * lsf->xbar + lsf->B * lsf->ybar);
+  return 0;
+}
 
+#endif // AUTO_BED_LEVELING_UBL

Marlin/least_squares_fit.h

@@ -27,7 +27,7 @@
  * Its results are identical to both the Iterative Least-Squares published
  * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE
  * it saves roughly 10K of program memory.   And even better...  the data
- * fed into the algorithm does not need to all be present at the same time.  
+ * fed into the algorithm does not need to all be present at the same time.
  * A point can be probed and its values fed into the algorithm and then discarded.
  *
  */
@@ -42,14 +42,14 @@
 
 struct linear_fit_data {
   int n;
-  float xbar, ybar, zbar;
+  float xbar, ybar, zbar,
-  float x2bar, y2bar, z2bar;
+        x2bar, y2bar, z2bar,
-  float xybar, xzbar, yzbar;
+        xybar, xzbar, yzbar,
-  float max_absx, max_absy;
+        max_absx, max_absy,
-  float A, B, D;
+        A, B, D;
 };
 
-void incremental_LSF_reset(struct linear_fit_data *); 
+void incremental_LSF_reset(struct linear_fit_data *);
 void incremental_LSF(struct linear_fit_data *, float x, float y, float z);
 int finish_incremental_LSF(struct linear_fit_data *);