2016-03-25 06:19:46 +00:00
|
|
|
/**
|
2016-03-24 18:01:20 +00:00
|
|
|
* Marlin 3D Printer Firmware
|
2020-02-03 14:00:57 +00:00
|
|
|
* Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
2016-03-24 18:01:20 +00:00
|
|
|
*
|
|
|
|
* Based on Sprinter and grbl.
|
2019-06-28 04:57:50 +00:00
|
|
|
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
|
2016-03-24 18:01:20 +00:00
|
|
|
*
|
|
|
|
* This program is free software: you can redistribute it and/or modify
|
|
|
|
* it under the terms of the GNU General Public License as published by
|
|
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
|
|
* (at your option) any later version.
|
|
|
|
*
|
|
|
|
* This program is distributed in the hope that it will be useful,
|
|
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
* GNU General Public License for more details.
|
|
|
|
*
|
|
|
|
* You should have received a copy of the GNU General Public License
|
2020-07-23 03:20:14 +00:00
|
|
|
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
2016-03-24 18:01:20 +00:00
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
2016-03-25 06:19:46 +00:00
|
|
|
/**
|
2017-11-18 08:08:03 +00:00
|
|
|
* vector_3.cpp - Vector library for bed leveling
|
|
|
|
* Copyright (c) 2012 Lars Brubaker. All right reserved.
|
|
|
|
*
|
|
|
|
* This library is free software; you can redistribute it and/or
|
|
|
|
* modify it under the terms of the GNU Lesser General Public
|
|
|
|
* License as published by the Free Software Foundation; either
|
|
|
|
* version 2.1 of the License, or (at your option) any later version.
|
|
|
|
*
|
|
|
|
* This library is distributed in the hope that it will be useful,
|
|
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
|
|
* Lesser General Public License for more details.
|
|
|
|
*
|
|
|
|
* You should have received a copy of the GNU Lesser General Public
|
|
|
|
* License along with this library; if not, write to the Free Software
|
|
|
|
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
|
|
|
*/
|
2017-09-06 11:28:32 +00:00
|
|
|
|
|
|
|
#include "../inc/MarlinConfig.h"
|
2013-09-29 16:20:06 +00:00
|
|
|
|
2018-05-01 12:05:18 +00:00
|
|
|
#if ABL_PLANAR || ENABLED(AUTO_BED_LEVELING_UBL)
|
2017-09-06 11:28:32 +00:00
|
|
|
|
2013-09-29 16:20:06 +00:00
|
|
|
#include "vector_3.h"
|
|
|
|
|
2017-09-06 11:28:32 +00:00
|
|
|
#include <math.h>
|
|
|
|
|
2019-09-29 09:25:39 +00:00
|
|
|
/**
|
|
|
|
* vector_3
|
|
|
|
*/
|
2013-09-29 16:20:06 +00:00
|
|
|
|
2018-09-30 04:04:40 +00:00
|
|
|
vector_3 vector_3::cross(const vector_3 &left, const vector_3 &right) {
|
2021-06-05 07:18:47 +00:00
|
|
|
return vector_3(left.y * right.z - left.z * right.y, // YZ cross
|
|
|
|
left.z * right.x - left.x * right.z, // ZX cross
|
|
|
|
left.x * right.y - left.y * right.x); // XY cross
|
2013-09-29 16:20:06 +00:00
|
|
|
}
|
|
|
|
|
2018-09-30 04:04:40 +00:00
|
|
|
vector_3 vector_3::get_normal() const {
|
2019-09-29 09:25:39 +00:00
|
|
|
vector_3 normalized = *this;
|
2015-10-03 06:08:58 +00:00
|
|
|
normalized.normalize();
|
|
|
|
return normalized;
|
2013-09-29 16:20:06 +00:00
|
|
|
}
|
|
|
|
|
2021-06-05 07:18:47 +00:00
|
|
|
float vector_3::magnitude() const { return SQRT(sq(x) + sq(y) + sq(z)); }
|
|
|
|
|
|
|
|
void vector_3::normalize() { *this *= RSQRT(sq(x) + sq(y) + sq(z)); }
|
2013-09-29 16:20:06 +00:00
|
|
|
|
2019-09-29 09:25:39 +00:00
|
|
|
// Apply a rotation to the matrix
|
2018-09-30 04:04:40 +00:00
|
|
|
void vector_3::apply_rotation(const matrix_3x3 &matrix) {
|
2019-09-29 09:25:39 +00:00
|
|
|
const float _x = x, _y = y, _z = z;
|
2021-06-05 07:18:47 +00:00
|
|
|
*this = { matrix.vectors[0].x * _x + matrix.vectors[1].x * _y + matrix.vectors[2].x * _z,
|
|
|
|
matrix.vectors[0].y * _x + matrix.vectors[1].y * _y + matrix.vectors[2].y * _z,
|
|
|
|
matrix.vectors[0].z * _x + matrix.vectors[1].z * _y + matrix.vectors[2].z * _z };
|
2013-09-29 16:20:06 +00:00
|
|
|
}
|
|
|
|
|
2018-10-01 04:44:33 +00:00
|
|
|
void vector_3::debug(PGM_P const title) {
|
2021-03-01 01:43:46 +00:00
|
|
|
SERIAL_ECHOPGM_P(title);
|
2019-11-29 10:45:07 +00:00
|
|
|
SERIAL_ECHOPAIR_F_P(SP_X_STR, x, 6);
|
|
|
|
SERIAL_ECHOPAIR_F_P(SP_Y_STR, y, 6);
|
|
|
|
SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, z, 6);
|
2013-09-29 16:20:06 +00:00
|
|
|
}
|
|
|
|
|
2019-09-29 09:25:39 +00:00
|
|
|
/**
|
|
|
|
* matrix_3x3
|
|
|
|
*/
|
|
|
|
|
2021-03-31 04:16:29 +00:00
|
|
|
void matrix_3x3::apply_rotation_xyz(float &_x, float &_y, float &_z) {
|
|
|
|
vector_3 vec = vector_3(_x, _y, _z); vec.apply_rotation(*this);
|
2019-09-29 09:25:39 +00:00
|
|
|
_x = vec.x; _y = vec.y; _z = vec.z;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Reset to identity. No rotate or translate.
|
|
|
|
void matrix_3x3::set_to_identity() {
|
2020-03-14 04:18:16 +00:00
|
|
|
LOOP_L_N(i, 3)
|
|
|
|
LOOP_L_N(j, 3)
|
2019-09-29 09:25:39 +00:00
|
|
|
vectors[i][j] = float(i == j);
|
2013-09-29 16:20:06 +00:00
|
|
|
}
|
|
|
|
|
2019-09-29 09:25:39 +00:00
|
|
|
// Create a matrix from 3 vector_3 inputs
|
2018-09-30 04:04:40 +00:00
|
|
|
matrix_3x3 matrix_3x3::create_from_rows(const vector_3 &row_0, const vector_3 &row_1, const vector_3 &row_2) {
|
2017-05-02 21:46:11 +00:00
|
|
|
//row_0.debug(PSTR("row_0"));
|
|
|
|
//row_1.debug(PSTR("row_1"));
|
|
|
|
//row_2.debug(PSTR("row_2"));
|
2015-10-03 06:08:58 +00:00
|
|
|
matrix_3x3 new_matrix;
|
2021-06-05 07:18:47 +00:00
|
|
|
new_matrix.vectors[0].x = row_0.x; new_matrix.vectors[1].y = row_0.y; new_matrix.vectors[2].z = row_0.z;
|
|
|
|
new_matrix.vectors[3].x = row_1.x; new_matrix.vectors[4].y = row_1.y; new_matrix.vectors[5].z = row_1.z;
|
|
|
|
new_matrix.vectors[6].x = row_2.x; new_matrix.vectors[7].y = row_2.y; new_matrix.vectors[8].z = row_2.z;
|
2017-05-02 21:46:11 +00:00
|
|
|
//new_matrix.debug(PSTR("new_matrix"));
|
2015-10-03 06:08:58 +00:00
|
|
|
return new_matrix;
|
2013-09-29 16:20:06 +00:00
|
|
|
}
|
|
|
|
|
2019-09-29 09:25:39 +00:00
|
|
|
// Create a matrix rotated to point towards a target
|
2018-09-30 04:04:40 +00:00
|
|
|
matrix_3x3 matrix_3x3::create_look_at(const vector_3 &target) {
|
2019-09-29 09:25:39 +00:00
|
|
|
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();
|
2013-12-06 20:32:21 +00:00
|
|
|
|
2017-05-02 21:46:11 +00:00
|
|
|
// x_row.debug(PSTR("x_row"));
|
|
|
|
// y_row.debug(PSTR("y_row"));
|
|
|
|
// z_row.debug(PSTR("z_row"));
|
2013-12-06 20:32:21 +00:00
|
|
|
|
2015-03-26 05:13:59 +00:00
|
|
|
// create the matrix already correctly transposed
|
|
|
|
matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
|
2013-12-06 20:32:21 +00:00
|
|
|
|
2017-05-02 21:46:11 +00:00
|
|
|
// rot.debug(PSTR("rot"));
|
2015-03-26 05:13:59 +00:00
|
|
|
return rot;
|
2013-09-29 16:20:06 +00:00
|
|
|
}
|
|
|
|
|
2019-09-29 09:25:39 +00:00
|
|
|
// Get a transposed copy of the matrix
|
2018-09-30 04:04:40 +00:00
|
|
|
matrix_3x3 matrix_3x3::transpose(const matrix_3x3 &original) {
|
2013-12-06 20:32:21 +00:00
|
|
|
matrix_3x3 new_matrix;
|
2020-03-14 04:18:16 +00:00
|
|
|
LOOP_L_N(i, 3)
|
|
|
|
LOOP_L_N(j, 3)
|
2019-09-29 09:25:39 +00:00
|
|
|
new_matrix.vectors[i][j] = original.vectors[j][i];
|
2013-12-06 20:32:21 +00:00
|
|
|
return new_matrix;
|
2013-09-29 16:20:06 +00:00
|
|
|
}
|
|
|
|
|
2018-10-01 04:44:33 +00:00
|
|
|
void matrix_3x3::debug(PGM_P const title) {
|
2021-03-01 01:43:46 +00:00
|
|
|
if (title) SERIAL_ECHOLNPGM_P(title);
|
2020-03-14 04:18:16 +00:00
|
|
|
LOOP_L_N(i, 3) {
|
|
|
|
LOOP_L_N(j, 3) {
|
2019-09-29 09:25:39 +00:00
|
|
|
if (vectors[i][j] >= 0.0) SERIAL_CHAR('+');
|
|
|
|
SERIAL_ECHO_F(vectors[i][j], 6);
|
2018-11-29 22:58:58 +00:00
|
|
|
SERIAL_CHAR(' ');
|
2015-03-07 06:14:34 +00:00
|
|
|
}
|
2017-06-09 15:51:23 +00:00
|
|
|
SERIAL_EOL();
|
2015-03-07 06:14:34 +00:00
|
|
|
}
|
2013-09-29 16:20:06 +00:00
|
|
|
}
|
|
|
|
|
2019-02-25 02:29:03 +00:00
|
|
|
#endif // HAS_ABL_OR_UBL
|