From 289c64c0b9173adbf600c2ae8b7121a833d90edf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Mar 2012 13:44:36 +1100 Subject: [PATCH] Math: added vector3.rotate() and matrix3.rotation() methods these operate on a "enum Rotation" which defines a set of standard rotations. These are much faster than our previous method, plus use less memory --- libraries/AP_Math/AP_Math.h | 6 ++ libraries/AP_Math/matrix3.cpp | 98 +++++++++++++++++++++++++++++ libraries/AP_Math/matrix3.h | 3 + libraries/AP_Math/rotations.h | 39 ++++++++++++ libraries/AP_Math/vector3.cpp | 114 ++++++++++++++++++++++++++++++++++ libraries/AP_Math/vector3.h | 3 + 6 files changed, 263 insertions(+) create mode 100644 libraries/AP_Math/matrix3.cpp create mode 100644 libraries/AP_Math/rotations.h create mode 100644 libraries/AP_Math/vector3.cpp diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index c2a1b89af3..22de694877 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -1,9 +1,13 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +#ifndef AP_MATH_H +#define AP_MATH_H + // Assorted useful math operations for ArduPilot(Mega) #include #include +#include "rotations.h" #include "vector2.h" #include "vector3.h" #include "matrix3.h" @@ -38,3 +42,5 @@ void quaternion_to_rotation_matrix(const Quaternion &q, Matrix3f &m); // convert a vector in earth frame to a vector in body frame, // assuming body current rotation is given by a quaternion void quaternion_earth_to_body(const Quaternion &q, Vector3f &v); + +#endif diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp new file mode 100644 index 0000000000..d50419f332 --- /dev/null +++ b/libraries/AP_Math/matrix3.cpp @@ -0,0 +1,98 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * matrix3.cpp + * Copyright (C) Andrew Tridgell 2012 + * + * This file 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 file 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 along + * with this program. If not, see . + */ + +#include "AP_Math.h" + +#define HALF_SQRT_2 0.70710678118654757 + +#define MATRIX_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) +#define MATRIX_ROTATION_YAW_45 Matrix3f(HALF_SQRT_2, -HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, 1) +#define MATRIX_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) +#define MATRIX_ROTATION_YAW_135 Matrix3f(-HALF_SQRT_2, -HALF_SQRT_2, 0, HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, 1) +#define MATRIX_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) +#define MATRIX_ROTATION_YAW_225 Matrix3f(-HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, 1) +#define MATRIX_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) +#define MATRIX_ROTATION_YAW_315 Matrix3f(HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, 1) +#define MATRIX_ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1) +#define MATRIX_ROTATION_ROLL_180_YAW_45 Matrix3f(HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, -1) +#define MATRIX_ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1) +#define MATRIX_ROTATION_ROLL_180_YAW_135 Matrix3f(-HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, -1) +#define MATRIX_ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1) +#define MATRIX_ROTATION_ROLL_180_YAW_225 Matrix3f(-HALF_SQRT_2, -HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, -1) +#define MATRIX_ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1) +#define MATRIX_ROTATION_ROLL_180_YAW_315 Matrix3f(HALF_SQRT_2, -HALF_SQRT_2, 0, -HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, -1) + +// fill in a matrix with a standard rotation +template +void Matrix3::rotation(enum Rotation r) +{ + switch (r) { + case ROTATION_NONE: + *this = MATRIX_ROTATION_NONE; + break; + case ROTATION_YAW_45: + *this = MATRIX_ROTATION_YAW_45; + break; + case ROTATION_YAW_90: + *this = MATRIX_ROTATION_YAW_90; + break; + case ROTATION_YAW_135: + *this = MATRIX_ROTATION_YAW_135; + break; + case ROTATION_YAW_180: + *this = MATRIX_ROTATION_YAW_180; + break; + case ROTATION_YAW_225: + *this = MATRIX_ROTATION_YAW_225; + break; + case ROTATION_YAW_270: + *this = MATRIX_ROTATION_YAW_270; + break; + case ROTATION_YAW_315: + *this = MATRIX_ROTATION_YAW_315; + break; + case ROTATION_ROLL_180: + *this = MATRIX_ROTATION_ROLL_180; + break; + case ROTATION_ROLL_180_YAW_45: + *this = MATRIX_ROTATION_ROLL_180_YAW_45; + break; + case ROTATION_ROLL_180_YAW_90: + *this = MATRIX_ROTATION_ROLL_180_YAW_90; + break; + case ROTATION_ROLL_180_YAW_135: + *this = MATRIX_ROTATION_ROLL_180_YAW_135; + break; + case ROTATION_PITCH_180: + *this = MATRIX_ROTATION_PITCH_180; + break; + case ROTATION_ROLL_180_YAW_225: + *this = MATRIX_ROTATION_ROLL_180_YAW_225; + break; + case ROTATION_ROLL_180_YAW_270: + *this = MATRIX_ROTATION_ROLL_180_YAW_270; + break; + case ROTATION_ROLL_180_YAW_315: + *this = MATRIX_ROTATION_ROLL_180_YAW_315; + break; + } +} + +// only define for float +template void Matrix3::rotation(enum Rotation); diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index 0df2ae7bce..561c56a64e 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -128,6 +128,9 @@ public: bool is_nan(void) { return a.is_nan() || b.is_nan() || c.is_nan(); } + // fill in the matrix with a standard rotation + void rotation(enum Rotation rotation); + }; typedef Matrix3 Matrix3i; diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h new file mode 100644 index 0000000000..839119b9e8 --- /dev/null +++ b/libraries/AP_Math/rotations.h @@ -0,0 +1,39 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * rotations.h + * Copyright (C) Andrew Tridgell 2012 + * + * This file 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 file 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 along + * with this program. If not, see . + */ + +// these rotation values are stored to EEPROM, so be careful not to +// change the numbering of any existing entry when adding a new entry. +enum Rotation { + ROTATION_NONE = 0, + ROTATION_YAW_45, + ROTATION_YAW_90, + ROTATION_YAW_135, + ROTATION_YAW_180, + ROTATION_YAW_225, + ROTATION_YAW_270, + ROTATION_YAW_315, + ROTATION_ROLL_180, + ROTATION_ROLL_180_YAW_45, + ROTATION_ROLL_180_YAW_90, + ROTATION_ROLL_180_YAW_135, + ROTATION_PITCH_180, + ROTATION_ROLL_180_YAW_225, + ROTATION_ROLL_180_YAW_270, + ROTATION_ROLL_180_YAW_315 +}; diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp new file mode 100644 index 0000000000..9e0be0630c --- /dev/null +++ b/libraries/AP_Math/vector3.cpp @@ -0,0 +1,114 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * vector3.cpp + * Copyright (C) Andrew Tridgell 2012 + * + * This file 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 file 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 along + * with this program. If not, see . + */ + +#include "AP_Math.h" + +#define HALF_SQRT_2 0.70710678118654757 + +// rotate a vector by a standard rotation, attempting +// to use the minimum number of floating point operations +template +void Vector3::rotate(enum Rotation rotation) +{ + T tmp; + switch (rotation) { + case ROTATION_NONE: + return; + case ROTATION_YAW_45: { + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_YAW_90: { + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_YAW_135: { + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_YAW_180: + x = -x; y = -y; + return; + case ROTATION_YAW_225: { + tmp = HALF_SQRT_2*(y - x); + y = -HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_YAW_270: { + tmp = x; x = y; y = -tmp; + return; + } + case ROTATION_YAW_315: { + tmp = HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(y - x); + x = tmp; + return; + } + case ROTATION_ROLL_180: { + y = -y; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_45: { + tmp = HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_90: { + tmp = x; x = y; y = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_135: { + tmp = HALF_SQRT_2*(y - x); + y = HALF_SQRT_2*(y + x); + x = tmp; z = -z; + return; + } + case ROTATION_PITCH_180: { + x = -x; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_225: { + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(y - x); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_270: { + tmp = x; x = -y; y = -tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_315: { + tmp = HALF_SQRT_2*(x - y); + y = -HALF_SQRT_2*(x + y); + x = tmp; z = -z; + return; + } + } +} + +// only define for signed numbers +template void Vector3::rotate(enum Rotation); +template void Vector3::rotate(enum Rotation); +template void Vector3::rotate(enum Rotation); diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 27dbbcb57e..acabf1776f 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -185,6 +185,9 @@ public: bool is_inf(void) { return isinf(x) || isinf(y) || isinf(z); } + // rotate by a standard rotation + void rotate(enum Rotation rotation); + }; typedef Vector3 Vector3i;