1 /*
2 * PhysicsMath.h
3 * Copyright (C) 2007 by Bryan Duff <duff0097@gmail.com>
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
18 * USA
19 */
20 #ifndef _PHYSICSMATH_H_
21 #define _PHYSICSMATH_H_
22
23 #include <cmath>
24
25 #include "Quaternions.h"
26
27 //------------------------------------------------------------------------//
28 // Misc. Constants
29 //------------------------------------------------------------------------//
30
31 float const pi = 3.14159265f;
32 float const g = -32.174f; // acceleration due to gravity, ft/s^2
33 float const rho = 0.0023769f; // desity of air at sea level, slugs/ft^3
34 float const tol = 0.0000000001f; // float type tolerance
35
36 //------------------------------------------------------------------------//
37 // Misc. Functions
38 //------------------------------------------------------------------------//
39
40 inline float DegreesToRadians(float deg);
41
42 inline float RadiansToDegrees(float rad);
43
DegreesToRadians(float deg)44 inline float DegreesToRadians(float deg)
45 {
46 return deg * pi / 180.0f;
47 }
48
RadiansToDegrees(float rad)49 inline float RadiansToDegrees(float rad)
50 {
51 return rad * 180.0f / pi;
52 }
53
54 //------------------------------------------------------------------------//
55 // Vector Class and vector functions
56 //------------------------------------------------------------------------//
57
58 class Vector {
59 public:
60 float x;
61 float y;
62 float z;
63
64 Vector(void);
65 Vector(float xi, float yi, float zi);
66
67 float Magnitude(void);
68 void Normalize(void);
69 void Reverse(void);
70
71 Vector & operator+=(Vector u); // vector addition
72 Vector & operator-=(Vector u); // vector subtraction
73 Vector & operator*=(float s); // scalar multiply
74 Vector & operator/=(float s); // scalar divide
75
76 Vector operator-(void);
77
78 };
79
80 inline Vector operator+(Vector u, Vector v);
81
82 inline Vector operator-(Vector u, Vector v);
83
84 inline Vector operator^(Vector u, Vector v);
85
86 inline float operator*(Vector u, Vector v);
87
88 inline Vector operator*(float s, Vector u);
89
90 inline Vector operator*(Vector u, float s);
91
92 inline Vector operator/(Vector u, float s);
93
94 inline float TripleScalarProduct(Vector u, Vector v, Vector w);
95
Vector(void)96 inline Vector::Vector(void)
97 {
98 x = 0;
99 y = 0;
100 z = 0;
101 }
102
Vector(float xi,float yi,float zi)103 inline Vector::Vector(float xi, float yi, float zi)
104 {
105 x = xi;
106 y = yi;
107 z = zi;
108 }
109
Magnitude(void)110 inline float Vector::Magnitude(void)
111 {
112 return (float)fast_sqrt(x * x + y * y + z * z);
113 }
114
Normalize(void)115 inline void Vector::Normalize(void)
116 {
117 float m = (float)fast_sqrt(x * x + y * y + z * z);
118 if(m <= tol)
119 m = 1;
120
121 x /= m;
122 y /= m;
123 z /= m;
124
125 if(fabs(x) < tol)
126 x = 0.0f;
127
128 if(fabs(y) < tol)
129 y = 0.0f;
130
131 if(fabs(z) < tol)
132 z = 0.0f;
133
134 }
135
Reverse(void)136 inline void Vector::Reverse(void)
137 {
138 x = -x;
139 y = -y;
140 z = -z;
141 }
142
143 inline Vector & Vector::operator+=(Vector u)
144 {
145 x += u.x;
146 y += u.y;
147 z += u.z;
148
149 return *this;
150 }
151
152 inline Vector & Vector::operator-=(Vector u)
153 {
154 x -= u.x;
155 y -= u.y;
156 z -= u.z;
157
158 return *this;
159 }
160
161
162 inline Vector & Vector::operator*=(float s)
163 {
164 x *= s;
165 y *= s;
166 z *= s;
167
168 return *this;
169 }
170
171
172 inline Vector & Vector::operator/=(float s)
173 {
174 x /= s;
175 y /= s;
176 z /= s;
177
178 return *this;
179 }
180
181
182 inline Vector Vector::operator-(void)
183 {
184 return Vector(-x, -y, -z);
185 }
186
187
188 inline Vector operator+(Vector u, Vector v)
189 {
190 return Vector(u.x + v.x, u.y + v.y, u.z + v.z);
191 }
192
193
194 inline Vector operator-(Vector u, Vector v)
195 {
196 return Vector(u.x - v.x, u.y - v.y, u.z - v.z);
197 }
198
199
200 // Vector cross product (u cross v)
201 inline Vector operator^(Vector u, Vector v)
202 {
203 return Vector(u.y * v.z - u.z * v.y,
204 -u.x * v.z + u.z * v.x, u.x * v.y - u.y * v.x);
205 }
206
207
208 // Vector dot product
209 inline float operator*(Vector u, Vector v)
210 {
211 return (u.x * v.x + u.y * v.y + u.z * v.z);
212 }
213
214
215 inline Vector operator*(float s, Vector u)
216 {
217 return Vector(u.x * s, u.y * s, u.z * s);
218 }
219
220
221 inline Vector operator*(Vector u, float s)
222 {
223 return Vector(u.x * s, u.y * s, u.z * s);
224 }
225
226
227 inline Vector operator/(Vector u, float s)
228 {
229 return Vector(u.x / s, u.y / s, u.z / s);
230 }
231
232
233 // triple scalar product (u dot (v cross w))
TripleScalarProduct(Vector u,Vector v,Vector w)234 inline float TripleScalarProduct(Vector u, Vector v, Vector w)
235 {
236 return float ((u.x * (v.y * w.z - v.z * w.y)) +
237 (u.y * (-v.x * w.z + v.z * w.x)) +
238 (u.z * (v.x * w.y - v.y * w.x)));
239 }
240
241
242 //------------------------------------------------------------------------//
243 // Matrix Class and matrix functions
244 //------------------------------------------------------------------------//
245
246
247 class Matrix3x3 {
248 public:
249 // elements eij: i -> row, j -> column
250
251 float e11, e12, e13, e21, e22, e23, e31, e32, e33;
252
253 Matrix3x3(void);
254
255 Matrix3x3(float r1c1, float r1c2, float r1c3,
256 float r2c1, float r2c2, float r2c3,
257 float r3c1, float r3c2, float r3c3);
258
259 float det(void);
260
261 Matrix3x3 Transpose(void);
262
263 Matrix3x3 Inverse(void);
264
265 Matrix3x3 & operator+=(Matrix3x3 m);
266
267 Matrix3x3 & operator-=(Matrix3x3 m);
268
269 Matrix3x3 & operator*=(float s);
270
271 Matrix3x3 & operator/=(float s);
272
273 };
274
275
276 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2);
277
278 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2);
279
280 inline Matrix3x3 operator/(Matrix3x3 m, float s);
281
282 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2);
283
284 inline Matrix3x3 operator*(Matrix3x3 m, float s);
285
286 inline Matrix3x3 operator*(float s, Matrix3x3 m);
287
288 inline Vector operator*(Matrix3x3 m, Vector u);
289
290 inline Vector operator*(Vector u, Matrix3x3 m);
291
292
Matrix3x3(void)293 inline Matrix3x3::Matrix3x3(void)
294 {
295 e11 = 0;
296 e12 = 0;
297 e13 = 0;
298
299 e21 = 0;
300 e22 = 0;
301 e23 = 0;
302
303 e31 = 0;
304 e32 = 0;
305 e33 = 0;
306 }
307
308
Matrix3x3(float r1c1,float r1c2,float r1c3,float r2c1,float r2c2,float r2c3,float r3c1,float r3c2,float r3c3)309 inline Matrix3x3::Matrix3x3(float r1c1, float r1c2, float r1c3,
310 float r2c1, float r2c2, float r2c3,
311 float r3c1, float r3c2, float r3c3)
312 {
313
314 e11 = r1c1;
315 e12 = r1c2;
316 e13 = r1c3;
317
318 e21 = r2c1;
319 e22 = r2c2;
320 e23 = r2c3;
321
322 e31 = r3c1;
323 e32 = r3c2;
324 e33 = r3c3;
325
326 }
327
328
det(void)329 inline float Matrix3x3::det(void)
330 {
331 return e11 * e22 * e33 -
332 e11 * e32 * e23 +
333 e21 * e32 * e13 - e21 * e12 * e33 + e31 * e12 * e23 - e31 * e22 * e13;
334 }
335
336
Transpose(void)337 inline Matrix3x3 Matrix3x3::Transpose(void)
338 {
339 return Matrix3x3(e11, e21, e31, e12, e22, e32, e13, e23, e33);
340 }
341
342
Inverse(void)343 inline Matrix3x3 Matrix3x3::Inverse(void)
344 {
345 float d = e11 * e22 * e33 -
346 e11 * e32 * e23 +
347 e21 * e32 * e13 - e21 * e12 * e33 + e31 * e12 * e23 - e31 * e22 * e13;
348
349 if(d == 0)
350 d = 1;
351
352 return Matrix3x3((e22 * e33 - e23 * e32) / d,
353 -(e12 * e33 - e13 * e32) / d,
354 (e12 * e23 - e13 * e22) / d,
355 -(e21 * e33 - e23 * e31) / d,
356 (e11 * e33 - e13 * e31) / d,
357 -(e11 * e23 - e13 * e21) / d,
358 (e21 * e32 - e22 * e31) / d,
359 -(e11 * e32 - e12 * e31) / d, (e11 * e22 - e12 * e21) / d);
360
361 }
362
363
364 inline Matrix3x3 & Matrix3x3::operator+=(Matrix3x3 m)
365 {
366 e11 += m.e11;
367 e12 += m.e12;
368 e13 += m.e13;
369
370 e21 += m.e21;
371 e22 += m.e22;
372 e23 += m.e23;
373
374 e31 += m.e31;
375 e32 += m.e32;
376 e33 += m.e33;
377
378 return *this;
379 }
380
381
382 inline Matrix3x3 & Matrix3x3::operator-=(Matrix3x3 m)
383 {
384 e11 -= m.e11;
385 e12 -= m.e12;
386 e13 -= m.e13;
387
388 e21 -= m.e21;
389 e22 -= m.e22;
390 e23 -= m.e23;
391
392 e31 -= m.e31;
393 e32 -= m.e32;
394 e33 -= m.e33;
395
396 return *this;
397 }
398
399
400
401 inline Matrix3x3 & Matrix3x3::operator*=(float s)
402 {
403 e11 *= s;
404 e12 *= s;
405 e13 *= s;
406
407 e21 *= s;
408 e22 *= s;
409 e23 *= s;
410
411 e31 *= s;
412 e32 *= s;
413 e33 *= s;
414
415 return *this;
416 }
417
418
419
420 inline Matrix3x3 & Matrix3x3::operator/=(float s)
421 {
422 e11 /= s;
423 e12 /= s;
424 e13 /= s;
425
426 e21 /= s;
427 e22 /= s;
428 e23 /= s;
429
430 e31 /= s;
431 e32 /= s;
432 e33 /= s;
433
434 return *this;
435 }
436
437
438 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2)
439 {
440 return Matrix3x3(m1.e11 + m2.e11,
441 m1.e12 + m2.e12,
442 m1.e13 + m2.e13,
443 m1.e21 + m2.e21,
444 m1.e22 + m2.e22,
445 m1.e23 + m2.e23,
446 m1.e31 + m2.e31, m1.e32 + m2.e32, m1.e33 + m2.e33);
447 }
448
449
450 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2)
451 {
452 return Matrix3x3(m1.e11 - m2.e11,
453 m1.e12 - m2.e12,
454 m1.e13 - m2.e13,
455 m1.e21 - m2.e21,
456 m1.e22 - m2.e22,
457 m1.e23 - m2.e23,
458 m1.e31 - m2.e31, m1.e32 - m2.e32, m1.e33 - m2.e33);
459 }
460
461
462 inline Matrix3x3 operator/(Matrix3x3 m, float s)
463 {
464 return Matrix3x3(m.e11 / s,
465 m.e12 / s,
466 m.e13 / s,
467 m.e21 / s,
468 m.e22 / s, m.e23 / s, m.e31 / s, m.e32 / s, m.e33 / s);
469 }
470
471
472 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2)
473 {
474 return Matrix3x3(m1.e11 * m2.e11 + m1.e12 * m2.e21 + m1.e13 * m2.e31,
475 m1.e11 * m2.e12 + m1.e12 * m2.e22 + m1.e13 * m2.e32,
476 m1.e11 * m2.e13 + m1.e12 * m2.e23 + m1.e13 * m2.e33,
477 m1.e21 * m2.e11 + m1.e22 * m2.e21 + m1.e23 * m2.e31,
478 m1.e21 * m2.e12 + m1.e22 * m2.e22 + m1.e23 * m2.e32,
479 m1.e21 * m2.e13 + m1.e22 * m2.e23 + m1.e23 * m2.e33,
480 m1.e31 * m2.e11 + m1.e32 * m2.e21 + m1.e33 * m2.e31,
481 m1.e31 * m2.e12 + m1.e32 * m2.e22 + m1.e33 * m2.e32,
482 m1.e31 * m2.e13 + m1.e32 * m2.e23 + m1.e33 * m2.e33);
483 }
484
485
486 inline Matrix3x3 operator*(Matrix3x3 m, float s)
487 {
488 return Matrix3x3(m.e11 * s,
489 m.e12 * s,
490 m.e13 * s,
491 m.e21 * s,
492 m.e22 * s, m.e23 * s, m.e31 * s, m.e32 * s, m.e33 * s);
493 }
494
495
496 inline Matrix3x3 operator*(float s, Matrix3x3 m)
497 {
498 return Matrix3x3(m.e11 * s,
499 m.e12 * s,
500 m.e13 * s,
501 m.e21 * s,
502 m.e22 * s, m.e23 * s, m.e31 * s, m.e32 * s, m.e33 * s);
503 }
504
505 inline Vector operator*(Matrix3x3 m, Vector u)
506 {
507 return Vector(m.e11 * u.x + m.e12 * u.y + m.e13 * u.z,
508 m.e21 * u.x + m.e22 * u.y + m.e23 * u.z,
509 m.e31 * u.x + m.e32 * u.y + m.e33 * u.z);
510 }
511
512 inline Vector operator*(Vector u, Matrix3x3 m)
513 {
514 return Vector(u.x * m.e11 + u.y * m.e21 + u.z * m.e31,
515 u.x * m.e12 + u.y * m.e22 + u.z * m.e32,
516 u.x * m.e13 + u.y * m.e23 + u.z * m.e33);
517 }
518
519
520 //------------------------------------------------------------------------//
521 // Quaternion Class and Quaternion functions
522 //------------------------------------------------------------------------//
523
524
525 class Quaternion {
526 public:
527
528 float n; // number (scalar) part
529 Vector v; // vector part: v.x, v.y, v.z
530
531 Quaternion(void);
532
533 Quaternion(float e0, float e1, float e2, float e3);
534
535 float Magnitude(void);
536
537 Vector GetVector(void);
538
539 float GetScalar(void);
540
541 Quaternion operator+=(Quaternion q);
542
543 Quaternion operator-=(Quaternion q);
544
545 Quaternion operator*=(float s);
546
547 Quaternion operator/=(float s);
548
549 Quaternion operator~(void) const {
550 return Quaternion(n, -v.x, -v.y, -v.z);
551 }
552 };
553
554 inline Quaternion operator+(Quaternion q1, Quaternion q2);
555
556 inline Quaternion operator-(Quaternion q1, Quaternion q2);
557
558 inline Quaternion operator*(Quaternion q1, Quaternion q2);
559
560 inline Quaternion operator*(Quaternion q, float s);
561
562 inline Quaternion operator*(float s, Quaternion q);
563
564 inline Quaternion operator*(Quaternion q, Vector v);
565
566 inline Quaternion operator*(Vector v, Quaternion q);
567
568 inline Quaternion operator/(Quaternion q, float s);
569
570 inline float QGetAngle(Quaternion q);
571
572 inline Vector QGetAxis(Quaternion q);
573
574 inline Quaternion QRotate(Quaternion q1, Quaternion q2);
575
576 inline Vector QVRotate(Quaternion q, Vector v);
577
578 inline Quaternion MakeQFromEulerAngles(float x, float y, float z);
579
580 inline Vector MakeEulerAnglesFromQ(Quaternion q);
581
Quaternion(void)582 inline Quaternion::Quaternion(void)
583 {
584 n = 0;
585 v.x = 0;
586 v.y = 0;
587 v.z = 0;
588 }
589
Quaternion(float e0,float e1,float e2,float e3)590 inline Quaternion::Quaternion(float e0, float e1, float e2, float e3)
591 {
592 n = e0;
593 v.x = e1;
594 v.y = e2;
595 v.z = e3;
596 }
597
Magnitude(void)598 inline float Quaternion::Magnitude(void)
599 {
600 return (float)fast_sqrt(n * n + v.x * v.x + v.y * v.y + v.z * v.z);
601 }
602
GetVector(void)603 inline Vector Quaternion::GetVector(void)
604 {
605 return Vector(v.x, v.y, v.z);
606 }
607
GetScalar(void)608 inline float Quaternion::GetScalar(void)
609 {
610 return n;
611 }
612
613 inline Quaternion Quaternion::operator+=(Quaternion q)
614 {
615 n += q.n;
616 v.x += q.v.x;
617 v.y += q.v.y;
618 v.z += q.v.z;
619 return *this;
620 }
621
622
623
624 inline Quaternion Quaternion::operator-=(Quaternion q)
625 {
626 n -= q.n;
627 v.x -= q.v.x;
628 v.y -= q.v.y;
629 v.z -= q.v.z;
630
631 return *this;
632 }
633
634 inline Quaternion Quaternion::operator*=(float s)
635 {
636 n *= s;
637 v.x *= s;
638 v.y *= s;
639 v.z *= s;
640
641 return *this;
642 }
643
644 inline Quaternion Quaternion::operator/=(float s)
645 {
646 n /= s;
647 v.x /= s;
648 v.y /= s;
649 v.z /= s;
650
651 return *this;
652 }
653
654
655
656 /*inline Quaternion Quaternion::operator~()
657
658 {
659
660 return Quaternion(n, -v.x, -v.y, -v.z);
661
662 }*/
663
664
665
666 inline Quaternion operator+(Quaternion q1, Quaternion q2)
667 {
668 return Quaternion(q1.n + q2.n,
669 q1.v.x + q2.v.x, q1.v.y + q2.v.y, q1.v.z + q2.v.z);
670 }
671
672
673
674 inline Quaternion operator-(Quaternion q1, Quaternion q2)
675 {
676 return Quaternion(q1.n - q2.n,
677 q1.v.x - q2.v.x, q1.v.y - q2.v.y, q1.v.z - q2.v.z);
678 }
679
680
681
682 inline Quaternion operator*(Quaternion q1, Quaternion q2)
683 {
684 return Quaternion(q1.n * q2.n - q1.v.x * q2.v.x - q1.v.y * q2.v.y -
685 q1.v.z * q2.v.z,
686 q1.n * q2.v.x + q1.v.x * q2.n + q1.v.y * q2.v.z -
687 q1.v.z * q2.v.y,
688 q1.n * q2.v.y + q1.v.y * q2.n + q1.v.z * q2.v.x -
689 q1.v.x * q2.v.z,
690 q1.n * q2.v.z + q1.v.z * q2.n + q1.v.x * q2.v.y -
691 q1.v.y * q2.v.x);
692 }
693
694
695
696 inline Quaternion operator*(Quaternion q, float s)
697 {
698 return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
699 }
700
701
702
703 inline Quaternion operator*(float s, Quaternion q)
704 {
705 return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
706 }
707
708
709
710 inline Quaternion operator*(Quaternion q, Vector v)
711 {
712 return Quaternion(-(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
713 q.n * v.x + q.v.y * v.z - q.v.z * v.y,
714 q.n * v.y + q.v.z * v.x - q.v.x * v.z,
715 q.n * v.z + q.v.x * v.y - q.v.y * v.x);
716 }
717
718
719
720 inline Quaternion operator*(Vector v, Quaternion q)
721 {
722 return Quaternion(-(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
723 q.n * v.x + q.v.z * v.y - q.v.y * v.z,
724 q.n * v.y + q.v.x * v.z - q.v.z * v.x,
725 q.n * v.z + q.v.y * v.x - q.v.x * v.y);
726 }
727
728
729
730 inline Quaternion operator/(Quaternion q, float s)
731 {
732 return Quaternion(q.n / s, q.v.x / s, q.v.y / s, q.v.z / s);
733 }
734
QGetAngle(Quaternion q)735 inline float QGetAngle(Quaternion q)
736 {
737 return (float)(2 * acos(q.n));
738 }
739
QGetAxis(Quaternion q)740 inline Vector QGetAxis(Quaternion q)
741 {
742 Vector v;
743 float m;
744
745 v = q.GetVector();
746 m = v.Magnitude();
747
748 if(m <= tol)
749 return Vector();
750 else
751 return v / m;
752 }
753
QRotate(Quaternion q1,Quaternion q2)754 inline Quaternion QRotate(Quaternion q1, Quaternion q2)
755 {
756 return q1 * q2 * (~q1);
757 }
758
QVRotate(Quaternion q,Vector v)759 inline Vector QVRotate(Quaternion q, Vector v)
760 {
761 Quaternion t;
762
763 t = q * v * (~q);
764
765 return t.GetVector();
766 }
767
MakeQFromEulerAngles(float x,float y,float z)768 inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
769 {
770 Quaternion q;
771 double roll = DegreesToRadians(x);
772 double pitch = DegreesToRadians(y);
773 double yaw = DegreesToRadians(z);
774
775 double cyaw, cpitch, croll, syaw, spitch, sroll;
776 double cyawcpitch, syawspitch, cyawspitch, syawcpitch;
777
778 cyaw = cos(0.5f * yaw);
779 cpitch = cos(0.5f * pitch);
780 croll = cos(0.5f * roll);
781 syaw = sin(0.5f * yaw);
782 spitch = sin(0.5f * pitch);
783 sroll = sin(0.5f * roll);
784
785 cyawcpitch = cyaw * cpitch;
786 syawspitch = syaw * spitch;
787 cyawspitch = cyaw * spitch;
788 syawcpitch = syaw * cpitch;
789
790 q.n = (float)(cyawcpitch * croll + syawspitch * sroll);
791 q.v.x = (float)(cyawcpitch * sroll - syawspitch * croll);
792 q.v.y = (float)(cyawspitch * croll + syawcpitch * sroll);
793 q.v.z = (float)(syawcpitch * croll - cyawspitch * sroll);
794
795 return q;
796 }
797
798
MakeEulerAnglesFromQ(Quaternion q)799 inline Vector MakeEulerAnglesFromQ(Quaternion q)
800 {
801 double r11, r21, r31, r32, r33, r12, r13;
802 double q00, q11, q22, q33;
803 double tmp;
804
805 Vector u;
806
807 q00 = q.n * q.n;
808 q11 = q.v.x * q.v.x;
809 q22 = q.v.y * q.v.y;
810 q33 = q.v.z * q.v.z;
811
812 r11 = q00 + q11 - q22 - q33;
813 r21 = 2 * (q.v.x * q.v.y + q.n * q.v.z);
814 r31 = 2 * (q.v.x * q.v.z - q.n * q.v.y);
815 r32 = 2 * (q.v.y * q.v.z + q.n * q.v.x);
816
817 r33 = q00 - q11 - q22 + q33;
818
819 tmp = fabs(r31);
820
821 if(tmp > 0.999999) {
822 r12 = 2 * (q.v.x * q.v.y - q.n * q.v.z);
823 r13 = 2 * (q.v.x * q.v.z + q.n * q.v.y);
824
825 u.x = RadiansToDegrees(0.0f); //roll
826 u.y = RadiansToDegrees((float)(-(pi / 2) * r31 / tmp)); // pitch
827 u.z = RadiansToDegrees((float)atan2(-r12, -r31 * r13)); // yaw
828
829 return u;
830 }
831
832 u.x = RadiansToDegrees((float)atan2(r32, r33)); // roll
833 u.y = RadiansToDegrees((float)asin(-r31)); // pitch
834 u.z = RadiansToDegrees((float)atan2(r21, r11)); // yaw
835
836 return u;
837 }
838
839 #endif
840