Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1702 - 1
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2
/*
3
 * matrix3.cpp
4
 * Copyright (C) Andrew Tridgell 2012
5
 *
6
 * This file is free software: you can redistribute it and/or modify it
7
 * under the terms of the GNU General Public License as published by the
8
 * Free Software Foundation, either version 3 of the License, or
9
 * (at your option) any later version.
10
 *
11
 * This file is distributed in the hope that it will be useful, but
12
 * WITHOUT ANY WARRANTY; without even the implied warranty of
13
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
14
 * See the GNU General Public License for more details.
15
 *
16
 * You should have received a copy of the GNU General Public License along
17
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
18
 */
19
 
20
#include "AP_Math.h"
21
 
22
#define HALF_SQRT_2 0.70710678118654757
23
 
24
#define MATRIX_ROTATION_NONE               Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
25
#define MATRIX_ROTATION_YAW_45             Matrix3f(HALF_SQRT_2, -HALF_SQRT_2, 0, HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, 1)
26
#define MATRIX_ROTATION_YAW_90             Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
27
#define MATRIX_ROTATION_YAW_135            Matrix3f(-HALF_SQRT_2, -HALF_SQRT_2, 0, HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, 1)
28
#define MATRIX_ROTATION_YAW_180            Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
29
#define MATRIX_ROTATION_YAW_225            Matrix3f(-HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, -HALF_SQRT_2, 0, 0, 0, 1)
30
#define MATRIX_ROTATION_YAW_270            Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
31
#define MATRIX_ROTATION_YAW_315            Matrix3f(HALF_SQRT_2, HALF_SQRT_2, 0, -HALF_SQRT_2, HALF_SQRT_2, 0, 0, 0, 1)
32
#define MATRIX_ROTATION_ROLL_180           Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)
33
#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)
34
#define MATRIX_ROTATION_ROLL_180_YAW_90    Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)
35
#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)
36
#define MATRIX_ROTATION_PITCH_180          Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)
37
#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)
38
#define MATRIX_ROTATION_ROLL_180_YAW_270   Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
39
#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)
40
 
41
// fill in a matrix with a standard rotation
42
template <typename T>
43
void Matrix3<T>::rotation(enum Rotation r)
44
{
45
    switch (r) {
46
    case ROTATION_NONE:
47
    case ROTATION_MAX:
48
            *this = MATRIX_ROTATION_NONE;
49
            break;
50
    case ROTATION_YAW_45:
51
            *this = MATRIX_ROTATION_YAW_45;
52
            break;
53
    case ROTATION_YAW_90:
54
            *this = MATRIX_ROTATION_YAW_90;
55
            break;
56
    case ROTATION_YAW_135:
57
            *this = MATRIX_ROTATION_YAW_135;
58
            break;
59
    case ROTATION_YAW_180:
60
            *this = MATRIX_ROTATION_YAW_180;
61
            break;
62
    case ROTATION_YAW_225:
63
            *this = MATRIX_ROTATION_YAW_225;
64
            break;
65
    case ROTATION_YAW_270:
66
            *this = MATRIX_ROTATION_YAW_270;
67
            break;
68
    case ROTATION_YAW_315:
69
            *this = MATRIX_ROTATION_YAW_315;
70
            break;
71
    case ROTATION_ROLL_180:
72
            *this = MATRIX_ROTATION_ROLL_180;
73
            break;
74
    case ROTATION_ROLL_180_YAW_45:
75
            *this = MATRIX_ROTATION_ROLL_180_YAW_45;
76
            break;
77
    case ROTATION_ROLL_180_YAW_90:
78
            *this = MATRIX_ROTATION_ROLL_180_YAW_90;
79
            break;
80
    case ROTATION_ROLL_180_YAW_135:
81
            *this = MATRIX_ROTATION_ROLL_180_YAW_135;
82
            break;
83
    case ROTATION_PITCH_180:
84
            *this = MATRIX_ROTATION_PITCH_180;
85
            break;
86
    case ROTATION_ROLL_180_YAW_225:
87
            *this = MATRIX_ROTATION_ROLL_180_YAW_225;
88
            break;
89
    case ROTATION_ROLL_180_YAW_270:
90
            *this = MATRIX_ROTATION_ROLL_180_YAW_270;
91
            break;
92
    case ROTATION_ROLL_180_YAW_315:
93
            *this = MATRIX_ROTATION_ROLL_180_YAW_315;
94
            break;
95
    }
96
}
97
 
