2 Copyright (C) 2003, 2010 - Wolfire Games
4 This file is part of Lugaru.
6 Lugaru is free software; you can redistribute it and/or
7 modify it under the terms of the GNU General Public License
8 as published by the Free Software Foundation; either version 2
9 of the License, or (at your option) any later version.
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.
15 See the GNU General Public License for more details.
17 You should have received a copy of the GNU General Public License
18 along with this program; if not, write to the Free Software
19 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
22 #ifndef _PHYSICSMATH_H_
23 #define _PHYSICSMATH_H_
27 #include "MacCompatibility.h"
29 //------------------------------------------------------------------------//
31 //------------------------------------------------------------------------//
33 float const pi = 3.14159265f;
34 float const g = -32.174f; // acceleration due to gravity, ft/s^2
35 float const rho = 0.0023769f; // desity of air at sea level, slugs/ft^3
36 float const tol = 0.0000000001f; // float type tolerance
39 //------------------------------------------------------------------------//
41 //------------------------------------------------------------------------//
42 inline float DegreesToRadians(float deg);
43 inline float RadiansToDegrees(float rad);
45 inline float DegreesToRadians(float deg)
47 return deg * pi / 180.0f;
50 inline float RadiansToDegrees(float rad)
52 return rad * 180.0f / pi;
55 //------------------------------------------------------------------------//
56 // Vector Class and vector functions
57 //------------------------------------------------------------------------//
66 Vector(float xi, float yi, float zi);
68 float Magnitude(void);
72 Vector& operator+=(Vector u); // vector addition
73 Vector& operator-=(Vector u); // vector subtraction
74 Vector& operator*=(float s); // scalar multiply
75 Vector& operator/=(float s); // scalar divide
77 Vector operator-(void);
81 inline Vector operator+(Vector u, Vector v);
82 inline Vector operator-(Vector u, Vector v);
83 inline Vector operator^(Vector u, Vector v);
84 inline float operator*(Vector u, Vector v);
85 inline Vector operator*(float s, Vector u);
86 inline Vector operator*(Vector u, float s);
87 inline Vector operator/(Vector u, float s);
88 inline float TripleScalarProduct(Vector u, Vector v, Vector w);
90 float fast_sqrt2 (register float arg);
91 float fast_sqrt2 (register float arg)
93 // Can replace with slower return std::sqrt(arg);
94 register float result;
96 if (arg == 0.0) return 0.0;
99 frsqrte result,arg // Calculate Square root
102 // Newton Rhapson iterations.
103 result = result + 0.5 * result * (1.0 - arg * result * result);
104 result = result + 0.5 * result * (1.0 - arg * result * result);
109 inline Vector::Vector(void)
116 inline Vector::Vector(float xi, float yi, float zi)
123 inline float Vector::Magnitude(void)
125 return (float) sqrt(x * x + y * y + z * z);
128 inline void Vector::Normalize(void)
130 float m = (float) sqrt(x * x + y * y + z * z);
145 inline void Vector::Reverse(void)
152 inline Vector& Vector::operator+=(Vector u)
160 inline Vector& Vector::operator-=(Vector u)
168 inline Vector& Vector::operator*=(float s)
176 inline Vector& Vector::operator/=(float s)
184 inline Vector Vector::operator-(void)
186 return Vector(-x, -y, -z);
190 inline Vector operator+(Vector u, Vector v)
192 return Vector(u.x + v.x, u.y + v.y, u.z + v.z);
195 inline Vector operator-(Vector u, Vector v)
197 return Vector(u.x - v.x, u.y - v.y, u.z - v.z);
200 // Vector cross product (u cross v)
201 inline Vector operator^(Vector u, Vector v)
203 return Vector( u.y * v.z - u.z * v.y,
204 -u.x * v.z + u.z * v.x,
205 u.x * v.y - u.y * v.x );
208 // Vector dot product
209 inline float operator*(Vector u, Vector v)
211 return (u.x * v.x + u.y * v.y + u.z * v.z);
214 inline Vector operator*(float s, Vector u)
216 return Vector(u.x * s, u.y * s, u.z * s);
219 inline Vector operator*(Vector u, float s)
221 return Vector(u.x * s, u.y * s, u.z * s);
224 inline Vector operator/(Vector u, float s)
226 return Vector(u.x / s, u.y / s, u.z / s);
229 // triple scalar product (u dot (v cross w))
230 inline float TripleScalarProduct(Vector u, Vector v, Vector w)
232 return float( (u.x * (v.y * w.z - v.z * w.y)) +
233 (u.y * (-v.x * w.z + v.z * w.x)) +
234 (u.z * (v.x * w.y - v.y * w.x)) );
241 //------------------------------------------------------------------------//
242 // Matrix Class and matrix functions
243 //------------------------------------------------------------------------//
248 // elements eij: i -> row, j -> column
249 float e11, e12, e13, e21, e22, e23, e31, e32, e33;
252 Matrix3x3( float r1c1, float r1c2, float r1c3,
253 float r2c1, float r2c2, float r2c3,
254 float r3c1, float r3c2, float r3c3 );
257 Matrix3x3 Transpose(void);
258 Matrix3x3 Inverse(void);
260 Matrix3x3& operator+=(Matrix3x3 m);
261 Matrix3x3& operator-=(Matrix3x3 m);
262 Matrix3x3& operator*=(float s);
263 Matrix3x3& operator/=(float s);
266 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2);
267 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2);
268 inline Matrix3x3 operator/(Matrix3x3 m, float s);
269 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2);
270 inline Matrix3x3 operator*(Matrix3x3 m, float s);
271 inline Matrix3x3 operator*(float s, Matrix3x3 m);
272 inline Vector operator*(Matrix3x3 m, Vector u);
273 inline Vector operator*(Vector u, Matrix3x3 m);
279 inline Matrix3x3::Matrix3x3(void)
292 inline Matrix3x3::Matrix3x3( float r1c1, float r1c2, float r1c3,
293 float r2c1, float r2c2, float r2c3,
294 float r3c1, float r3c2, float r3c3 )
307 inline float Matrix3x3::det(void)
309 return e11 * e22 * e33 -
317 inline Matrix3x3 Matrix3x3::Transpose(void)
319 return Matrix3x3(e11, e21, e31, e12, e22, e32, e13, e23, e33);
322 inline Matrix3x3 Matrix3x3::Inverse(void)
324 float d = e11 * e22 * e33 -
334 return Matrix3x3( (e22 * e33 - e23 * e32) / d,
335 -(e12 * e33 - e13 * e32) / d,
336 (e12 * e23 - e13 * e22) / d,
337 -(e21 * e33 - e23 * e31) / d,
338 (e11 * e33 - e13 * e31) / d,
339 -(e11 * e23 - e13 * e21) / d,
340 (e21 * e32 - e22 * e31) / d,
341 -(e11 * e32 - e12 * e31) / d,
342 (e11 * e22 - e12 * e21) / d );
345 inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m)
359 inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m)
373 inline Matrix3x3& Matrix3x3::operator*=(float s)
387 inline Matrix3x3& Matrix3x3::operator/=(float s)
401 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2)
403 return Matrix3x3( m1.e11 + m2.e11,
414 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2)
416 return Matrix3x3( m1.e11 - m2.e11,
427 inline Matrix3x3 operator/(Matrix3x3 m, float s)
429 return Matrix3x3( m.e11 / s,
440 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2)
442 return Matrix3x3( m1.e11 * m2.e11 + m1.e12 * m2.e21 + m1.e13 * m2.e31,
443 m1.e11 * m2.e12 + m1.e12 * m2.e22 + m1.e13 * m2.e32,
444 m1.e11 * m2.e13 + m1.e12 * m2.e23 + m1.e13 * m2.e33,
445 m1.e21 * m2.e11 + m1.e22 * m2.e21 + m1.e23 * m2.e31,
446 m1.e21 * m2.e12 + m1.e22 * m2.e22 + m1.e23 * m2.e32,
447 m1.e21 * m2.e13 + m1.e22 * m2.e23 + m1.e23 * m2.e33,
448 m1.e31 * m2.e11 + m1.e32 * m2.e21 + m1.e33 * m2.e31,
449 m1.e31 * m2.e12 + m1.e32 * m2.e22 + m1.e33 * m2.e32,
450 m1.e31 * m2.e13 + m1.e32 * m2.e23 + m1.e33 * m2.e33 );
453 inline Matrix3x3 operator*(Matrix3x3 m, float s)
455 return Matrix3x3( m.e11 * s,
466 inline Matrix3x3 operator*(float s, Matrix3x3 m)
468 return Matrix3x3( m.e11 * s,
479 inline Vector operator*(Matrix3x3 m, Vector u)
481 return Vector( m.e11 * u.x + m.e12 * u.y + m.e13 * u.z,
482 m.e21 * u.x + m.e22 * u.y + m.e23 * u.z,
483 m.e31 * u.x + m.e32 * u.y + m.e33 * u.z);
486 inline Vector operator*(Vector u, Matrix3x3 m)
488 return Vector( u.x * m.e11 + u.y * m.e21 + u.z * m.e31,
489 u.x * m.e12 + u.y * m.e22 + u.z * m.e32,
490 u.x * m.e13 + u.y * m.e23 + u.z * m.e33);
493 //------------------------------------------------------------------------//
494 // Quaternion Class and Quaternion functions
495 //------------------------------------------------------------------------//
500 float n; // number (scalar) part
501 Vector v; // vector part: v.x, v.y, v.z
504 Quaternion(float e0, float e1, float e2, float e3);
506 float Magnitude(void);
507 Vector GetVector(void);
508 float GetScalar(void);
509 Quaternion operator+=(Quaternion q);
510 Quaternion operator-=(Quaternion q);
511 Quaternion operator*=(float s);
512 Quaternion operator/=(float s);
513 Quaternion operator~(void) const {
514 return Quaternion(n, -v.x, -v.y, -v.z);
518 inline Quaternion operator+(Quaternion q1, Quaternion q2);
519 inline Quaternion operator-(Quaternion q1, Quaternion q2);
520 inline Quaternion operator*(Quaternion q1, Quaternion q2);
521 inline Quaternion operator*(Quaternion q, float s);
522 inline Quaternion operator*(float s, Quaternion q);
523 inline Quaternion operator*(Quaternion q, Vector v);
524 inline Quaternion operator*(Vector v, Quaternion q);
525 inline Quaternion operator/(Quaternion q, float s);
526 inline float QGetAngle(Quaternion q);
527 inline Vector QGetAxis(Quaternion q);
528 inline Quaternion QRotate(Quaternion q1, Quaternion q2);
529 inline Vector QVRotate(Quaternion q, Vector v);
530 inline Quaternion MakeQFromEulerAngles(float x, float y, float z);
531 inline Vector MakeEulerAnglesFromQ(Quaternion q);
534 inline Quaternion::Quaternion(void)
542 inline Quaternion::Quaternion(float e0, float e1, float e2, float e3)
550 inline float Quaternion::Magnitude(void)
552 return (float) sqrt(n * n + v.x * v.x + v.y * v.y + v.z * v.z);
555 inline Vector Quaternion::GetVector(void)
557 return Vector(v.x, v.y, v.z);
560 inline float Quaternion::GetScalar(void)
565 inline Quaternion Quaternion::operator+=(Quaternion q)
574 inline Quaternion Quaternion::operator-=(Quaternion q)
583 inline Quaternion Quaternion::operator*=(float s)
592 inline Quaternion Quaternion::operator/=(float s)
601 /*inline Quaternion Quaternion::operator~()
603 return Quaternion(n, -v.x, -v.y, -v.z);
606 inline Quaternion operator+(Quaternion q1, Quaternion q2)
608 return Quaternion( q1.n + q2.n,
614 inline Quaternion operator-(Quaternion q1, Quaternion q2)
616 return Quaternion( q1.n - q2.n,
622 inline Quaternion operator*(Quaternion q1, Quaternion q2)
624 return Quaternion( q1.n * q2.n - q1.v.x * q2.v.x - q1.v.y * q2.v.y - q1.v.z * q2.v.z,
625 q1.n * q2.v.x + q1.v.x * q2.n + q1.v.y * q2.v.z - q1.v.z * q2.v.y,
626 q1.n * q2.v.y + q1.v.y * q2.n + q1.v.z * q2.v.x - q1.v.x * q2.v.z,
627 q1.n * q2.v.z + q1.v.z * q2.n + q1.v.x * q2.v.y - q1.v.y * q2.v.x);
630 inline Quaternion operator*(Quaternion q, float s)
632 return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
635 inline Quaternion operator*(float s, Quaternion q)
637 return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
640 inline Quaternion operator*(Quaternion q, Vector v)
642 return Quaternion( -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
643 q.n * v.x + q.v.y * v.z - q.v.z * v.y,
644 q.n * v.y + q.v.z * v.x - q.v.x * v.z,
645 q.n * v.z + q.v.x * v.y - q.v.y * v.x);
648 inline Quaternion operator*(Vector v, Quaternion q)
650 return Quaternion( -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
651 q.n * v.x + q.v.z * v.y - q.v.y * v.z,
652 q.n * v.y + q.v.x * v.z - q.v.z * v.x,
653 q.n * v.z + q.v.y * v.x - q.v.x * v.y);
656 inline Quaternion operator/(Quaternion q, float s)
658 return Quaternion(q.n / s, q.v.x / s, q.v.y / s, q.v.z / s);
661 inline float QGetAngle(Quaternion q)
663 return (float) (2 * acosf(q.n));
666 inline Vector QGetAxis(Quaternion q)
680 inline Quaternion QRotate(Quaternion q1, Quaternion q2)
682 return q1 * q2 * (~q1);
685 inline Vector QVRotate(Quaternion q, Vector v)
692 return t.GetVector();
695 inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
698 double roll = DegreesToRadians(x);
699 double pitch = DegreesToRadians(y);
700 double yaw = DegreesToRadians(z);
702 double cyaw, cpitch, croll, syaw, spitch, sroll;
703 double cyawcpitch, syawspitch, cyawspitch, syawcpitch;
705 cyaw = cos(0.5f * yaw);
706 cpitch = cos(0.5f * pitch);
707 croll = cos(0.5f * roll);
708 syaw = sin(0.5f * yaw);
709 spitch = sin(0.5f * pitch);
710 sroll = sin(0.5f * roll);
712 cyawcpitch = cyaw * cpitch;
713 syawspitch = syaw * spitch;
714 cyawspitch = cyaw * spitch;
715 syawcpitch = syaw * cpitch;
717 q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
718 q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
719 q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
720 q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);
725 inline Vector MakeEulerAnglesFromQ(Quaternion q)
727 double r11, r21, r31, r32, r33;
728 double q00, q11, q22, q33;
737 r11 = q00 + q11 - q22 - q33;
738 r21 = 2 * (q.v.x * q.v.y + q.n * q.v.z);
739 r31 = 2 * (q.v.x * q.v.z - q.n * q.v.y);
740 r32 = 2 * (q.v.y * q.v.z + q.n * q.v.x);
741 r33 = q00 - q11 - q22 + q33;
744 if (tmp > 0.999999) {
745 double r12 = 2 * (q.v.x * q.v.y - q.n * q.v.z);
746 double r13 = 2 * (q.v.x * q.v.z + q.n * q.v.y);
748 u.x = RadiansToDegrees(0.0f); //roll
749 u.y = RadiansToDegrees((float) (-(pi / 2) * r31 / tmp)); // pitch
750 u.z = RadiansToDegrees((float) atan2(-r12, -r31 * r13)); // yaw
754 u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll
755 u.y = RadiansToDegrees((float) asinf(-r31)); // pitch
756 u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw