Prettify Bed Level Correction Matrix
- Put + in front of positive values in the output
This commit is contained in:
parent
f5f4c2b366
commit
32331faee4
|
@ -26,57 +26,40 @@ vector_3::vector_3() : x(0), y(0), z(0) { }
|
||||||
|
|
||||||
vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
|
vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
|
||||||
|
|
||||||
vector_3 vector_3::cross(vector_3 left, vector_3 right)
|
vector_3 vector_3::cross(vector_3 left, vector_3 right) {
|
||||||
{
|
|
||||||
return vector_3(left.y * right.z - left.z * right.y,
|
return vector_3(left.y * right.z - left.z * right.y,
|
||||||
left.z * right.x - left.x * right.z,
|
left.z * right.x - left.x * right.z,
|
||||||
left.x * right.y - left.y * right.x);
|
left.x * right.y - left.y * right.x);
|
||||||
}
|
}
|
||||||
|
|
||||||
vector_3 vector_3::operator+(vector_3 v)
|
vector_3 vector_3::operator+(vector_3 v) { return vector_3((x + v.x), (y + v.y), (z + v.z)); }
|
||||||
{
|
vector_3 vector_3::operator-(vector_3 v) { return vector_3((x - v.x), (y - v.y), (z - v.z)); }
|
||||||
return vector_3((x + v.x), (y + v.y), (z + v.z));
|
|
||||||
}
|
|
||||||
|
|
||||||
vector_3 vector_3::operator-(vector_3 v)
|
vector_3 vector_3::get_normal() {
|
||||||
{
|
|
||||||
return vector_3((x - v.x), (y - v.y), (z - v.z));
|
|
||||||
}
|
|
||||||
|
|
||||||
vector_3 vector_3::get_normal()
|
|
||||||
{
|
|
||||||
vector_3 normalized = vector_3(x, y, z);
|
vector_3 normalized = vector_3(x, y, z);
|
||||||
normalized.normalize();
|
normalized.normalize();
|
||||||
return normalized;
|
return normalized;
|
||||||
}
|
}
|
||||||
|
|
||||||
float vector_3::get_length()
|
float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
|
||||||
{
|
|
||||||
float length = sqrt((x * x) + (y * y) + (z * z));
|
void vector_3::normalize() {
|
||||||
return length;
|
|
||||||
}
|
|
||||||
|
|
||||||
void vector_3::normalize()
|
|
||||||
{
|
|
||||||
float length = get_length();
|
float length = get_length();
|
||||||
x /= length;
|
x /= length;
|
||||||
y /= length;
|
y /= length;
|
||||||
z /= length;
|
z /= length;
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector_3::apply_rotation(matrix_3x3 matrix)
|
void vector_3::apply_rotation(matrix_3x3 matrix) {
|
||||||
{
|
|
||||||
float resultX = x * matrix.matrix[3*0+0] + y * matrix.matrix[3*1+0] + z * matrix.matrix[3*2+0];
|
float resultX = x * matrix.matrix[3*0+0] + y * matrix.matrix[3*1+0] + z * matrix.matrix[3*2+0];
|
||||||
float resultY = x * matrix.matrix[3*0+1] + y * matrix.matrix[3*1+1] + z * matrix.matrix[3*2+1];
|
float resultY = x * matrix.matrix[3*0+1] + y * matrix.matrix[3*1+1] + z * matrix.matrix[3*2+1];
|
||||||
float resultZ = x * matrix.matrix[3*0+2] + y * matrix.matrix[3*1+2] + z * matrix.matrix[3*2+2];
|
float resultZ = x * matrix.matrix[3*0+2] + y * matrix.matrix[3*1+2] + z * matrix.matrix[3*2+2];
|
||||||
|
|
||||||
x = resultX;
|
x = resultX;
|
||||||
y = resultY;
|
y = resultY;
|
||||||
z = resultZ;
|
z = resultZ;
|
||||||
}
|
}
|
||||||
|
|
||||||
void vector_3::debug(char* title)
|
void vector_3::debug(char* title) {
|
||||||
{
|
|
||||||
SERIAL_PROTOCOL(title);
|
SERIAL_PROTOCOL(title);
|
||||||
SERIAL_PROTOCOLPGM(" x: ");
|
SERIAL_PROTOCOLPGM(" x: ");
|
||||||
SERIAL_PROTOCOL_F(x, 6);
|
SERIAL_PROTOCOL_F(x, 6);
|
||||||
|
@ -87,8 +70,7 @@ void vector_3::debug(char* title)
|
||||||
SERIAL_EOL;
|
SERIAL_EOL;
|
||||||
}
|
}
|
||||||
|
|
||||||
void apply_rotation_xyz(matrix_3x3 matrix, float &x, float& y, float& z)
|
void apply_rotation_xyz(matrix_3x3 matrix, float &x, float& y, float& z) {
|
||||||
{
|
|
||||||
vector_3 vector = vector_3(x, y, z);
|
vector_3 vector = vector_3(x, y, z);
|
||||||
vector.apply_rotation(matrix);
|
vector.apply_rotation(matrix);
|
||||||
x = vector.x;
|
x = vector.x;
|
||||||
|
@ -96,48 +78,41 @@ void apply_rotation_xyz(matrix_3x3 matrix, float &x, float& y, float& z)
|
||||||
z = vector.z;
|
z = vector.z;
|
||||||
}
|
}
|
||||||
|
|
||||||
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2)
|
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
|
||||||
{
|
//row_0.debug("row_0");
|
||||||
//row_0.debug("row_0");
|
//row_1.debug("row_1");
|
||||||
//row_1.debug("row_1");
|
//row_2.debug("row_2");
|
||||||
//row_2.debug("row_2");
|
|
||||||
matrix_3x3 new_matrix;
|
matrix_3x3 new_matrix;
|
||||||
new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z;
|
new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z;
|
||||||
new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z;
|
new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z;
|
||||||
new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z;
|
new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z;
|
||||||
//new_matrix.debug("new_matrix");
|
//new_matrix.debug("new_matrix");
|
||||||
|
|
||||||
return new_matrix;
|
return new_matrix;
|
||||||
}
|
}
|
||||||
|
|
||||||
void matrix_3x3::set_to_identity()
|
void matrix_3x3::set_to_identity() {
|
||||||
{
|
|
||||||
matrix[0] = 1; matrix[1] = 0; matrix[2] = 0;
|
matrix[0] = 1; matrix[1] = 0; matrix[2] = 0;
|
||||||
matrix[3] = 0; matrix[4] = 1; matrix[5] = 0;
|
matrix[3] = 0; matrix[4] = 1; matrix[5] = 0;
|
||||||
matrix[6] = 0; matrix[7] = 0; matrix[8] = 1;
|
matrix[6] = 0; matrix[7] = 0; matrix[8] = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
matrix_3x3 matrix_3x3::create_look_at(vector_3 target)
|
matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
|
||||||
{
|
vector_3 z_row = target.get_normal();
|
||||||
vector_3 z_row = target.get_normal();
|
vector_3 x_row = vector_3(1, 0, -target.x/target.z).get_normal();
|
||||||
vector_3 x_row = vector_3(1, 0, -target.x/target.z).get_normal();
|
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
|
||||||
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
|
|
||||||
|
|
||||||
// x_row.debug("x_row");
|
// x_row.debug("x_row");
|
||||||
// y_row.debug("y_row");
|
// y_row.debug("y_row");
|
||||||
// z_row.debug("z_row");
|
// z_row.debug("z_row");
|
||||||
|
|
||||||
|
// create the matrix already correctly transposed
|
||||||
// create the matrix already correctly transposed
|
matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
|
||||||
matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
|
|
||||||
|
|
||||||
// rot.debug("rot");
|
// rot.debug("rot");
|
||||||
return rot;
|
return rot;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
|
||||||
matrix_3x3 matrix_3x3::transpose(matrix_3x3 original)
|
|
||||||
{
|
|
||||||
matrix_3x3 new_matrix;
|
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[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[3] = original.matrix[1]; new_matrix.matrix[4] = original.matrix[4]; new_matrix.matrix[5] = original.matrix[7];
|
||||||
|
@ -150,6 +125,7 @@ void matrix_3x3::debug(char* title) {
|
||||||
int count = 0;
|
int count = 0;
|
||||||
for(int i=0; i<3; i++) {
|
for(int i=0; i<3; i++) {
|
||||||
for(int j=0; j<3; j++) {
|
for(int j=0; j<3; j++) {
|
||||||
|
if (matrix[count] >= 0.0) SERIAL_PROTOCOLPGM("+");
|
||||||
SERIAL_PROTOCOL_F(matrix[count], 6);
|
SERIAL_PROTOCOL_F(matrix[count], 6);
|
||||||
SERIAL_PROTOCOLPGM(" ");
|
SERIAL_PROTOCOLPGM(" ");
|
||||||
count++;
|
count++;
|
||||||
|
@ -158,5 +134,5 @@ void matrix_3x3::debug(char* title) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // #ifdef ENABLE_AUTO_BED_LEVELING
|
#endif // ENABLE_AUTO_BED_LEVELING
|
||||||
|
|
||||||
|
|
Loading…
Reference in a new issue