MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

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

Browse Source 
Removed the calculation of the inverse matrix since the rotation matrix is orthogonal, therefore inverted == transposed.
Much simpler and mathematically robust.
This commit is contained in:
fsantini 2013-12-06 21:32:21 +01:00
parent 5c44f6c434
commit b64661070e
4 changed files with 26 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Marlin/Marlin_main.cpp
View File

@ -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();

2
Marlin/planner.cpp
View File

@ -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");

65
Marlin/vector_3.cpp
View File

@ -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)

4
Marlin/vector_3.h
View File

@ -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();