diff --git a/Marlin/vector_3.cpp b/Marlin/vector_3.cpp index e6b2d70c35..f28a95a8f6 100644 --- a/Marlin/vector_3.cpp +++ b/Marlin/vector_3.cpp @@ -27,78 +27,78 @@ 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::cross(vector_3 left, vector_3 right) { - return vector_3(left.y * right.z - left.z * right.y, - left.z * right.x - left.x * right.z, - left.x * right.y - left.y * right.x); + return vector_3(left.y * right.z - left.z * right.y, + left.z * right.x - left.x * right.z, + left.x * right.y - left.y * right.x); } 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)); } vector_3 vector_3::get_normal() { - vector_3 normalized = vector_3(x, y, z); - normalized.normalize(); - return normalized; + vector_3 normalized = vector_3(x, y, z); + normalized.normalize(); + return normalized; } float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); } void vector_3::normalize() { - float length = get_length(); - x /= length; - y /= length; - z /= length; + float length = get_length(); + x /= length; + y /= length; + z /= length; } 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 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]; - x = resultX; - y = resultY; - z = resultZ; + 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 resultZ = x * matrix.matrix[3 * 0 + 2] + y * matrix.matrix[3 * 1 + 2] + z * matrix.matrix[3 * 2 + 2]; + x = resultX; + y = resultY; + z = resultZ; } void vector_3::debug(const char title[]) { - SERIAL_PROTOCOL(title); - SERIAL_PROTOCOLPGM(" x: "); - SERIAL_PROTOCOL_F(x, 6); - SERIAL_PROTOCOLPGM(" y: "); - SERIAL_PROTOCOL_F(y, 6); - SERIAL_PROTOCOLPGM(" z: "); - SERIAL_PROTOCOL_F(z, 6); - SERIAL_EOL; + SERIAL_PROTOCOL(title); + SERIAL_PROTOCOLPGM(" x: "); + SERIAL_PROTOCOL_F(x, 6); + SERIAL_PROTOCOLPGM(" y: "); + SERIAL_PROTOCOL_F(y, 6); + SERIAL_PROTOCOLPGM(" z: "); + SERIAL_PROTOCOL_F(z, 6); + SERIAL_EOL; } -void apply_rotation_xyz(matrix_3x3 matrix, float &x, float& y, float& z) { - vector_3 vector = vector_3(x, y, z); - vector.apply_rotation(matrix); - x = vector.x; - y = vector.y; - z = vector.z; +void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) { + vector_3 vector = vector_3(x, y, z); + vector.apply_rotation(matrix); + x = vector.x; + y = vector.y; + z = vector.z; } 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_1.debug("row_1"); //row_2.debug("row_2"); - 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[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; + 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[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.debug("new_matrix"); - return new_matrix; + return new_matrix; } void matrix_3x3::set_to_identity() { - matrix[0] = 1; matrix[1] = 0; matrix[2] = 0; - matrix[3] = 0; matrix[4] = 1; matrix[5] = 0; - matrix[6] = 0; matrix[7] = 0; matrix[8] = 1; + matrix[0] = 1; matrix[1] = 0; matrix[2] = 0; + matrix[3] = 0; matrix[4] = 1; matrix[5] = 0; + matrix[6] = 0; matrix[7] = 0; matrix[8] = 1; } matrix_3x3 matrix_3x3::create_look_at(vector_3 target) { 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(); // x_row.debug("x_row"); @@ -114,8 +114,8 @@ matrix_3x3 matrix_3x3::create_look_at(vector_3 target) { 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[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; } @@ -123,8 +123,8 @@ matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) { void matrix_3x3::debug(const char title[]) { SERIAL_PROTOCOLLN(title); int count = 0; - for(int i=0; i<3; i++) { - for(int j=0; j<3; j++) { + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { if (matrix[count] >= 0.0) SERIAL_PROTOCOLCHAR('+'); SERIAL_PROTOCOL_F(matrix[count], 6); SERIAL_PROTOCOLCHAR(' '); diff --git a/Marlin/vector_3.h b/Marlin/vector_3.h index 5aba350007..50a8e15dc2 100644 --- a/Marlin/vector_3.h +++ b/Marlin/vector_3.h @@ -22,41 +22,39 @@ #if ENABLED(AUTO_BED_LEVELING_FEATURE) class matrix_3x3; -struct vector_3 -{ - float x, y, z; +struct vector_3 { + float x, y, z; - vector_3(); - vector_3(float x, float y, float z); + vector_3(); + vector_3(float x, float y, float z); - static vector_3 cross(vector_3 a, vector_3 b); + static vector_3 cross(vector_3 a, vector_3 b); - vector_3 operator+(vector_3 v); - vector_3 operator-(vector_3 v); - void normalize(); - float get_length(); - vector_3 get_normal(); + vector_3 operator+(vector_3 v); + vector_3 operator-(vector_3 v); + void normalize(); + float get_length(); + vector_3 get_normal(); - void debug(const char title[]); - - void apply_rotation(matrix_3x3 matrix); + void debug(const char title[]); + + void apply_rotation(matrix_3x3 matrix); }; -struct matrix_3x3 -{ - float matrix[9]; +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); - static matrix_3x3 transpose(matrix_3x3 original); + 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); + static matrix_3x3 transpose(matrix_3x3 original); - void set_to_identity(); + void set_to_identity(); - void debug(const char title[]); + void debug(const char title[]); }; -void apply_rotation_xyz(matrix_3x3 rotationMatrix, float &x, float& y, float& z); +void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z); #endif // AUTO_BED_LEVELING_FEATURE #endif // VECTOR_3_H