adafruit_bno055 / utility / quaternion.h @ 0ecc7129
History | View | Annotate | Download (6.675 KB)
1 |
/*
|
---|---|
2 |
Inertial Measurement Unit Maths Library
|
3 |
Copyright (C) 2013-2014 Samuel Cowen
|
4 |
www.camelsoftware.com
|
5 |
|
6 |
This program is free software: you can redistribute it and/or modify
|
7 |
it under the terms of the GNU General Public License as published by
|
8 |
the Free Software Foundation, either version 3 of the License, or
|
9 |
(at your option) any later version.
|
10 |
|
11 |
This program is distributed in the hope that it will be useful,
|
12 |
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
13 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
14 |
GNU General Public License for more details.
|
15 |
|
16 |
You should have received a copy of the GNU General Public License
|
17 |
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
18 |
*/
|
19 |
|
20 |
|
21 |
#ifndef IMUMATH_QUATERNION_HPP
|
22 |
#define IMUMATH_QUATERNION_HPP
|
23 |
|
24 |
#include <stdlib.h> |
25 |
#include <string.h> |
26 |
#include <stdint.h> |
27 |
#include <math.h> |
28 |
|
29 |
#include "matrix.h" |
30 |
|
31 |
|
32 |
namespace imu |
33 |
{ |
34 |
|
35 |
class Quaternion |
36 |
{ |
37 |
public:
|
38 |
Quaternion() |
39 |
{ |
40 |
_w = 1.0; |
41 |
_x = _y = _z = 0.0; |
42 |
} |
43 |
|
44 |
Quaternion(double iw, double ix, double iy, double iz) |
45 |
{ |
46 |
_w = iw; |
47 |
_x = ix; |
48 |
_y = iy; |
49 |
_z = iz; |
50 |
} |
51 |
|
52 |
Quaternion(double w, Vector<3> vec) |
53 |
{ |
54 |
_w = w; |
55 |
_x = vec.x(); |
56 |
_y = vec.y(); |
57 |
_z = vec.z(); |
58 |
} |
59 |
|
60 |
double& w()
|
61 |
{ |
62 |
return _w;
|
63 |
} |
64 |
double& x()
|
65 |
{ |
66 |
return _x;
|
67 |
} |
68 |
double& y()
|
69 |
{ |
70 |
return _y;
|
71 |
} |
72 |
double& z()
|
73 |
{ |
74 |
return _z;
|
75 |
} |
76 |
|
77 |
double w() const |
78 |
{ |
79 |
return _w;
|
80 |
} |
81 |
double x() const |
82 |
{ |
83 |
return _x;
|
84 |
} |
85 |
double y() const |
86 |
{ |
87 |
return _y;
|
88 |
} |
89 |
double z() const |
90 |
{ |
91 |
return _z;
|
92 |
} |
93 |
|
94 |
double magnitude() const |
95 |
{ |
96 |
double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z);
|
97 |
return sqrt(res);
|
98 |
} |
99 |
|
100 |
void normalize()
|
101 |
{ |
102 |
double mag = magnitude();
|
103 |
*this = this->scale(1/mag);
|
104 |
} |
105 |
|
106 |
const Quaternion conjugate() const |
107 |
{ |
108 |
return Quaternion(_w, -_x, -_y, -_z);
|
109 |
} |
110 |
|
111 |
void fromAxisAngle(Vector<3> axis, double theta) |
112 |
{ |
113 |
_w = cos(theta/2);
|
114 |
//only need to calculate sine of half theta once
|
115 |
double sht = sin(theta/2); |
116 |
_x = axis.x() * sht; |
117 |
_y = axis.y() * sht; |
118 |
_z = axis.z() * sht; |
119 |
} |
120 |
|
121 |
void fromMatrix(const Matrix<3>& m) |
122 |
{ |
123 |
double tr = m.trace();
|
124 |
|
125 |
double S;
|
126 |
if (tr > 0) |
127 |
{ |
128 |
S = sqrt(tr+1.0) * 2; |
129 |
_w = 0.25 * S; |
130 |
_x = (m(2, 1) - m(1, 2)) / S; |
131 |
_y = (m(0, 2) - m(2, 0)) / S; |
132 |
_z = (m(1, 0) - m(0, 1)) / S; |
133 |
} |
134 |
else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) |
135 |
{ |
136 |
S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; |
137 |
_w = (m(2, 1) - m(1, 2)) / S; |
138 |
_x = 0.25 * S; |
139 |
_y = (m(0, 1) + m(1, 0)) / S; |
140 |
_z = (m(0, 2) + m(2, 0)) / S; |
141 |
} |
142 |
else if (m(1, 1) > m(2, 2)) |
143 |
{ |
144 |
S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; |
145 |
_w = (m(0, 2) - m(2, 0)) / S; |
146 |
_x = (m(0, 1) + m(1, 0)) / S; |
147 |
_y = 0.25 * S; |
148 |
_z = (m(1, 2) + m(2, 1)) / S; |
149 |
} |
150 |
else
|
151 |
{ |
152 |
S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; |
153 |
_w = (m(1, 0) - m(0, 1)) / S; |
154 |
_x = (m(0, 2) + m(2, 0)) / S; |
155 |
_y = (m(1, 2) + m(2, 1)) / S; |
156 |
_z = 0.25 * S; |
157 |
} |
158 |
} |
159 |
|
160 |
void toAxisAngle(Vector<3>& axis, float& angle) const |
161 |
{ |
162 |
float sqw = sqrt(1-_w*_w); |
163 |
if(sqw == 0) //it's a singularity and divide by zero, avoid |
164 |
return;
|
165 |
|
166 |
angle = 2 * acos(_w);
|
167 |
axis.x() = _x / sqw; |
168 |
axis.y() = _y / sqw; |
169 |
axis.z() = _z / sqw; |
170 |
} |
171 |
|
172 |
Matrix<3> toMatrix() const |
173 |
{ |
174 |
Matrix<3> ret;
|
175 |
ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z)); |
176 |
ret.cell(0, 1) = (2*_x*_y)-(2*_w*_z); |
177 |
ret.cell(0, 2) = (2*_x*_z)+(2*_w*_y); |
178 |
|
179 |
ret.cell(1, 0) = (2*_x*_y)+(2*_w*_z); |
180 |
ret.cell(1, 1) = 1-(2*(_x*_x))-(2*(_z*_z)); |
181 |
ret.cell(1, 2) = (2*(_y*_z))-(2*(_w*_x)); |
182 |
|
183 |
ret.cell(2, 0) = (2*(_x*_z))-(2*_w*_y); |
184 |
ret.cell(2, 1) = (2*_y*_z)+(2*_w*_x); |
185 |
ret.cell(2, 2) = 1-(2*(_x*_x))-(2*(_y*_y)); |
186 |
return ret;
|
187 |
} |
188 |
|
189 |
|
190 |
// Returns euler angles that represent the quaternion. Angles are
|
191 |
// returned in rotation order and right-handed about the specified
|
192 |
// axes:
|
193 |
//
|
194 |
// v[0] is applied 1st about z (ie, roll)
|
195 |
// v[1] is applied 2nd about y (ie, pitch)
|
196 |
// v[2] is applied 3rd about x (ie, yaw)
|
197 |
//
|
198 |
// Note that this means result.x() is not a rotation about x;
|
199 |
// similarly for result.z().
|
200 |
//
|
201 |
Vector<3> toEuler() const |
202 |
{ |
203 |
Vector<3> ret;
|
204 |
double sqw = _w*_w;
|
205 |
double sqx = _x*_x;
|
206 |
double sqy = _y*_y;
|
207 |
double sqz = _z*_z;
|
208 |
|
209 |
ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw)); |
210 |
ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw)); |
211 |
ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw)); |
212 |
|
213 |
return ret;
|
214 |
} |
215 |
|
216 |
Vector<3> toAngularVelocity(float dt) const |
217 |
{ |
218 |
Vector<3> ret;
|
219 |
Quaternion one(1.0, 0.0, 0.0, 0.0); |
220 |
Quaternion delta = one - *this; |
221 |
Quaternion r = (delta/dt); |
222 |
r = r * 2;
|
223 |
r = r * one; |
224 |
|
225 |
ret.x() = r.x(); |
226 |
ret.y() = r.y(); |
227 |
ret.z() = r.z(); |
228 |
return ret;
|
229 |
} |
230 |
|
231 |
Vector<3> rotateVector(Vector<2> v) const |
232 |
{ |
233 |
Vector<3> ret(v.x(), v.y(), 0.0); |
234 |
return rotateVector(ret);
|
235 |
} |
236 |
|
237 |
Vector<3> rotateVector(Vector<3> v) const |
238 |
{ |
239 |
Vector<3> qv(this->x(), this->y(), this->z());
|
240 |
Vector<3> t;
|
241 |
t = qv.cross(v) * 2.0; |
242 |
return v + (t * _w) + qv.cross(t);
|
243 |
} |
244 |
|
245 |
|
246 |
const Quaternion operator*(const Quaternion& q) const |
247 |
{ |
248 |
return Quaternion(
|
249 |
_w*q._w - _x*q._x - _y*q._y - _z*q._z, |
250 |
_w*q._x + _x*q._w + _y*q._z - _z*q._y, |
251 |
_w*q._y - _x*q._z + _y*q._w + _z*q._x, |
252 |
_w*q._z + _x*q._y - _y*q._x + _z*q._w |
253 |
); |
254 |
} |
255 |
|
256 |
const Quaternion operator+(const Quaternion& q) const |
257 |
{ |
258 |
return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z);
|
259 |
} |
260 |
|
261 |
const Quaternion operator-(const Quaternion& q) const |
262 |
{ |
263 |
return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z);
|
264 |
} |
265 |
|
266 |
const Quaternion operator/(double scalar) const |
267 |
{ |
268 |
return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar);
|
269 |
} |
270 |
|
271 |
const Quaternion operator*(double scalar) const |
272 |
{ |
273 |
return scale(scalar);
|
274 |
} |
275 |
|
276 |
const Quaternion scale(double scalar) const |
277 |
{ |
278 |
return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar);
|
279 |
} |
280 |
|
281 |
private:
|
282 |
double _w, _x, _y, _z;
|
283 |
}; |
284 |
|
285 |
} // namespace
|
286 |
|
287 |
#endif
|