sravan/Marlin-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Keep "astyled" elements in vector_3.*

Browse Source
This commit is contained in:
Scott Lahteine
2015-10-02 23:19:13 -07:00
parent 7909c6e4dd
commit c2d4a1db58
2 changed files with 64 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
Marlin/vector_3.cpp
View File

@@ -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(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) { 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::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 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() { return sqrt((x * x) + (y * y) + (z * z)); } float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
void vector_3::normalize() { 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(const char title[]) { void vector_3::debug(const char title[]) {
SERIAL_PROTOCOL(title); SERIAL_PROTOCOL(title);
SERIAL_PROTOCOLPGM(" x: "); SERIAL_PROTOCOLPGM(" x: ");
SERIAL_PROTOCOL_F(x, 6); SERIAL_PROTOCOL_F(x, 6);
SERIAL_PROTOCOLPGM(" y: "); SERIAL_PROTOCOLPGM(" y: ");
SERIAL_PROTOCOL_F(y, 6); SERIAL_PROTOCOL_F(y, 6);
SERIAL_PROTOCOLPGM(" z: "); SERIAL_PROTOCOLPGM(" z: ");
SERIAL_PROTOCOL_F(z, 6); SERIAL_PROTOCOL_F(z, 6);
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;
y = vector.y; y = vector.y;
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");
@@ -123,8 +123,8 @@ matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
void matrix_3x3::debug(const char title[]) { void matrix_3x3::debug(const char title[]) {
SERIAL_PROTOCOLLN(title); SERIAL_PROTOCOLLN(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_PROTOCOLCHAR('+'); if (matrix[count] >= 0.0) SERIAL_PROTOCOLCHAR('+');
SERIAL_PROTOCOL_F(matrix[count], 6); SERIAL_PROTOCOL_F(matrix[count], 6);
SERIAL_PROTOCOLCHAR(' '); SERIAL_PROTOCOLCHAR(' ');

42
Marlin/vector_3.h
View File

@@ -22,41 +22,39 @@
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #if ENABLED(AUTO_BED_LEVELING_FEATURE)
class matrix_3x3; class matrix_3x3;
struct vector_3 struct vector_3 {
{ float x, y, z;
float x, y, z;
vector_3(); vector_3();
vector_3(float x, float y, float z); 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);
vector_3 operator-(vector_3 v); vector_3 operator-(vector_3 v);
void normalize(); void normalize();
float get_length(); float get_length();
vector_3 get_normal(); vector_3 get_normal();
void debug(const char title[]); void debug(const char title[]);
void apply_rotation(matrix_3x3 matrix); void apply_rotation(matrix_3x3 matrix);
}; };
struct matrix_3x3 struct matrix_3x3 {
{ float matrix[9];
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_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 create_look_at(vector_3 target);
static matrix_3x3 transpose(matrix_3x3 original); 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 // AUTO_BED_LEVELING_FEATURE
#endif // VECTOR_3_H #endif // VECTOR_3_H