adafruit_bno055 / utility / quaternion.h @ 0ecc7129
History | View | Annotate | Download (6.675 KB)
1 | 4bc1c0c1 | Kevin Townsend | /*
|
---|---|---|---|
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 | 4a94251b | Gé Vissers | #include "matrix.h" |
30 | 4bc1c0c1 | Kevin Townsend | |
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 | 0695bf91 | Paul Du Bois (laptop) | 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 | 4bc1c0c1 | Kevin Townsend | { |
96 | double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z);
|
||
97 | return sqrt(res);
|
||
98 | } |
||
99 | |||
100 | void normalize()
|
||
101 | { |
||
102 | 0695bf91 | Paul Du Bois (laptop) | double mag = magnitude();
|
103 | 4bc1c0c1 | Kevin Townsend | *this = this->scale(1/mag);
|
104 | } |
||
105 | |||
106 | 0ecc7129 | Gé Vissers | const Quaternion conjugate() const |
107 | 4bc1c0c1 | Kevin Townsend | { |
108 | 0ecc7129 | Gé Vissers | return Quaternion(_w, -_x, -_y, -_z);
|
109 | 4bc1c0c1 | Kevin Townsend | } |
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 | e8e79779 | Gé Vissers | void fromMatrix(const Matrix<3>& m) |
122 | 4bc1c0c1 | Kevin Townsend | { |
123 | e8e79779 | Gé Vissers | double tr = m.trace();
|
124 | 4bc1c0c1 | Kevin Townsend | |
125 | e8e79779 | Gé Vissers | double S;
|
126 | 4bc1c0c1 | Kevin Townsend | 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 | e8e79779 | Gé Vissers | else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) |
135 | 4bc1c0c1 | Kevin Townsend | { |
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 | e8e79779 | Gé Vissers | else if (m(1, 1) > m(2, 2)) |
143 | 4bc1c0c1 | Kevin Townsend | { |
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 | 0695bf91 | Paul Du Bois (laptop) | void toAxisAngle(Vector<3>& axis, float& angle) const |
161 | 4bc1c0c1 | Kevin Townsend | { |
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 | 0695bf91 | Paul Du Bois (laptop) | Matrix<3> toMatrix() const |
173 | 4bc1c0c1 | Kevin Townsend | { |
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 | 0695bf91 | Paul Du Bois (laptop) | // 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 | 4bc1c0c1 | Kevin Townsend | { |
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 | 0695bf91 | Paul Du Bois (laptop) | Vector<3> toAngularVelocity(float dt) const |
217 | 4bc1c0c1 | Kevin Townsend | { |
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 | 0695bf91 | Paul Du Bois (laptop) | Vector<3> rotateVector(Vector<2> v) const |
232 | 4bc1c0c1 | Kevin Townsend | { |
233 | Vector<3> ret(v.x(), v.y(), 0.0); |
||
234 | return rotateVector(ret);
|
||
235 | } |
||
236 | |||
237 | 0695bf91 | Paul Du Bois (laptop) | Vector<3> rotateVector(Vector<3> v) const |
238 | 4bc1c0c1 | Kevin Townsend | { |
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 | 0ecc7129 | Gé Vissers | const Quaternion operator*(const Quaternion& q) const |
247 | 4bc1c0c1 | Kevin Townsend | { |
248 | 0ecc7129 | Gé Vissers | 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 | 4bc1c0c1 | Kevin Townsend | } |
255 | |||
256 | 0ecc7129 | Gé Vissers | const Quaternion operator+(const Quaternion& q) const |
257 | 4bc1c0c1 | Kevin Townsend | { |
258 | 0ecc7129 | Gé Vissers | return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z);
|
259 | 4bc1c0c1 | Kevin Townsend | } |
260 | |||
261 | 0ecc7129 | Gé Vissers | const Quaternion operator-(const Quaternion& q) const |
262 | 4bc1c0c1 | Kevin Townsend | { |
263 | 0ecc7129 | Gé Vissers | return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z);
|
264 | 4bc1c0c1 | Kevin Townsend | } |
265 | |||
266 | 0ecc7129 | Gé Vissers | const Quaternion operator/(double scalar) const |
267 | 4bc1c0c1 | Kevin Townsend | { |
268 | 0ecc7129 | Gé Vissers | return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar);
|
269 | 4bc1c0c1 | Kevin Townsend | } |
270 | |||
271 | 0ecc7129 | Gé Vissers | const Quaternion operator*(double scalar) const |
272 | 4bc1c0c1 | Kevin Townsend | { |
273 | 0ecc7129 | Gé Vissers | return scale(scalar);
|
274 | 4bc1c0c1 | Kevin Townsend | } |
275 | |||
276 | 0ecc7129 | Gé Vissers | const Quaternion scale(double scalar) const |
277 | 0695bf91 | Paul Du Bois (laptop) | { |
278 | 0ecc7129 | Gé Vissers | return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar);
|
279 | 4bc1c0c1 | Kevin Townsend | } |
280 | |||
281 | private:
|
||
282 | double _w, _x, _y, _z;
|
||
283 | }; |
||
284 | |||
285 | 0ecc7129 | Gé Vissers | } // namespace
|
286 | 4bc1c0c1 | Kevin Townsend | |
287 | #endif |