Line data Source code
1 : /*
2 : * quaternion.cpp
3 : * Copyright (C) Andrew Tridgell 2012
4 : *
5 : * This file is free software: you can redistribute it and/or modify it
6 : * under the terms of the GNU General Public License as published by the
7 : * Free Software Foundation, either version 3 of the License, or
8 : * (at your option) any later version.
9 : *
10 : * This file is distributed in the hope that it will be useful, but
11 : * WITHOUT ANY WARRANTY; without even the implied warranty of
12 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 : * See the GNU General Public License for more details.
14 : *
15 : * You should have received a copy of the GNU General Public License along
16 : * with this program. If not, see <http://www.gnu.org/licenses/>.
17 : */
18 :
19 : #pragma GCC optimize("O2")
20 :
21 : #include "quaternion.h"
22 : #include "AP_Math.h"
23 : #include <AP_InternalError/AP_InternalError.h>
24 : #include <AP_CustomRotations/AP_CustomRotations.h>
25 : #include <AP_Vehicle/AP_Vehicle_Type.h>
26 :
27 : #define HALF_SQRT_2_PlUS_SQRT_2 0.92387953251128673848313610506011 // sqrt(2 + sqrt(2)) / 2
28 : #define HALF_SQRT_2_MINUS_SQTR_2 0.38268343236508972626808144923416 // sqrt(2 - sqrt(2)) / 2
29 : #define HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO 0.65328148243818828788676000840496 // sqrt((2 + sqrt(2))/2) / 2
30 : #define HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO 0.27059805007309845059637609665515 // sqrt((2 - sqrt(2))/2) / 2
31 :
32 : // return the rotation matrix equivalent for this quaternion
33 : template <typename T>
34 189498012 : void QuaternionT<T>::rotation_matrix(Matrix3d &m) const
35 : {
36 189498012 : const T q3q3 = q3 * q3;
37 189498012 : const T q3q4 = q3 * q4;
38 189498012 : const T q2q2 = q2 * q2;
39 189498012 : const T q2q3 = q2 * q3;
40 189498012 : const T q2q4 = q2 * q4;
41 189498012 : const T q1q2 = q1 * q2;
42 189498012 : const T q1q3 = q1 * q3;
43 189498012 : const T q1q4 = q1 * q4;
44 189498012 : const T q4q4 = q4 * q4;
45 :
46 189498012 : m.a.x = 1.0f-2.0f*(q3q3 + q4q4);
47 189498012 : m.a.y = 2.0f*(q2q3 - q1q4);
48 189498012 : m.a.z = 2.0f*(q2q4 + q1q3);
49 189498012 : m.b.x = 2.0f*(q2q3 + q1q4);
50 189498012 : m.b.y = 1.0f-2.0f*(q2q2 + q4q4);
51 189498012 : m.b.z = 2.0f*(q3q4 - q1q2);
52 189498012 : m.c.x = 2.0f*(q2q4 - q1q3);
53 189498012 : m.c.y = 2.0f*(q3q4 + q1q2);
54 189498012 : m.c.z = 1.0f-2.0f*(q2q2 + q3q3);
55 189498012 : }
56 :
57 : // populate the supplied rotation matrix equivalent from this quaternion
58 : template <typename T>
59 2023851299 : void QuaternionT<T>::rotation_matrix(Matrix3f &m) const
60 : {
61 2023851299 : const T q3q3 = q3 * q3;
62 2023851299 : const T q3q4 = q3 * q4;
63 2023851299 : const T q2q2 = q2 * q2;
64 2023851299 : const T q2q3 = q2 * q3;
65 2023851299 : const T q2q4 = q2 * q4;
66 2023851299 : const T q1q2 = q1 * q2;
67 2023851299 : const T q1q3 = q1 * q3;
68 2023851299 : const T q1q4 = q1 * q4;
69 2023851299 : const T q4q4 = q4 * q4;
70 :
71 2023851299 : m.a.x = 1.0f-2.0f*(q3q3 + q4q4);
72 2023851299 : m.a.y = 2.0f*(q2q3 - q1q4);
73 2023851299 : m.a.z = 2.0f*(q2q4 + q1q3);
74 2023851299 : m.b.x = 2.0f*(q2q3 + q1q4);
75 2023851299 : m.b.y = 1.0f-2.0f*(q2q2 + q4q4);
76 2023851299 : m.b.z = 2.0f*(q3q4 - q1q2);
77 2023851299 : m.c.x = 2.0f*(q2q4 - q1q3);
78 2023851299 : m.c.y = 2.0f*(q3q4 + q1q2);
79 2023851299 : m.c.z = 1.0f-2.0f*(q2q2 + q3q3);
80 2023851299 : }
81 :
82 : // make this quaternion equivalent to the supplied matrix
83 : // Thanks to Martin John Baker
84 : // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
85 : template <typename T>
86 765043377 : void QuaternionT<T>::from_rotation_matrix(const Matrix3<T> &m)
87 : {
88 : const T &m00 = m.a.x;
89 : const T &m11 = m.b.y;
90 : const T &m22 = m.c.z;
91 : const T &m10 = m.b.x;
92 : const T &m01 = m.a.y;
93 : const T &m20 = m.c.x;
94 : const T &m02 = m.a.z;
95 : const T &m21 = m.c.y;
96 : const T &m12 = m.b.z;
97 : T &qw = q1;
98 : T &qx = q2;
99 : T &qy = q3;
100 : T &qz = q4;
101 :
102 765043377 : const T tr = m00 + m11 + m22;
103 :
104 765043377 : if (tr > 0) {
105 631097352 : const T S = sqrtF(tr+1) * 2;
106 631097352 : qw = 0.25f * S;
107 631097352 : qx = (m21 - m12) / S;
108 631097352 : qy = (m02 - m20) / S;
109 631097352 : qz = (m10 - m01) / S;
110 133946025 : } else if ((m00 > m11) && (m00 > m22)) {
111 1617362 : const T S = sqrtF(1.0f + m00 - m11 - m22) * 2.0f;
112 1617362 : qw = (m21 - m12) / S;
113 1617362 : qx = 0.25f * S;
114 1617362 : qy = (m01 + m10) / S;
115 1617362 : qz = (m02 + m20) / S;
116 133946025 : } else if (m11 > m22) {
117 1376854 : const T S = sqrtF(1.0f + m11 - m00 - m22) * 2.0f;
118 1376854 : qw = (m02 - m20) / S;
119 1376854 : qx = (m01 + m10) / S;
120 1376854 : qy = 0.25f * S;
121 1376854 : qz = (m12 + m21) / S;
122 : } else {
123 130951809 : const T S = sqrtF(1.0f + m22 - m00 - m11) * 2.0f;
124 130951809 : qw = (m10 - m01) / S;
125 130951809 : qx = (m02 + m20) / S;
126 130951809 : qy = (m12 + m21) / S;
127 130951809 : qz = 0.25f * S;
128 : }
129 765043377 : }
130 :
131 : // create a quaternion from a given rotation
132 : template <typename T>
133 258 : void QuaternionT<T>::from_rotation(enum Rotation rotation)
134 : {
135 : // the constants below can be calculated using the following formula:
136 : // Matrix3f m_from_rot;
137 : // m_from_rot.from_rotation(rotation);
138 : // Quaternion q_from_m;
139 : // from_rotation_matrix(m_from_rot);
140 :
141 258 : switch (rotation) {
142 4 : case ROTATION_NONE:
143 4 : q1 = 1;
144 4 : q2 = q3 = q4 = 0;
145 4 : return;
146 :
147 4 : case ROTATION_YAW_45:
148 4 : q1 = HALF_SQRT_2_PlUS_SQRT_2;
149 4 : q2 = q3 = 0;
150 4 : q4 = HALF_SQRT_2_MINUS_SQTR_2;
151 4 : return;
152 :
153 4 : case ROTATION_YAW_90:
154 4 : q1 = HALF_SQRT_2;
155 4 : q2 = q3 = 0;
156 4 : q4 = HALF_SQRT_2;
157 4 : return;
158 :
159 4 : case ROTATION_YAW_135:
160 4 : q1 = HALF_SQRT_2_MINUS_SQTR_2;
161 4 : q2 = q3 = 0;
162 4 : q4 = HALF_SQRT_2_PlUS_SQRT_2;
163 4 : return;
164 :
165 4 : case ROTATION_YAW_180:
166 4 : q1 = q2 = q3 = 0;
167 4 : q4=1;
168 4 : return;
169 :
170 4 : case ROTATION_YAW_225:
171 4 : q1 = -HALF_SQRT_2_MINUS_SQTR_2;
172 4 : q2 = q3 = 0;
173 4 : q4 = HALF_SQRT_2_PlUS_SQRT_2;
174 4 : return;
175 :
176 4 : case ROTATION_YAW_270:
177 4 : q1 = HALF_SQRT_2;
178 4 : q2 = q3 = 0;
179 4 : q4 = -HALF_SQRT_2;
180 4 : return;
181 :
182 4 : case ROTATION_YAW_315:
183 4 : q1 = HALF_SQRT_2_PlUS_SQRT_2;
184 4 : q2 = q3 = 0;
185 4 : q4 = -HALF_SQRT_2_MINUS_SQTR_2;
186 4 : return;
187 :
188 4 : case ROTATION_ROLL_180:
189 4 : q1 = q3 = q4 = 0;
190 4 : q2 = 1;
191 4 : return;
192 :
193 4 : case ROTATION_ROLL_180_YAW_45:
194 4 : q1 = q4 = 0;
195 4 : q2 = HALF_SQRT_2_PlUS_SQRT_2;
196 4 : q3 = HALF_SQRT_2_MINUS_SQTR_2;
197 4 : return;
198 :
199 8 : case ROTATION_ROLL_180_YAW_90:
200 : case ROTATION_PITCH_180_YAW_270:
201 8 : q1 = q4 = 0;
202 8 : q2 = q3 = HALF_SQRT_2;
203 8 : return;
204 :
205 4 : case ROTATION_ROLL_180_YAW_135:
206 4 : q1 = q4 = 0;
207 4 : q2 = HALF_SQRT_2_MINUS_SQTR_2;
208 4 : q3 = HALF_SQRT_2_PlUS_SQRT_2;
209 4 : return;
210 :
211 4 : case ROTATION_PITCH_180:
212 4 : q1 = q2 = q4 = 0;
213 4 : q3 = 1;
214 4 : return;
215 :
216 4 : case ROTATION_ROLL_180_YAW_225:
217 4 : q1 = q4 = 0;
218 4 : q2 = -HALF_SQRT_2_MINUS_SQTR_2;
219 4 : q3 = HALF_SQRT_2_PlUS_SQRT_2;
220 4 : return;
221 :
222 8 : case ROTATION_ROLL_180_YAW_270:
223 : case ROTATION_PITCH_180_YAW_90:
224 8 : q1 = q4 = 0;
225 8 : q2 = -HALF_SQRT_2;
226 8 : q3 = HALF_SQRT_2;
227 8 : return;
228 :
229 4 : case ROTATION_ROLL_180_YAW_315:
230 4 : q1 = q4 = 0;
231 4 : q2 = HALF_SQRT_2_PlUS_SQRT_2;
232 4 : q3 = -HALF_SQRT_2_MINUS_SQTR_2;
233 4 : return;
234 :
235 4 : case ROTATION_ROLL_90:
236 4 : q1 = q2 = HALF_SQRT_2;
237 4 : q3 = q4 = 0;
238 4 : return;
239 :
240 4 : case ROTATION_ROLL_90_YAW_45:
241 4 : q1 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
242 4 : q2 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
243 4 : q3 = q4 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
244 4 : return;
245 :
246 4 : case ROTATION_ROLL_90_YAW_90:
247 4 : q1 = q2 = q3 = q4 = 0.5f;
248 4 : return;
249 :
250 4 : case ROTATION_ROLL_90_YAW_135:
251 4 : q1 = q2 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
252 4 : q3 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
253 4 : q4 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
254 4 : return;
255 :
256 4 : case ROTATION_ROLL_270:
257 4 : q1 = HALF_SQRT_2;
258 4 : q2 = -HALF_SQRT_2;
259 4 : q3 = q4 = 0;
260 4 : return;
261 :
262 4 : case ROTATION_ROLL_270_YAW_45:
263 4 : q1 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
264 4 : q2 = -HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
265 4 : q3 = -HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
266 4 : q4 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
267 4 : return;
268 :
269 4 : case ROTATION_ROLL_270_YAW_90:
270 4 : q1 = q4 = 0.5f;
271 4 : q2 = q3 = -0.5f;
272 4 : return;
273 :
274 4 : case ROTATION_ROLL_270_YAW_135:
275 4 : q1 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
276 4 : q2 = -HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
277 4 : q3 = -HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
278 4 : q4 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
279 4 : return;
280 :
281 4 : case ROTATION_PITCH_90:
282 4 : q1 = q3 = HALF_SQRT_2;
283 4 : q2 = q4 = 0;
284 4 : return;
285 :
286 4 : case ROTATION_PITCH_270:
287 4 : q1 = HALF_SQRT_2;
288 4 : q2 = q4 = 0;
289 4 : q3 = -HALF_SQRT_2;
290 4 : return;
291 :
292 4 : case ROTATION_ROLL_90_PITCH_90:
293 4 : q1 = q2 = q3 = -0.5f;
294 4 : q4 = 0.5f;
295 4 : return;
296 :
297 4 : case ROTATION_ROLL_180_PITCH_90:
298 4 : q1 = q3 = 0;
299 4 : q2 = -HALF_SQRT_2;
300 4 : q4 = HALF_SQRT_2;
301 4 : return;
302 :
303 4 : case ROTATION_ROLL_270_PITCH_90:
304 4 : q1 = q3 = q4 = 0.5f;
305 4 : q2 = -0.5f;
306 4 : return;
307 :
308 4 : case ROTATION_ROLL_90_PITCH_180:
309 4 : q1 = q2 = 0;
310 4 : q3 = -HALF_SQRT_2;
311 4 : q4 = HALF_SQRT_2;
312 4 : return;
313 :
314 4 : case ROTATION_ROLL_270_PITCH_180:
315 4 : q1 = q2 = 0;
316 4 : q3 = q4 = HALF_SQRT_2;
317 4 : return;
318 :
319 4 : case ROTATION_ROLL_90_PITCH_270:
320 4 : q1 = q2 = q4 = 0.5f;
321 4 : q3 = -0.5;
322 4 : return;
323 :
324 4 : case ROTATION_ROLL_180_PITCH_270:
325 4 : q1 = q3 = 0;
326 4 : q2 = q4 = HALF_SQRT_2;
327 4 : return;
328 :
329 4 : case ROTATION_ROLL_270_PITCH_270:
330 4 : q1 = -0.5f;
331 4 : q2 = q3 = q4 = 0.5f;
332 4 : return;
333 :
334 4 : case ROTATION_ROLL_90_PITCH_180_YAW_90:
335 4 : q1 = q3 = -0.5f;
336 4 : q2 = q4 = 0.5f;
337 4 : return;
338 :
339 4 : case ROTATION_ROLL_90_YAW_270:
340 4 : q1 = q2 = -0.5f;
341 4 : q3 = q4 = 0.5f;
342 4 : return;
343 :
344 4 : case ROTATION_ROLL_90_PITCH_68_YAW_293:
345 4 : q1 = 0.26774500501681575137524760066299;
346 4 : q2 = 0.70698804688952421315661922562867;
347 4 : q3 = 0.012957683254962659713527273197542;
348 4 : q4 = -0.65445596665363614530264158020145;
349 4 : return;
350 :
351 2 : case ROTATION_PITCH_315:
352 2 : q1 = HALF_SQRT_2_PlUS_SQRT_2;
353 2 : q2 = q4 = 0;
354 2 : q3 = -HALF_SQRT_2_MINUS_SQTR_2;
355 2 : return;
356 :
357 2 : case ROTATION_ROLL_90_PITCH_315:
358 2 : q1 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
359 2 : q2 = HALF_SQRT_HALF_TIMES_TWO_PLUS_SQRT_TWO;
360 2 : q3 = -HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
361 2 : q4 = HALF_SQRT_HALF_TIMES_TWO_MINUS_SQRT_TWO;
362 2 : return;
363 :
364 4 : case ROTATION_PITCH_7:
365 4 : q1 = 0.99813479842186692003735970502021;
366 4 : q2 = q4 = 0;
367 4 : q3 = 0.061048539534856872956769535676358;
368 4 : return;
369 :
370 4 : case ROTATION_ROLL_45:
371 4 : q1 = HALF_SQRT_2_PlUS_SQRT_2;
372 4 : q2 = HALF_SQRT_2_MINUS_SQTR_2;
373 4 : q3 = q4 = 0.0;
374 4 : return;
375 :
376 4 : case ROTATION_ROLL_315:
377 4 : q1 = HALF_SQRT_2_PlUS_SQRT_2;
378 4 : q2 = -HALF_SQRT_2_MINUS_SQTR_2;
379 4 : q3 = q4 = 0.0;
380 4 : return;
381 :
382 84 : case ROTATION_CUSTOM_1:
383 : case ROTATION_CUSTOM_2:
384 : #if AP_CUSTOMROTATIONS_ENABLED
385 : // custom rotations not supported on eg. Periph by default
386 84 : AP::custom_rotations().from_rotation(rotation, *this);
387 84 : return;
388 : #endif
389 : case ROTATION_MAX:
390 : case ROTATION_CUSTOM_OLD:
391 : case ROTATION_CUSTOM_END:
392 : break;
393 : }
394 : // rotation invalid
395 2 : INTERNAL_ERROR(AP_InternalError::error_t::bad_rotation);
396 : }
397 :
398 : // rotate this quaternion by the given rotation
399 : template <typename T>
400 0 : void QuaternionT<T>::rotate(enum Rotation rotation)
401 : {
402 : // create quaternion from rotation matrix
403 0 : QuaternionT<T> q_from_rot;
404 0 : q_from_rot.from_rotation(rotation);
405 :
406 : // rotate this quaternion
407 0 : *this *= q_from_rot;
408 0 : }
409 :
410 : // convert a vector from earth to body frame
411 : template <typename T>
412 117835 : void QuaternionT<T>::earth_to_body(Vector3<T> &v) const
413 : {
414 117835 : Matrix3<T> m;
415 117835 : rotation_matrix(m);
416 117835 : v = m * v;
417 117835 : }
418 :
419 : // create a quaternion from Euler angles
420 : template <typename T>
421 43451915 : void QuaternionT<T>::from_euler(T roll, T pitch, T yaw)
422 : {
423 43451915 : const T cr2 = cosF(roll*0.5);
424 43451915 : const T cp2 = cosF(pitch*0.5);
425 43451915 : const T cy2 = cosF(yaw*0.5);
426 43451915 : const T sr2 = sinF(roll*0.5);
427 43451915 : const T sp2 = sinF(pitch*0.5);
428 43451915 : const T sy2 = sinF(yaw*0.5);
429 :
430 43451915 : q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
431 43451915 : q2 = sr2*cp2*cy2 - cr2*sp2*sy2;
432 43451915 : q3 = cr2*sp2*cy2 + sr2*cp2*sy2;
433 43451915 : q4 = cr2*cp2*sy2 - sr2*sp2*cy2;
434 43451915 : }
435 : template <typename T>
436 41752280 : void QuaternionT<T>::from_euler(const Vector3<T> &v)
437 : {
438 41752280 : from_euler(v[0], v[1], v[2]);
439 41752280 : }
440 :
441 : // create a quaternion from Euler angles applied in yaw, roll, pitch order
442 : // instead of the normal yaw, pitch, roll order
443 : template <typename T>
444 37625 : void QuaternionT<T>::from_vector312(T roll, T pitch, T yaw)
445 : {
446 37625 : Matrix3<T> m;
447 37625 : m.from_euler312(roll, pitch, yaw);
448 :
449 37625 : from_rotation_matrix(m);
450 37625 : }
451 :
452 : // create a quaternion from its axis-angle representation
453 : template <typename T>
454 404984646 : void QuaternionT<T>::from_axis_angle(Vector3<T> v)
455 : {
456 404984646 : const T theta = v.length();
457 404984646 : if (::is_zero(theta)) {
458 182589276 : q1 = 1.0f;
459 182589276 : q2=q3=q4=0.0f;
460 182589276 : return;
461 : }
462 222395370 : v /= theta;
463 222395370 : from_axis_angle(v,theta);
464 : }
465 :
466 : // create a quaternion from its axis-angle representation
467 : // the axis vector must be length 1, theta is in radians
468 : template <typename T>
469 301748738 : void QuaternionT<T>::from_axis_angle(const Vector3<T> &axis, T theta)
470 : {
471 : // axis must be a unit vector as there is no check for length
472 301748738 : if (::is_zero(theta)) {
473 32338192 : q1 = 1.0f;
474 32338192 : q2=q3=q4=0.0f;
475 32338192 : return;
476 : }
477 269410546 : const T st2 = sinF(0.5*theta);
478 :
479 269410546 : q1 = cosF(0.5*theta);
480 269410546 : q2 = axis.x * st2;
481 269410546 : q3 = axis.y * st2;
482 269410546 : q4 = axis.z * st2;
483 : }
484 :
485 : // rotate by the provided axis angle
486 : template <typename T>
487 277910317 : void QuaternionT<T>::rotate(const Vector3<T> &v)
488 : {
489 277910317 : QuaternionT<T> r;
490 277910317 : r.from_axis_angle(v);
491 277910317 : (*this) *= r;
492 277910317 : }
493 :
494 : // convert this quaternion to a rotation vector where the direction of the vector represents
495 : // the axis of rotation and the length of the vector represents the angle of rotation
496 : template <typename T>
497 282282996 : void QuaternionT<T>::to_axis_angle(Vector3<T> &v) const
498 : {
499 282282996 : const T l = sqrtF(sq(q2)+sq(q3)+sq(q4));
500 282282994 : v = Vector3<T>(q2,q3,q4);
501 282282995 : if (!::is_zero(l)) {
502 215323819 : v /= l;
503 215323819 : v *= wrap_PI(2.0f * atan2F(l,q1));
504 : }
505 282282990 : }
506 :
507 : // create a quaternion from its axis-angle representation
508 : // only use with small angles. I.e. length of v should less than 0.17 radians (i.e. 10 degrees)
509 : template <typename T>
510 0 : void QuaternionT<T>::from_axis_angle_fast(Vector3<T> v)
511 : {
512 0 : const T theta = v.length();
513 0 : if (::is_zero(theta)) {
514 0 : q1 = 1.0f;
515 0 : q2=q3=q4=0.0f;
516 0 : return;
517 : }
518 0 : v /= theta;
519 0 : from_axis_angle_fast(v,theta);
520 : }
521 :
522 : // create a quaternion from its axis-angle representation
523 : // theta should less than 0.17 radians (i.e. 10 degrees)
524 : template <typename T>
525 0 : void QuaternionT<T>::from_axis_angle_fast(const Vector3<T> &axis, T theta)
526 : {
527 0 : const T t2 = 0.5*theta;
528 0 : const T sqt2 = sq(t2);
529 0 : const T st2 = t2-sqt2*t2/6.0f;
530 :
531 0 : q1 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
532 0 : q2 = axis.x * st2;
533 0 : q3 = axis.y * st2;
534 0 : q4 = axis.z * st2;
535 0 : }
536 :
537 : // create a quaternion by integrating an angular velocity over some time_delta, which is
538 : // assumed to be small
539 : template <typename T>
540 14859 : void QuaternionT<T>::from_angular_velocity(const Vector3<T>& angular_velocity, float time_delta)
541 : {
542 14859 : const float half_time_delta = 0.5f*time_delta;
543 :
544 14859 : q1 = 1.0;
545 14859 : q2 = half_time_delta*angular_velocity.x;
546 14859 : q3 = half_time_delta*angular_velocity.y;
547 14859 : q4 = half_time_delta*angular_velocity.z;
548 14859 : normalize();
549 14859 : }
550 :
551 :
552 : // rotate by the provided axis angle
553 : // only use with small angles. I.e. length of v should less than 0.17 radians (i.e. 10 degrees)
554 : template <typename T>
555 0 : void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
556 : {
557 0 : const T theta = v.length();
558 0 : if (::is_zero(theta)) {
559 : return;
560 : }
561 0 : const T t2 = 0.5*theta;
562 0 : const T sqt2 = sq(t2);
563 0 : T st2 = t2-sqt2*t2/6.0f;
564 0 : st2 /= theta;
565 :
566 : //"rotation quaternion"
567 0 : const T w2 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
568 0 : const T x2 = v.x * st2;
569 0 : const T y2 = v.y * st2;
570 0 : const T z2 = v.z * st2;
571 :
572 : //copy our quaternion
573 0 : const T w1 = q1;
574 0 : const T x1 = q2;
575 0 : const T y1 = q3;
576 0 : const T z1 = q4;
577 :
578 : //do the multiply into our quaternion
579 0 : q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
580 0 : q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
581 0 : q3 = w1*y2 - x1*z2 + y1*w2 + z1*x2;
582 0 : q4 = w1*z2 + x1*y2 - y1*x2 + z1*w2;
583 : }
584 :
585 : // get euler roll angle
586 : template <typename T>
587 228045698 : T QuaternionT<T>::get_euler_roll() const
588 : {
589 228045698 : return (atan2F(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
590 : }
591 :
592 : // get euler pitch angle
593 : template <typename T>
594 406909506 : T QuaternionT<T>::get_euler_pitch() const
595 : {
596 406909506 : return safe_asin(2.0f*(q1*q3 - q4*q2));
597 : }
598 :
599 : // get euler yaw angle
600 : template <typename T>
601 130013128 : T QuaternionT<T>::get_euler_yaw() const
602 : {
603 130013128 : return atan2F(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
604 : }
605 :
606 : // create eulers from a quaternion
607 : template <typename T>
608 3107819 : void QuaternionT<T>::to_euler(double &roll, double &pitch, double &yaw) const
609 : {
610 3107819 : roll = get_euler_roll();
611 3107819 : pitch = get_euler_pitch();
612 3107819 : yaw = get_euler_yaw();
613 3107819 : }
614 :
615 : template <typename T>
616 126889929 : void QuaternionT<T>::to_euler(float &roll, float &pitch, float &yaw) const
617 : {
618 126889929 : roll = get_euler_roll();
619 126889928 : pitch = get_euler_pitch();
620 126889924 : yaw = get_euler_yaw();
621 126889923 : }
622 :
623 : // create eulers from a quaternion
624 : template <typename T>
625 307568 : Vector3<T> QuaternionT<T>::to_vector312(void) const
626 : {
627 307568 : Matrix3<T> m;
628 307568 : rotation_matrix(m);
629 307568 : return m.to_euler312();
630 : }
631 :
632 : template <typename T>
633 746622058 : T QuaternionT<T>::length(void) const
634 : {
635 746622058 : return sqrtF(sq(q1) + sq(q2) + sq(q3) + sq(q4));
636 : }
637 :
638 : // gets the length squared of the quaternion
639 : template <typename T>
640 85 : T QuaternionT<T>::length_squared() const
641 : {
642 85 : return (T)(q1*q1 + q2*q2 + q3*q3 + q4*q4);
643 : }
644 :
645 : // return the reverse rotation of this quaternion
646 : template <typename T>
647 423627449 : QuaternionT<T> QuaternionT<T>::inverse(void) const
648 : {
649 423627449 : return QuaternionT<T>(q1, -q2, -q3, -q4);
650 : }
651 :
652 : // reverse the rotation of this quaternion
653 : template <typename T>
654 15467 : void QuaternionT<T>::invert()
655 : {
656 15467 : q2 = -q2;
657 15467 : q3 = -q3;
658 15467 : q4 = -q4;
659 15467 : }
660 :
661 : template <typename T>
662 746622048 : void QuaternionT<T>::normalize(void)
663 : {
664 746622048 : const T quatMag = length();
665 746622023 : if (!::is_zero(quatMag)) {
666 746622023 : const T quatMagInv = 1.0f/quatMag;
667 746622023 : q1 *= quatMagInv;
668 746622023 : q2 *= quatMagInv;
669 746622023 : q3 *= quatMagInv;
670 746622023 : q4 *= quatMagInv;
671 : } else {
672 : // The code goes here if the quaternion is [0,0,0,0]. This shouldn't happen.
673 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
674 : }
675 746622023 : }
676 :
677 : // Checks if each element of the quaternion is zero
678 : template <typename T>
679 185253325 : bool QuaternionT<T>::is_zero(void) const {
680 185253325 : return ::is_zero(q1) && ::is_zero(q2) && ::is_zero(q3) && ::is_zero(q4);
681 : }
682 :
683 : // zeros the quaternion to [0, 0, 0, 0], an invalid quaternion
684 : // See initialize() if you want the zero rotation quaternion
685 : template <typename T>
686 251 : void QuaternionT<T>::zero(void)
687 : {
688 251 : q1 = q2 = q3 = q4 = 0.0;
689 251 : }
690 :
691 : // Checks if the quaternion is unit_length within a tolerance
692 : // Returns True: if its magnitude is close to unit length +/- 1E-3
693 : // This limit is somewhat greater than sqrt(FLT_EPSL)
694 : template <typename T>
695 73 : bool QuaternionT<T>::is_unit_length(void) const
696 : {
697 73 : if (fabsF(length_squared() - 1) < 1E-3) {
698 69 : return true;
699 : }
700 :
701 : return false;
702 : }
703 :
704 : template <typename T>
705 190491311 : QuaternionT<T> QuaternionT<T>::operator*(const QuaternionT<T> &v) const
706 : {
707 190491311 : QuaternionT<T> ret;
708 : const T &w1 = q1;
709 : const T &x1 = q2;
710 : const T &y1 = q3;
711 : const T &z1 = q4;
712 :
713 190491311 : const T w2 = v.q1;
714 190491311 : const T x2 = v.q2;
715 190491311 : const T y2 = v.q3;
716 190491311 : const T z2 = v.q4;
717 :
718 190491311 : ret.q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
719 190491311 : ret.q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
720 190491311 : ret.q3 = w1*y2 - x1*z2 + y1*w2 + z1*x2;
721 190491311 : ret.q4 = w1*z2 + x1*y2 - y1*x2 + z1*w2;
722 :
723 190491311 : return ret;
724 : }
725 :
726 : // Optimized quaternion rotation operator, equivalent to converting
727 : // (*this) to a rotation matrix then multiplying it to the argument `v`.
728 : //
729 : // 15 multiplies and 15 add / subtracts. Caches 3 floats
730 : template <typename T>
731 178274488 : Vector3<T> QuaternionT<T>::operator*(const Vector3<T> &v) const
732 : {
733 : // This uses the formula
734 : //
735 : // v2 = v1 + 2 q1 * qv x v1 + 2 qv x qv x v1
736 : //
737 : // where "x" is the cross product (explicitly inlined for performance below),
738 : // "q1" is the scalar part and "qv" is the vector part of this quaternion
739 :
740 178274488 : Vector3<T> ret = v;
741 :
742 : // Compute and cache "qv x v1"
743 178274488 : T uv[] = {q3 * v.z - q4 * v.y, q4 * v.x - q2 * v.z, q2 * v.y - q3 * v.x};
744 :
745 178274488 : uv[0] += uv[0];
746 178274488 : uv[1] += uv[1];
747 178274488 : uv[2] += uv[2];
748 178274488 : ret.x += q1 * uv[0] + q3 * uv[2] - q4 * uv[1];
749 178274488 : ret.y += q1 * uv[1] + q4 * uv[0] - q2 * uv[2];
750 178274488 : ret.z += q1 * uv[2] + q2 * uv[1] - q3 * uv[0];
751 178274488 : return ret;
752 : }
753 :
754 : template <typename T>
755 435084473 : QuaternionT<T> &QuaternionT<T>::operator*=(const QuaternionT<T> &v)
756 : {
757 435084473 : const T w1 = q1;
758 435084473 : const T x1 = q2;
759 435084473 : const T y1 = q3;
760 435084473 : const T z1 = q4;
761 :
762 435084473 : const T w2 = v.q1;
763 435084473 : const T x2 = v.q2;
764 435084473 : const T y2 = v.q3;
765 435084473 : const T z2 = v.q4;
766 :
767 435084473 : q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
768 435084473 : q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
769 435084473 : q3 = w1*y2 - x1*z2 + y1*w2 + z1*x2;
770 435084473 : q4 = w1*z2 + x1*y2 - y1*x2 + z1*w2;
771 :
772 435084473 : return *this;
773 : }
774 :
775 : // Note: Quaternions have an inverse, but do not have a single, universally agreed-upon
776 : // "division" definition like real numbers. Quaternion multiplication is non-commutative,
777 : // so the result of q1 / q2 can follow two equally valid conventions:
778 : // (1) q1 / q2 = q1 * q2^-1
779 : // (2) q1 / q2 = q2^-1 * q1
780 : // Here, we use the second definition, following MATLAB: q1 / q2 = q2^-1 * q1.
781 : // For detailed definitions, see:
782 : // https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm
783 : // https://www.mathworks.com/help/aerotbx/ug/quatdivide.html
784 : // Note: We assume q2 is already normalized (a unit quaternion).
785 : template <typename T>
786 185153066 : QuaternionT<T> QuaternionT<T>::operator/(const QuaternionT<T> &v) const
787 : {
788 185153066 : QuaternionT<T> ret;
789 : const T &quat0 = q1;
790 : const T &quat1 = q2;
791 : const T &quat2 = q3;
792 : const T &quat3 = q4;
793 :
794 185153066 : if (is_zero()) {
795 : // The code goes here if the quaternion is [0,0,0,0]. This shouldn't happen.
796 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
797 : }
798 :
799 185153066 : const T rquat0 = v.q1;
800 185153066 : const T rquat1 = v.q2;
801 185153066 : const T rquat2 = v.q3;
802 185153066 : const T rquat3 = v.q4;
803 :
804 185153066 : ret.q1 = (rquat0*quat0 + rquat1*quat1 + rquat2*quat2 + rquat3*quat3);
805 185153066 : ret.q2 = (rquat0*quat1 - rquat1*quat0 - rquat2*quat3 + rquat3*quat2);
806 185153066 : ret.q3 = (rquat0*quat2 + rquat1*quat3 - rquat2*quat0 - rquat3*quat1);
807 185153066 : ret.q4 = (rquat0*quat3 - rquat1*quat2 + rquat2*quat1 - rquat3*quat0);
808 185153066 : return ret;
809 : }
810 :
811 : // angular difference in radians between quaternions
812 : template <typename T>
813 92744 : QuaternionT<T> QuaternionT<T>::angular_difference(const QuaternionT<T> &v) const
814 : {
815 92744 : return v.inverse() * *this;
816 : }
817 :
818 : // absolute (e.g. always positive) earth-frame roll-pitch difference (in radians) between this Quaternion and another
819 : template <typename T>
820 93429 : T QuaternionT<T>::roll_pitch_difference(const QuaternionT<T> &v) const
821 : {
822 : // convert Quaternions to rotation matrices
823 93429 : Matrix3<T> m, vm;
824 93429 : rotation_matrix(m);
825 93429 : v.rotation_matrix(vm);
826 :
827 : // rotate earth frame vertical vector by each rotation matrix
828 93429 : const Vector3<T> z_unit_vec{0,0,1};
829 93429 : const Vector3<T> z_unit_m = m.mul_transpose(z_unit_vec);
830 93429 : const Vector3<T> z_unit_vm = vm.mul_transpose(z_unit_vec);
831 93429 : const Vector3<T> vec_diff = z_unit_vm - z_unit_m;
832 93429 : const T vec_len_div2 = constrain_float(vec_diff.length() * 0.5, 0.0, 1.0);
833 :
834 : // calculate and return angular difference
835 93429 : return (2.0 * asinF(vec_len_div2));
836 : }
837 :
838 : // define for float and double
839 : template class QuaternionT<float>;
840 : template class QuaternionT<double>;
|
