diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 8993ca6955..6c662e5099 100644 --- a/Marlin/Marlin_main.cpp +++ b/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(); diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 008c8d257f..d9299c4a7e 100644 --- a/Marlin/planner.cpp +++ b/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"); diff --git a/Marlin/vector_3.cpp b/Marlin/vector_3.cpp index 8c8a0e1dc7..c9f4803231 100644 --- a/Marlin/vector_3.cpp +++ b/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)); + // x_row.debug("x_row"); + // y_row.debug("y_row"); + // z_row.debug("z_row"); - //rot.debug("rot"); + + // 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) + +matrix_3x3 matrix_3x3::transpose(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 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) diff --git a/Marlin/vector_3.h b/Marlin/vector_3.h index b08c336e8f..0b9decafad 100644 --- a/Marlin/vector_3.h +++ b/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();