melch/Marlin
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Adjust vector_3 code with const, some optimization

Browse Source
This commit is contained in:
Scott Lahteine 2017-03-23 19:39:24 -05:00
parent 8ee2612ebb
commit 8d54ffbf05
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
Marlin/vector_3.cpp
View File

@ -66,16 +66,16 @@ vector_3 vector_3::get_normal() {
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(); const float inv_length = 1.0 / get_length();
x /= length; x *= inv_length;
y /= length; y *= inv_length;
z /= length; z *= inv_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]; const 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]; 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]; 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;
@ -92,7 +92,7 @@ void vector_3::debug(const 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;
@ -144,9 +144,9 @@ 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; uint8_t count = 0;
for (int i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) { for (uint8_t 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(' ');