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

Add custom types for position (#15204)

Browse Source
This commit is contained in:
Scott Lahteine
2019-09-29 04:25:39 -05:00
committed by GitHub
parent 43d6e9fa43
commit 50e4545255
227 changed files with 3147 additions and 3264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
Marlin/src/libs/vector_3.cpp
View File

@@ -47,81 +47,74 @@
#include <math.h>
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 vector_3::cross(const vector_3 &left, const 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);
const xyz_float_t &lv = left, &rv = right;
return vector_3(lv.y * rv.z - lv.z * rv.y, // YZ cross
lv.z * rv.x - lv.x * rv.z, // ZX cross
lv.x * rv.y - lv.y * rv.x); // XY cross
}
vector_3 vector_3::operator+(const vector_3 &v) { return vector_3(x + v.x, y + v.y, z + v.z); }
vector_3 vector_3::operator-(const vector_3 &v) { return vector_3(x - v.x, y - v.y, z - v.z); }
vector_3 vector_3::operator* (const float &v) { return vector_3(x * v, y * v, z * v); }
vector_3& vector_3::operator*=(const float &v) { x *= v; y *= v; z *= v; return *this; }
vector_3 vector_3::get_normal() const {
vector_3 normalized = vector_3(x, y, z);
vector_3 normalized = *this;
normalized.normalize();
return normalized;
}
float vector_3::get_length() const { return SQRT(sq(x) + sq(y) + sq(z)); }
void vector_3::normalize() {
const float inv_length = RSQRT(sq(x) + sq(y) + sq(z));
x *= inv_length;
y *= inv_length;
z *= inv_length;
*this *= RSQRT(sq(x) + sq(y) + sq(z));
}
// Apply a rotation to the matrix
void vector_3::apply_rotation(const matrix_3x3 &matrix) {
const float _x = x, _y = y;
x = _x * matrix.matrix[3 * 0 + 0] + _y * matrix.matrix[3 * 1 + 0] + z * matrix.matrix[3 * 2 + 0];
y = _x * matrix.matrix[3 * 0 + 1] + _y * matrix.matrix[3 * 1 + 1] + z * matrix.matrix[3 * 2 + 1];
z = _x * matrix.matrix[3 * 0 + 2] + _y * matrix.matrix[3 * 1 + 2] + z * matrix.matrix[3 * 2 + 2];
const float _x = x, _y = y, _z = z;
*this = matrix.vectors[0] * _x + matrix.vectors[1] * _y + matrix.vectors[2] * _z;
}
void vector_3::debug(PGM_P const title) {
serialprintPGM(title);
SERIAL_ECHOPAIR_F(" x: ", x, 6);
SERIAL_ECHOPAIR_F(" y: ", y, 6);
SERIAL_ECHOLNPAIR_F(" z: ", z, 6);
SERIAL_ECHOPAIR_F(" X", x, 6);
SERIAL_ECHOPAIR_F(" Y", y, 6);
SERIAL_ECHOLNPAIR_F(" Z", z, 6);
}
void apply_rotation_xyz(const 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
*/
void apply_rotation_xyz(const matrix_3x3 &matrix, float &_x, float &_y, float &_z) {
vector_3 vec = vector_3(_x, _y, _z); vec.apply_rotation(matrix);
_x = vec.x; _y = vec.y; _z = vec.z;
}
// Reset to identity. No rotate or translate.
void matrix_3x3::set_to_identity() {
for (uint8_t i = 0; i < 3; i++)
for (uint8_t j = 0; j < 3; j++)
vectors[i][j] = float(i == j);
}
// Create a matrix from 3 vector_3 inputs
matrix_3x3 matrix_3x3::create_from_rows(const vector_3 &row_0, const vector_3 &row_1, const vector_3 &row_2) {
//row_0.debug(PSTR("row_0"));
//row_1.debug(PSTR("row_1"));
//row_2.debug(PSTR("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;
new_matrix.vectors[0] = row_0;
new_matrix.vectors[1] = row_1;
new_matrix.vectors[2] = row_2;
//new_matrix.debug(PSTR("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;
}
// Create a matrix rotated to point towards a target
matrix_3x3 matrix_3x3::create_look_at(const vector_3 &target) {
vector_3 z_row = target.get_normal(),
x_row = vector_3(1, 0, -target.x / target.z).get_normal(),
y_row = vector_3::cross(z_row, x_row).get_normal();
const vector_3 z_row = target.get_normal(),
x_row = vector_3(1, 0, -target.x / target.z).get_normal(),
y_row = vector_3::cross(z_row, x_row).get_normal();
// x_row.debug(PSTR("x_row"));
// y_row.debug(PSTR("y_row"));
@@ -134,11 +127,12 @@ matrix_3x3 matrix_3x3::create_look_at(const vector_3 &target) {
return rot;
}
// Get a transposed copy of the matrix
matrix_3x3 matrix_3x3::transpose(const 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[6] = original.matrix[2]; new_matrix.matrix[7] = original.matrix[5]; new_matrix.matrix[8] = original.matrix[8];
for (uint8_t i = 0; i < 3; i++)
for (uint8_t j = 0; j < 3; j++)
new_matrix.vectors[i][j] = original.vectors[j][i];
return new_matrix;
}
@@ -147,13 +141,11 @@ void matrix_3x3::debug(PGM_P const title) {
serialprintPGM(title);
SERIAL_EOL();
}
uint8_t count = 0;
for (uint8_t i = 0; i < 3; i++) {
for (uint8_t j = 0; j < 3; j++) {
if (matrix[count] >= 0.0) SERIAL_CHAR('+');
SERIAL_ECHO_F(matrix[count], 6);
if (vectors[i][j] >= 0.0) SERIAL_CHAR('+');
SERIAL_ECHO_F(vectors[i][j], 6);
SERIAL_CHAR(' ');
count++;
}
SERIAL_EOL();
}