zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
mission-4.1.18
Andrew Tridgell 13 years ago
parent
62c2aa4924
commit
289c64c0b9
6 changed files with 263 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      libraries/AP_Math/AP_Math.h
  2. 98
      libraries/AP_Math/matrix3.cpp
  3. 3
      libraries/AP_Math/matrix3.h
  4. 39
      libraries/AP_Math/rotations.h
  5. 114
      libraries/AP_Math/vector3.cpp
  6. 3
      libraries/AP_Math/vector3.h

6
libraries/AP_Math/AP_Math.h
Unescape View File

@ -1,9 +1,13 @@ @@ -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); @@ -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
Unescape View File

@ -0,0 +1,98 @@ @@ -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
Unescape View File

@ -128,6 +128,9 @@ public: @@ -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
Unescape View File

@ -0,0 +1,39 @@ @@ -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
Unescape View File

@ -0,0 +1,114 @@ @@ -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
Unescape View File

@ -185,6 +185,9 @@ public: @@ -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;

Write Preview
Loading…
Cancel
Save