From 259115bb3f75fb6cf99e19fce230a327d8afd094 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Tue, 30 Mar 2021 23:16:29 -0500
Subject: [PATCH] Move apply_rotation_xyz into matrix_3x3

---
 Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp |  2 +-
 Marlin/src/gcode/bedlevel/abl/G29.cpp       |  2 +-
 Marlin/src/libs/vector_3.cpp                |  4 ++--
 Marlin/src/libs/vector_3.h                  | 10 ++++------
 Marlin/src/module/planner.cpp               |  4 ++--
 5 files changed, 10 insertions(+), 12 deletions(-)

diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
index 2003e9b6f5..c11962bf9d 100644
--- a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
+++ b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
@@ -1592,7 +1592,7 @@ void unified_bed_leveling::smart_fill_mesh() {
         DEBUG_DELAY(20);
       }
 
-      apply_rotation_xyz(rotation, mx, my, mz);
+      rotation.apply_rotation_xyz(mx, my, mz);
 
       if (DEBUGGING(LEVELING)) {
         DEBUG_ECHOPAIR_F("after rotation = [", mx, 7);
diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp
index f3cfe02e40..42d3a75da1 100644
--- a/Marlin/src/gcode/bedlevel/abl/G29.cpp
+++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp
@@ -794,7 +794,7 @@ G29_TYPE GcodeSuite::G29() {
               const int ind = abl.indexIntoAB[xx][yy];
               xyz_float_t tmp = { abl.eqnAMatrix[ind + 0 * abl.abl_points],
                                   abl.eqnAMatrix[ind + 1 * abl.abl_points], 0 };
-              apply_rotation_xyz(planner.bed_level_matrix, tmp);
+              planner.bed_level_matrix.apply_rotation_xyz(tmp);
               if (get_min) NOMORE(min_diff, abl.eqnBVector[ind] - tmp.z);
               const float subval = get_min ? abl.mean : tmp.z + min_diff,
                             diff = abl.eqnBVector[ind] - subval;
diff --git a/Marlin/src/libs/vector_3.cpp b/Marlin/src/libs/vector_3.cpp
index 0ef29f33bd..f1bff7d4c6 100644
--- a/Marlin/src/libs/vector_3.cpp
+++ b/Marlin/src/libs/vector_3.cpp
@@ -87,8 +87,8 @@ void vector_3::debug(PGM_P const title) {
  *  matrix_3x3
  */
 
-void apply_rotation_xyz(const matrix_3x3 &matrix, float &_x, float &_y, float &_z) {
-  vector_3 vec = vector_3(_x, _y, _z); vec.apply_rotation(matrix);
+void matrix_3x3::apply_rotation_xyz(float &_x, float &_y, float &_z) {
+  vector_3 vec = vector_3(_x, _y, _z); vec.apply_rotation(*this);
   _x = vec.x; _y = vec.y; _z = vec.z;
 }
 
diff --git a/Marlin/src/libs/vector_3.h b/Marlin/src/libs/vector_3.h
index 58428314d9..764e10accb 100644
--- a/Marlin/src/libs/vector_3.h
+++ b/Marlin/src/libs/vector_3.h
@@ -45,7 +45,6 @@
 class matrix_3x3;
 
 struct vector_3 : xyz_float_t {
-
   vector_3(const float &_x, const float &_y, const float &_z) { set(_x, _y, _z); }
   vector_3(const xy_float_t   &in) { set(in.x, in.y); }
   vector_3(const xyz_float_t  &in) { set(in.x, in.y, in.z); }
@@ -82,9 +81,8 @@ struct matrix_3x3 {
   void set_to_identity();
 
   void debug(PGM_P const title);
-};
 
-void apply_rotation_xyz(const matrix_3x3 &rotationMatrix, float &x, float &y, float &z);
-FORCE_INLINE void apply_rotation_xyz(const matrix_3x3 &rotationMatrix, xyz_pos_t &pos) {
-  apply_rotation_xyz(rotationMatrix, pos.x, pos.y, pos.z);
-}
+  void apply_rotation_xyz(float &x, float &y, float &z);
+
+  FORCE_INLINE void apply_rotation_xyz(xyz_pos_t &pos) { apply_rotation_xyz(pos.x, pos.y, pos.z); }
+};
diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp
index 832d6e316a..eee1a9f269 100644
--- a/Marlin/src/module/planner.cpp
+++ b/Marlin/src/module/planner.cpp
@@ -1534,7 +1534,7 @@ void Planner::check_axes_activity() {
     #if ABL_PLANAR
 
       xy_pos_t d = raw - level_fulcrum;
-      apply_rotation_xyz(bed_level_matrix, d.x, d.y, raw.z);
+      bed_level_matrix.apply_rotation_xyz(d.x, d.y, raw.z);
       raw = d + level_fulcrum;
 
     #elif HAS_MESH
@@ -1571,7 +1571,7 @@ void Planner::check_axes_activity() {
         matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
 
         xy_pos_t d = raw - level_fulcrum;
-        apply_rotation_xyz(inverse, d.x, d.y, raw.z);
+        inverse.apply_rotation_xyz(d.x, d.y, raw.z);
         raw = d + level_fulcrum;
 
       #elif HAS_MESH