diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 8078732418..5bfac98334 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -21,120 +21,6 @@ #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) -#define MATRIX_ROTATION_ROLL_90 Matrix3f(1, 0, 0, 0, 0, -1, 0, 1, 0) -#define MATRIX_ROTATION_ROLL_90_YAW_45 Matrix3f(HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, 0, 1, 0) -#define MATRIX_ROTATION_ROLL_90_YAW_90 Matrix3f(0, 0, 1, 1, 0, 0, 0, 1, 0) -#define MATRIX_ROTATION_ROLL_90_YAW_135 Matrix3f(-HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, 0, 1, 0) -#define MATRIX_ROTATION_ROLL_270 Matrix3f(1, 0, 0, 0, 0, 1, 0, -1, 0) -#define MATRIX_ROTATION_ROLL_270_YAW_45 Matrix3f(HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, HALF_SQRT_2, 0, -1, 0) -#define MATRIX_ROTATION_ROLL_270_YAW_90 Matrix3f(0, 0, -1, 1, 0, 0, 0, -1, 0) -#define MATRIX_ROTATION_ROLL_270_YAW_135 Matrix3f(-HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, 0, -1, 0) -#define MATRIX_ROTATION_PITCH_90 Matrix3f(0, 0, 1, 0, 1, 0, -1, 0, 0) -#define MATRIX_ROTATION_PITCH_270 Matrix3f(0, 0, -1, 0, 1, 0, 1, 0, 0) - -// fill in a matrix with a standard rotation -template -void Matrix3::rotation(enum Rotation r) -{ - switch (r) { - case ROTATION_NONE: - case ROTATION_MAX: - *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; - case ROTATION_ROLL_90: - *this = MATRIX_ROTATION_ROLL_90; - break; - case ROTATION_ROLL_90_YAW_45: - *this = MATRIX_ROTATION_ROLL_90_YAW_45; - break; - case ROTATION_ROLL_90_YAW_90: - *this = MATRIX_ROTATION_ROLL_90_YAW_90; - break; - case ROTATION_ROLL_90_YAW_135: - *this = MATRIX_ROTATION_ROLL_90_YAW_135; - break; - case ROTATION_ROLL_270: - *this = MATRIX_ROTATION_ROLL_270; - break; - case ROTATION_ROLL_270_YAW_45: - *this = MATRIX_ROTATION_ROLL_270_YAW_45; - break; - case ROTATION_ROLL_270_YAW_90: - *this = MATRIX_ROTATION_ROLL_270_YAW_90; - break; - case ROTATION_ROLL_270_YAW_135: - *this = MATRIX_ROTATION_ROLL_270_YAW_135; - break; - case ROTATION_PITCH_90: - *this = MATRIX_ROTATION_PITCH_90; - break; - case ROTATION_PITCH_270: - *this = MATRIX_ROTATION_PITCH_270; - break; - } -} - // create a rotation matrix given some euler angles // this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf template @@ -272,7 +158,6 @@ void Matrix3::zero(void) // only define for float -template void Matrix3::rotation(enum Rotation); template void Matrix3::zero(void); template void Matrix3::rotate(const Vector3 &g); template void Matrix3::rotateXY(const Vector3 &g); diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index 4343f2ce30..62066197dc 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -176,9 +176,6 @@ public: return a.is_nan() || b.is_nan() || c.is_nan(); } - // fill in the matrix with a standard rotation - void rotation(enum Rotation rotation); - // create a rotation matrix from Euler angles void from_euler(float roll, float pitch, float yaw);