98
// create a rotation matrix given some euler angles
99
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
100
template <typename T>
101
void Matrix3<T>::from_euler(float roll, float pitch, float yaw)
102
{
103
        float cp = cos(pitch);
104
        float sp = sin(pitch);
105
        float sr = sin(roll);
106
        float cr = cos(roll);
107
        float sy = sin(yaw);
108
        float cy = cos(yaw);
109
 
110
        a.x = cp * cy;
111
        a.y = (sr * sp * cy) - (cr * sy);
112
        a.z = (cr * sp * cy) + (sr * sy);
113
        b.x = cp * sy;
114
        b.y = (sr * sp * sy) + (cr * cy);
115
        b.z = (cr * sp * sy) - (sr * cy);
116
        c.x = -sp;
117
        c.y = sr * cp;
118
        c.z = cr * cp;
119
}
120
 
121
// calculate euler angles from a rotation matrix
122
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
123
template <typename T>
124
void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw)
125
{
126
        if (pitch != NULL) {
127
                *pitch = -safe_asin(c.x);
128
        }
129
        if (roll != NULL) {
130
                *roll = atan2(c.y, c.z);
131
        }
132
        if (yaw != NULL) {
133
                *yaw = atan2(b.x, a.x);
134
        }
135
}
136
 
137
// apply an additional rotation from a body frame gyro vector
138
// to a rotation matrix.
139
template <typename T>
140
void Matrix3<T>::rotate(const Vector3<T> &g)
141
{
142
        Matrix3f temp_matrix;
143
        temp_matrix.a.x = a.y * g.z - a.z * g.y;
144
        temp_matrix.a.y = a.z * g.x - a.x * g.z;
145
        temp_matrix.a.z = a.x * g.y - a.y * g.x;
146
        temp_matrix.b.x = b.y * g.z - b.z * g.y;
147
        temp_matrix.b.y = b.z * g.x - b.x * g.z;
148
        temp_matrix.b.z = b.x * g.y - b.y * g.x;
149
        temp_matrix.c.x = c.y * g.z - c.z * g.y;
150
        temp_matrix.c.y = c.z * g.x - c.x * g.z;
151
        temp_matrix.c.z = c.x * g.y - c.y * g.x;
152
 
153
        (*this) += temp_matrix;
154
}
155
 
156
 
157
// multiplication by a vector
158
template <typename T>
159
Vector3<T> Matrix3<T>::operator *(const Vector3<T> &v) const
160
{
161
    return Vector3<T>(a.x * v.x + a.y * v.y + a.z * v.z,
162
                      b.x * v.x + b.y * v.y + b.z * v.z,
163
                      c.x * v.x + c.y * v.y + c.z * v.z);
164
}
165
 
166
// multiplication of transpose by a vector
167
template <typename T>
168
Vector3<T> Matrix3<T>::mul_transpose(const Vector3<T> &v) const
169
{
170
    return Vector3<T>(a.x * v.x + b.x * v.y + c.x * v.z,
171
                      a.y * v.x + b.y * v.y + c.y * v.z,
172
                      a.z * v.x + b.z * v.y + c.z * v.z);
173
}
174
 
175
// multiplication by another Matrix3<T>
176
template <typename T>
177
Matrix3<T> Matrix3<T>::operator *(const Matrix3<T> &m) const
178
{
179
    Matrix3<T> temp (Vector3<T>(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x,
180
                                a.x * m.a.y + a.y * m.b.y + a.z * m.c.y,
181
                                a.x * m.a.z + a.y * m.b.z + a.z * m.c.z),
182
                     Vector3<T>(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x,
183
                                b.x * m.a.y + b.y * m.b.y + b.z * m.c.y,
184
                                b.x * m.a.z + b.y * m.b.z + b.z * m.c.z),
185
                     Vector3<T>(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x,
186
                                c.x * m.a.y + c.y * m.b.y + c.z * m.c.y,
187
                                c.x * m.a.z + c.y * m.b.z + c.z * m.c.z));
188
    return temp;
189
}
190
 
191
 
192
// only define for float
193
template void Matrix3<float>::rotation(enum Rotation);
194
template void Matrix3<float>::rotate(const Vector3<float> &g);
195
template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
196
template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);
197
template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) const;
198
template Vector3<float> Matrix3<float>::mul_transpose(const Vector3<float> &v) const;
199
template Matrix3<float> Matrix3<float>::operator *(const Matrix3<float> &m) const;