Fixed the plane vector equation to a simpler one (only dependent on the normal)

Removed the calculation of the inverse matrix since the rotation matrix is orthogonal, therefore inverted == transposed.
Much simpler and mathematically robust.

Marlin/Marlin_main.cpp

@@ -807,11 +807,11 @@ static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yF
 
     vector_3 xPositive = (xRightyFront - xLeftyFront).get_normal();
     vector_3 yPositive = (xLeftyBack - xLeftyFront).get_normal();
-    vector_3 planeNormal = vector_3::cross(yPositive, xPositive).get_normal();
+    vector_3 planeNormal = vector_3::cross(xPositive, yPositive).get_normal();
 
     //planeNormal.debug("planeNormal");
     //yPositive.debug("yPositive");
-    matrix_3x3 bedLevel = matrix_3x3::create_look_at(planeNormal, yPositive);
+    plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
     //bedLevel.debug("bedLevel");
 
     //plan_bed_level_matrix.debug("bed level before");
@@ -819,7 +819,6 @@ static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yF
     //uncorrected_position.debug("position before");
 
     // and set our bed level equation to do the right thing
-    plan_bed_level_matrix = matrix_3x3::create_inverse(bedLevel);
     //plan_bed_level_matrix.debug("bed level after");
 
     vector_3 corrected_position = plan_get_position();

Marlin/planner.cpp

@@ -942,7 +942,7 @@ vector_3 plan_get_position() {
 
 	//position.debug("in plan_get position");
 	//plan_bed_level_matrix.debug("in plan_get bed_level");
-	matrix_3x3 inverse = matrix_3x3::create_inverse(plan_bed_level_matrix);
+	matrix_3x3 inverse = matrix_3x3::transpose(plan_bed_level_matrix);
 	//inverse.debug("in plan_get inverse");
 	position.apply_rotation(inverse);
 	//position.debug("after rotation");

Marlin/vector_3.cpp

@@ -127,57 +127,34 @@ void matrix_3x3::set_to_identity()
 	matrix[6] = 0; matrix[7] = 0; matrix[8] = 1;
 }
 
-matrix_3x3 matrix_3x3::create_look_at(vector_3 target, vector_3 up)
+matrix_3x3 matrix_3x3::create_look_at(vector_3 target)
 {
-    // There are lots of examples of look at code on the internet that don't do all these noramize and also find the position
-    // through several dot products.  The problem with them is that they have a bit of error in that all the vectors arn't normal and need to be.
-    vector_3 z_row = vector_3(-target.x, -target.y, -target.z).get_normal();
-    vector_3 x_row = vector_3::cross(up, z_row).get_normal();
-    vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
+    vector_3 z_row = vector_3(target.x, target.y, target.z).get_normal();
+    vector_3 x_row = vector_3(1, 0, -target.x/target.z).get_normal();
+    vector_3 y_row = vector_3(0, 1, -target.y/target.z).get_normal();
 
    // x_row.debug("x_row");
    // y_row.debug("y_row");
    // z_row.debug("z_row");
 
-    matrix_3x3 rot = matrix_3x3::create_from_rows(vector_3(x_row.x, y_row.x, z_row.x),
-                                vector_3(x_row.y, y_row.y, z_row.y),
-                                vector_3(x_row.z, y_row.z, z_row.z));
+ 
+     // create the matrix already correctly transposed
+    matrix_3x3 rot = matrix_3x3::create_from_rows(vector_3(x_row.x, x_row.y, x_row.z),
+                                vector_3(y_row.x, y_row.y, y_row.z),
+                                vector_3(z_row.x, z_row.y, z_row.z));
 
  //   rot.debug("rot");
     return rot;
 }
 
-matrix_3x3 matrix_3x3::create_inverse(matrix_3x3 original)
-{
-	//original.debug("original");
-	float* A = original.matrix;
-	float determinant = 
-		+ A[0 * 3 + 0] * (A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2])
-		- A[0 * 3 + 1] * (A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0])
-		+ A[0 * 3 + 2] * (A[1 * 3 + 0] * A[2 * 3 + 1] - A[1 * 3 + 1] * A[2 * 3 + 0]);
-	matrix_3x3 inverse;
-	inverse.matrix[0 * 3 + 0] = +(A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2]) / determinant;
-	inverse.matrix[0 * 3 + 1] = -(A[0 * 3 + 1] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 1]) / determinant;
-	inverse.matrix[0 * 3 + 2] = +(A[0 * 3 + 1] * A[1 * 3 + 2] - A[0 * 3 + 2] * A[1 * 3 + 1]) / determinant;
-	inverse.matrix[1 * 3 + 0] = -(A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0]) / determinant;
-	inverse.matrix[1 * 3 + 1] = +(A[0 * 3 + 0] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 0]) / determinant;
-	inverse.matrix[1 * 3 + 2] = -(A[0 * 3 + 0] * A[1 * 3 + 2] - A[1 * 3 + 0] * A[0 * 3 + 2]) / determinant;
-	inverse.matrix[2 * 3 + 0] = +(A[1 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[1 * 3 + 1]) / determinant;
-	inverse.matrix[2 * 3 + 1] = -(A[0 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[0 * 3 + 1]) / determinant;
-	inverse.matrix[2 * 3 + 2] = +(A[0 * 3 + 0] * A[1 * 3 + 1] - A[1 * 3 + 0] * A[0 * 3 + 1]) / determinant;
-
-	vector_3 row0 = vector_3(inverse.matrix[0 * 3 + 0], inverse.matrix[0 * 3 + 1], inverse.matrix[0 * 3 + 2]);
-	vector_3 row1 = vector_3(inverse.matrix[1 * 3 + 0], inverse.matrix[1 * 3 + 1], inverse.matrix[1 * 3 + 2]);
-	vector_3 row2 = vector_3(inverse.matrix[2 * 3 + 0], inverse.matrix[2 * 3 + 1], inverse.matrix[2 * 3 + 2]);
-
-    row0.normalize();
-    row1.normalize();
-    row2.normalize();
-
-	inverse = matrix_3x3::create_from_rows(row0, row1, row2);
-
-	//inverse.debug("inverse");
-	return inverse;
+
+matrix_3x3 matrix_3x3::transpose(matrix_3x3 original)
+{
+  matrix_3x3 new_matrix;
+  new_matrix.matrix[0] = original.matrix[0]; new_matrix.matrix[1] = original.matrix[3]; new_matrix.matrix[2] = original.matrix[6]; 
+  new_matrix.matrix[3] = original.matrix[1]; new_matrix.matrix[4] = original.matrix[4]; new_matrix.matrix[5] = original.matrix[7]; 
+  new_matrix.matrix[6] = original.matrix[2]; new_matrix.matrix[7] = original.matrix[5]; new_matrix.matrix[8] = original.matrix[8];
+  return new_matrix;
 }
 
 void matrix_3x3::debug(char* title)

Marlin/vector_3.h

@@ -47,8 +47,8 @@ struct matrix_3x3
 	float matrix[9];
 
 	static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
-	static matrix_3x3 create_look_at(vector_3 target, vector_3 up);
-	static matrix_3x3 create_inverse(matrix_3x3 original);
+	static matrix_3x3 create_look_at(vector_3 target);
+	static matrix_3x3 transpose(matrix_3x3 original);
 
 	void set_to_identity();