Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2012-03-10 13:44:36 +11:00
parent 62c2aa4924
commit 289c64c0b9
6 changed files with 263 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Math/AP_Math.h
View File

@ -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 <AP_Common.h>
#include <stdint.h>
#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

98
libraries/AP_Math/matrix3.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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 <typename T>
void Matrix3<T>::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<float>::rotation(enum Rotation);

3
libraries/AP_Math/matrix3.h
View File

@ -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<int16_t> Matrix3i;

39
libraries/AP_Math/rotations.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
// 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
};

114
libraries/AP_Math/vector3.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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 <typename T>
void Vector3<T>::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<float>::rotate(enum Rotation);
template void Vector3<int16_t>::rotate(enum Rotation);
template void Vector3<int32_t>::rotate(enum Rotation);

3
libraries/AP_Math/vector3.h
View File

@ -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<int16_t> Vector3i;