From 3d70b645b613be63b697dd9c45089e396eac565d Mon Sep 17 00:00:00 2001
From: Ramiro Polla <ramiropolla@users.noreply.github.com>
Date: Thu, 25 Mar 2021 04:56:48 +0100
Subject: [PATCH] Optimize LSF for size, efficiency (#21443)

---
 Marlin/src/libs/least_squares_fit.cpp | 26 +++++++++++++-------------
 Marlin/src/libs/least_squares_fit.h   |  4 +---
 2 files changed, 14 insertions(+), 16 deletions(-)

diff --git a/Marlin/src/libs/least_squares_fit.cpp b/Marlin/src/libs/least_squares_fit.cpp
index c7593c049..aac21c019 100644
--- a/Marlin/src/libs/least_squares_fit.cpp
+++ b/Marlin/src/libs/least_squares_fit.cpp
@@ -46,23 +46,23 @@ int finish_incremental_LSF(struct linear_fit_data *lsf) {
   if (N == 0.0)
     return 1;
 
-  lsf->xbar /= N;
-  lsf->ybar /= N;
-  lsf->zbar /= N;
-  lsf->x2bar = lsf->x2bar / N - sq(lsf->xbar);
-  lsf->y2bar = lsf->y2bar / N - sq(lsf->ybar);
-  lsf->z2bar = lsf->z2bar / N - sq(lsf->zbar);
-  lsf->xybar = lsf->xybar / N - lsf->xbar * lsf->ybar;
-  lsf->yzbar = lsf->yzbar / N - lsf->ybar * lsf->zbar;
-  lsf->xzbar = lsf->xzbar / N - lsf->xbar * lsf->zbar;
-  const float DD = lsf->x2bar * lsf->y2bar - sq(lsf->xybar);
+  const float RN = 1.0f / N,
+              xbar = lsf->xbar * RN,
+              ybar = lsf->ybar * RN,
+              zbar = lsf->zbar * RN,
+              x2bar = lsf->x2bar * RN - sq(xbar),
+              y2bar = lsf->y2bar * RN - sq(ybar),
+              xybar = lsf->xybar * RN - xbar * ybar,
+              yzbar = lsf->yzbar * RN - ybar * zbar,
+              xzbar = lsf->xzbar * RN - xbar * zbar,
+              DD = x2bar * y2bar - sq(xybar);
 
   if (ABS(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy))
     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);
+  lsf->A = (yzbar * xybar - xzbar * y2bar) / DD;
+  lsf->B = (xzbar * xybar - yzbar * x2bar) / DD;
+  lsf->D = -(zbar + lsf->A * xbar + lsf->B * ybar);
   return 0;
 }
 
diff --git a/Marlin/src/libs/least_squares_fit.h b/Marlin/src/libs/least_squares_fit.h
index 44ca8afc7..dbbab0a55 100644
--- a/Marlin/src/libs/least_squares_fit.h
+++ b/Marlin/src/libs/least_squares_fit.h
@@ -37,7 +37,7 @@
 
 struct linear_fit_data {
   float xbar, ybar, zbar,
-        x2bar, y2bar, z2bar,
+        x2bar, y2bar,
         xybar, xzbar, yzbar,
         max_absx, max_absy,
         A, B, D, N;
@@ -56,7 +56,6 @@ inline void incremental_WLSF(struct linear_fit_data *lsf, const float &x, const
   lsf->zbar  += wz;
   lsf->x2bar += wx * x;
   lsf->y2bar += wy * y;
-  lsf->z2bar += wz * z;
   lsf->xybar += wx * y;
   lsf->xzbar += wx * z;
   lsf->yzbar += wy * z;
@@ -74,7 +73,6 @@ inline void incremental_LSF(struct linear_fit_data *lsf, const float &x, const f
   lsf->zbar += z;
   lsf->x2bar += sq(x);
   lsf->y2bar += sq(y);
-  lsf->z2bar += sq(z);
   lsf->xybar += x * y;
   lsf->xzbar += x * z;
   lsf->yzbar += y * z;