adafruit_bno055 / utility / quaternion.h @ 67f3cff5
History | View | Annotate | Download (6.572 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 | #include "vector.h" |
||
30 | |||
31 | |||
32 | namespace imu |
||
33 | { |
||
34 | |||
35 | |||
36 | |||
37 | class Quaternion |
||
38 | { |
||
39 | public:
|
||
40 | Quaternion() |
||
41 | { |
||
42 | _w = 1.0; |
||
43 | _x = _y = _z = 0.0; |
||
44 | } |
||
45 | |||
46 | Quaternion(double iw, double ix, double iy, double iz) |
||
47 | { |
||
48 | _w = iw; |
||
49 | _x = ix; |
||
50 | _y = iy; |
||
51 | _z = iz; |
||
52 | } |
||
53 | |||
54 | Quaternion(double w, Vector<3> vec) |
||
55 | { |
||
56 | _w = w; |
||
57 | _x = vec.x(); |
||
58 | _y = vec.y(); |
||
59 | _z = vec.z(); |
||
60 | } |
||
61 | |||
62 | double& w()
|
||
63 | { |
||
64 | return _w;
|
||
65 | } |
||
66 | double& x()
|
||
67 | { |
||
68 | return _x;
|
||
69 | } |
||
70 | double& y()
|
||
71 | { |
||
72 | return _y;
|
||
73 | } |
||
74 | double& z()
|
||
75 | { |
||
76 | return _z;
|
||
77 | } |
||
78 | |||
79 | double magnitude()
|
||
80 | { |
||
81 | double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z);
|
||
82 | return sqrt(res);
|
||
83 | } |
||
84 | |||
85 | void normalize()
|
||
86 | { |
||
87 | double mag = magnitude();
|
||
88 | *this = this->scale(1/mag);
|
||
89 | } |
||
90 | |||
91 | |||
92 | Quaternion conjugate() |
||
93 | { |
||
94 | Quaternion q; |
||
95 | q.w() = _w; |
||
96 | q.x() = -_x; |
||
97 | q.y() = -_y; |
||
98 | q.z() = -_z; |
||
99 | return q;
|
||
100 | } |
||
101 | |||
102 | void fromAxisAngle(Vector<3> axis, double theta) |
||
103 | { |
||
104 | _w = cos(theta/2);
|
||
105 | //only need to calculate sine of half theta once
|
||
106 | double sht = sin(theta/2); |
||
107 | _x = axis.x() * sht; |
||
108 | _y = axis.y() * sht; |
||
109 | _z = axis.z() * sht; |
||
110 | } |
||
111 | |||
112 | void fromMatrix(Matrix<3> m) |
||
113 | { |
||
114 | float tr = m(0, 0) + m(1, 1) + m(2, 2); |
||
115 | |||
116 | float S = 0.0; |
||
117 | if (tr > 0) |
||
118 | { |
||
119 | S = sqrt(tr+1.0) * 2; |
||
120 | _w = 0.25 * S; |
||
121 | _x = (m(2, 1) - m(1, 2)) / S; |
||
122 | _y = (m(0, 2) - m(2, 0)) / S; |
||
123 | _z = (m(1, 0) - m(0, 1)) / S; |
||
124 | } |
||
125 | else if ((m(0, 0) < m(1, 1))&(m(0, 0) < m(2, 2))) |
||
126 | { |
||
127 | S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; |
||
128 | _w = (m(2, 1) - m(1, 2)) / S; |
||
129 | _x = 0.25 * S; |
||
130 | _y = (m(0, 1) + m(1, 0)) / S; |
||
131 | _z = (m(0, 2) + m(2, 0)) / S; |
||
132 | } |
||
133 | else if (m(1, 1) < m(2, 2)) |
||
134 | { |
||
135 | S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; |
||
136 | _w = (m(0, 2) - m(2, 0)) / S; |
||
137 | _x = (m(0, 1) + m(1, 0)) / S; |
||
138 | _y = 0.25 * S; |
||
139 | _z = (m(1, 2) + m(2, 1)) / S; |
||
140 | } |
||
141 | else
|
||
142 | { |
||
143 | S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; |
||
144 | _w = (m(1, 0) - m(0, 1)) / S; |
||
145 | _x = (m(0, 2) + m(2, 0)) / S; |
||
146 | _y = (m(1, 2) + m(2, 1)) / S; |
||
147 | _z = 0.25 * S; |
||
148 | } |
||
149 | } |
||
150 | |||
151 | void toAxisAngle(Vector<3>& axis, float& angle) |
||
152 | { |
||
153 | float sqw = sqrt(1-_w*_w); |
||
154 | if(sqw == 0) //it's a singularity and divide by zero, avoid |
||
155 | return;
|
||
156 | |||
157 | angle = 2 * acos(_w);
|
||
158 | axis.x() = _x / sqw; |
||
159 | axis.y() = _y / sqw; |
||
160 | axis.z() = _z / sqw; |
||
161 | } |
||
162 | |||
163 | Matrix<3> toMatrix()
|
||
164 | { |
||
165 | Matrix<3> ret;
|
||
166 | ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z)); |
||
167 | ret.cell(0, 1) = (2*_x*_y)-(2*_w*_z); |
||
168 | ret.cell(0, 2) = (2*_x*_z)+(2*_w*_y); |
||
169 | |||
170 | ret.cell(1, 0) = (2*_x*_y)+(2*_w*_z); |
||
171 | ret.cell(1, 1) = 1-(2*(_x*_x))-(2*(_z*_z)); |
||
172 | ret.cell(1, 2) = (2*(_y*_z))-(2*(_w*_x)); |
||
173 | |||
174 | ret.cell(2, 0) = (2*(_x*_z))-(2*_w*_y); |
||
175 | ret.cell(2, 1) = (2*_y*_z)+(2*_w*_x); |
||
176 | ret.cell(2, 2) = 1-(2*(_x*_x))-(2*(_y*_y)); |
||
177 | return ret;
|
||
178 | } |
||
179 | |||
180 | |||
181 | Vector<3> toEuler()
|
||
182 | { |
||
183 | Vector<3> ret;
|
||
184 | double sqw = _w*_w;
|
||
185 | double sqx = _x*_x;
|
||
186 | double sqy = _y*_y;
|
||
187 | double sqz = _z*_z;
|
||
188 | |||
189 | ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw)); |
||
190 | ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw)); |
||
191 | ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw)); |
||
192 | |||
193 | return ret;
|
||
194 | } |
||
195 | |||
196 | Vector<3> toAngularVelocity(float dt) |
||
197 | { |
||
198 | Vector<3> ret;
|
||
199 | Quaternion one(1.0, 0.0, 0.0, 0.0); |
||
200 | Quaternion delta = one - *this; |
||
201 | Quaternion r = (delta/dt); |
||
202 | r = r * 2;
|
||
203 | r = r * one; |
||
204 | |||
205 | ret.x() = r.x(); |
||
206 | ret.y() = r.y(); |
||
207 | ret.z() = r.z(); |
||
208 | return ret;
|
||
209 | } |
||
210 | |||
211 | Vector<3> rotateVector(Vector<2> v) |
||
212 | { |
||
213 | Vector<3> ret(v.x(), v.y(), 0.0); |
||
214 | return rotateVector(ret);
|
||
215 | } |
||
216 | |||
217 | Vector<3> rotateVector(Vector<3> v) |
||
218 | { |
||
219 | Vector<3> qv(this->x(), this->y(), this->z());
|
||
220 | Vector<3> t;
|
||
221 | t = qv.cross(v) * 2.0; |
||
222 | return v + (t * _w) + qv.cross(t);
|
||
223 | } |
||
224 | |||
225 | |||
226 | Quaternion operator * (Quaternion q) |
||
227 | { |
||
228 | Quaternion ret; |
||
229 | ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z)); |
||
230 | ret._x = ((_w*q._x) + (_x*q._w) + (_y*q._z) - (_z*q._y)); |
||
231 | ret._y = ((_w*q._y) - (_x*q._z) + (_y*q._w) + (_z*q._x)); |
||
232 | ret._z = ((_w*q._z) + (_x*q._y) - (_y*q._x) + (_z*q._w)); |
||
233 | return ret;
|
||
234 | } |
||
235 | |||
236 | Quaternion operator + (Quaternion q) |
||
237 | { |
||
238 | Quaternion ret; |
||
239 | ret._w = _w + q._w; |
||
240 | ret._x = _x + q._x; |
||
241 | ret._y = _y + q._y; |
||
242 | ret._z = _z + q._z; |
||
243 | return ret;
|
||
244 | } |
||
245 | |||
246 | Quaternion operator - (Quaternion q) |
||
247 | { |
||
248 | Quaternion ret; |
||
249 | ret._w = _w - q._w; |
||
250 | ret._x = _x - q._x; |
||
251 | ret._y = _y - q._y; |
||
252 | ret._z = _z - q._z; |
||
253 | return ret;
|
||
254 | } |
||
255 | |||
256 | Quaternion operator / (float scalar)
|
||
257 | { |
||
258 | Quaternion ret; |
||
259 | ret._w = this->_w/scalar; |
||
260 | ret._x = this->_x/scalar; |
||
261 | ret._y = this->_y/scalar; |
||
262 | ret._z = this->_z/scalar; |
||
263 | return ret;
|
||
264 | } |
||
265 | |||
266 | Quaternion operator * (float scalar)
|
||
267 | { |
||
268 | Quaternion ret; |
||
269 | ret._w = this->_w*scalar; |
||
270 | ret._x = this->_x*scalar; |
||
271 | ret._y = this->_y*scalar; |
||
272 | ret._z = this->_z*scalar; |
||
273 | return ret;
|
||
274 | } |
||
275 | |||
276 | Quaternion scale(double scalar)
|
||
277 | { |
||
278 | Quaternion ret; |
||
279 | ret._w = this->_w*scalar; |
||
280 | ret._x = this->_x*scalar; |
||
281 | ret._y = this->_y*scalar; |
||
282 | ret._z = this->_z*scalar; |
||
283 | return ret;
|
||
284 | } |
||
285 | |||
286 | private:
|
||
287 | double _w, _x, _y, _z;
|
||
288 | }; |
||
289 | |||
290 | |||
291 | }; |
||
292 | |||
293 | #endif |