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 modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation; either version 2 of the License, or
9 (at your option) any later version.
11 Lugaru 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.
16 You should have received a copy of the GNU General Public License
17 along with Lugaru. If not, see <http://www.gnu.org/licenses/>.
20 #ifndef _PHYSICSMATH_H_
21 #define _PHYSICSMATH_H_
25 #include "MacCompatibility.h"
27 //------------------------------------------------------------------------//
29 //------------------------------------------------------------------------//
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
37 //------------------------------------------------------------------------//
39 //------------------------------------------------------------------------//
40 inline float DegreesToRadians(float deg);
41 inline float RadiansToDegrees(float rad);
43 inline float DegreesToRadians(float deg)
45 return deg * pi / 180.0f;
48 inline float RadiansToDegrees(float rad)
50 return rad * 180.0f / pi;
53 //------------------------------------------------------------------------//
54 // Vector Class and vector functions
55 //------------------------------------------------------------------------//
64 Vector(float xi, float yi, float zi);
66 float Magnitude(void);
70 Vector& operator+=(Vector u); // vector addition
71 Vector& operator-=(Vector u); // vector subtraction
72 Vector& operator*=(float s); // scalar multiply
73 Vector& operator/=(float s); // scalar divide
75 Vector operator-(void);
79 inline Vector operator+(Vector u, Vector v);
80 inline Vector operator-(Vector u, Vector v);
81 inline Vector operator^(Vector u, Vector v);
82 inline float operator*(Vector u, Vector v);
83 inline Vector operator*(float s, Vector u);
84 inline Vector operator*(Vector u, float s);
85 inline Vector operator/(Vector u, float s);
86 inline float TripleScalarProduct(Vector u, Vector v, Vector w);
88 float fast_sqrt2 (register float arg);
89 float fast_sqrt2 (register float arg)
91 // Can replace with slower return std::sqrt(arg);
92 register float result;
94 if (arg == 0.0) return 0.0;
97 frsqrte result,arg // Calculate Square root
100 // Newton Rhapson iterations.
101 result = result + 0.5 * result * (1.0 - arg * result * result);
102 result = result + 0.5 * result * (1.0 - arg * result * result);
107 inline Vector::Vector(void)
114 inline Vector::Vector(float xi, float yi, float zi)
121 inline float Vector::Magnitude(void)
123 return (float) sqrt(x * x + y * y + z * z);
126 inline void Vector::Normalize(void)
128 float m = (float) sqrt(x * x + y * y + z * z);
143 inline void Vector::Reverse(void)
150 inline Vector& Vector::operator+=(Vector u)
158 inline Vector& Vector::operator-=(Vector u)
166 inline Vector& Vector::operator*=(float s)
174 inline Vector& Vector::operator/=(float s)
182 inline Vector Vector::operator-(void)
184 return Vector(-x, -y, -z);
188 inline Vector operator+(Vector u, Vector v)
190 return Vector(u.x + v.x, u.y + v.y, u.z + v.z);
193 inline Vector operator-(Vector u, Vector v)
195 return Vector(u.x - v.x, u.y - v.y, u.z - v.z);
198 // Vector cross product (u cross v)
199 inline Vector operator^(Vector u, Vector v)
201 return Vector( u.y * v.z - u.z * v.y,
202 -u.x * v.z + u.z * v.x,
203 u.x * v.y - u.y * v.x );
206 // Vector dot product
207 inline float operator*(Vector u, Vector v)
209 return (u.x * v.x + u.y * v.y + u.z * v.z);
212 inline Vector operator*(float s, Vector u)
214 return Vector(u.x * s, u.y * s, u.z * s);
217 inline Vector operator*(Vector u, float s)
219 return Vector(u.x * s, u.y * s, u.z * s);
222 inline Vector operator/(Vector u, float s)
224 return Vector(u.x / s, u.y / s, u.z / s);
227 // triple scalar product (u dot (v cross w))
228 inline float TripleScalarProduct(Vector u, Vector v, Vector w)
230 return float( (u.x * (v.y * w.z - v.z * w.y)) +
231 (u.y * (-v.x * w.z + v.z * w.x)) +
232 (u.z * (v.x * w.y - v.y * w.x)) );
239 //------------------------------------------------------------------------//
240 // Matrix Class and matrix functions
241 //------------------------------------------------------------------------//
246 // elements eij: i -> row, j -> column
247 float e11, e12, e13, e21, e22, e23, e31, e32, e33;
250 Matrix3x3( float r1c1, float r1c2, float r1c3,
251 float r2c1, float r2c2, float r2c3,
252 float r3c1, float r3c2, float r3c3 );
255 Matrix3x3 Transpose(void);
256 Matrix3x3 Inverse(void);
258 Matrix3x3& operator+=(Matrix3x3 m);
259 Matrix3x3& operator-=(Matrix3x3 m);
260 Matrix3x3& operator*=(float s);
261 Matrix3x3& operator/=(float s);
264 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2);
265 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2);
266 inline Matrix3x3 operator/(Matrix3x3 m, float s);
267 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2);
268 inline Matrix3x3 operator*(Matrix3x3 m, float s);
269 inline Matrix3x3 operator*(float s, Matrix3x3 m);
270 inline Vector operator*(Matrix3x3 m, Vector u);
271 inline Vector operator*(Vector u, Matrix3x3 m);
277 inline Matrix3x3::Matrix3x3(void)
290 inline Matrix3x3::Matrix3x3( float r1c1, float r1c2, float r1c3,
291 float r2c1, float r2c2, float r2c3,
292 float r3c1, float r3c2, float r3c3 )
305 inline float Matrix3x3::det(void)
307 return e11 * e22 * e33 -
315 inline Matrix3x3 Matrix3x3::Transpose(void)
317 return Matrix3x3(e11, e21, e31, e12, e22, e32, e13, e23, e33);
320 inline Matrix3x3 Matrix3x3::Inverse(void)
322 float d = e11 * e22 * e33 -
332 return Matrix3x3( (e22 * e33 - e23 * e32) / d,
333 -(e12 * e33 - e13 * e32) / d,
334 (e12 * e23 - e13 * e22) / d,
335 -(e21 * e33 - e23 * e31) / d,
336 (e11 * e33 - e13 * e31) / d,
337 -(e11 * e23 - e13 * e21) / d,
338 (e21 * e32 - e22 * e31) / d,
339 -(e11 * e32 - e12 * e31) / d,
340 (e11 * e22 - e12 * e21) / d );
343 inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m)
357 inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m)
371 inline Matrix3x3& Matrix3x3::operator*=(float s)
385 inline Matrix3x3& Matrix3x3::operator/=(float s)
399 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2)
401 return Matrix3x3( m1.e11 + m2.e11,
412 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2)
414 return Matrix3x3( m1.e11 - m2.e11,
425 inline Matrix3x3 operator/(Matrix3x3 m, float s)
427 return Matrix3x3( m.e11 / s,
438 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2)
440 return Matrix3x3( m1.e11 * m2.e11 + m1.e12 * m2.e21 + m1.e13 * m2.e31,
441 m1.e11 * m2.e12 + m1.e12 * m2.e22 + m1.e13 * m2.e32,
442 m1.e11 * m2.e13 + m1.e12 * m2.e23 + m1.e13 * m2.e33,
443 m1.e21 * m2.e11 + m1.e22 * m2.e21 + m1.e23 * m2.e31,
444 m1.e21 * m2.e12 + m1.e22 * m2.e22 + m1.e23 * m2.e32,
445 m1.e21 * m2.e13 + m1.e22 * m2.e23 + m1.e23 * m2.e33,
446 m1.e31 * m2.e11 + m1.e32 * m2.e21 + m1.e33 * m2.e31,
447 m1.e31 * m2.e12 + m1.e32 * m2.e22 + m1.e33 * m2.e32,
448 m1.e31 * m2.e13 + m1.e32 * m2.e23 + m1.e33 * m2.e33 );
451 inline Matrix3x3 operator*(Matrix3x3 m, float s)
453 return Matrix3x3( m.e11 * s,
464 inline Matrix3x3 operator*(float s, Matrix3x3 m)
466 return Matrix3x3( m.e11 * s,
477 inline Vector operator*(Matrix3x3 m, Vector u)
479 return Vector( m.e11 * u.x + m.e12 * u.y + m.e13 * u.z,
480 m.e21 * u.x + m.e22 * u.y + m.e23 * u.z,
481 m.e31 * u.x + m.e32 * u.y + m.e33 * u.z);
484 inline Vector operator*(Vector u, Matrix3x3 m)
486 return Vector( u.x * m.e11 + u.y * m.e21 + u.z * m.e31,
487 u.x * m.e12 + u.y * m.e22 + u.z * m.e32,
488 u.x * m.e13 + u.y * m.e23 + u.z * m.e33);
491 //------------------------------------------------------------------------//
492 // Quaternion Class and Quaternion functions
493 //------------------------------------------------------------------------//
498 float n; // number (scalar) part
499 Vector v; // vector part: v.x, v.y, v.z
502 Quaternion(float e0, float e1, float e2, float e3);
504 float Magnitude(void);
505 Vector GetVector(void);
506 float GetScalar(void);
507 Quaternion operator+=(Quaternion q);
508 Quaternion operator-=(Quaternion q);
509 Quaternion operator*=(float s);
510 Quaternion operator/=(float s);
511 Quaternion operator~(void) const {
512 return Quaternion(n, -v.x, -v.y, -v.z);
516 inline Quaternion operator+(Quaternion q1, Quaternion q2);
517 inline Quaternion operator-(Quaternion q1, Quaternion q2);
518 inline Quaternion operator*(Quaternion q1, Quaternion q2);
519 inline Quaternion operator*(Quaternion q, float s);
520 inline Quaternion operator*(float s, Quaternion q);
521 inline Quaternion operator*(Quaternion q, Vector v);
522 inline Quaternion operator*(Vector v, Quaternion q);
523 inline Quaternion operator/(Quaternion q, float s);
524 inline float QGetAngle(Quaternion q);
525 inline Vector QGetAxis(Quaternion q);
526 inline Quaternion QRotate(Quaternion q1, Quaternion q2);
527 inline Vector QVRotate(Quaternion q, Vector v);
528 inline Quaternion MakeQFromEulerAngles(float x, float y, float z);
529 inline Vector MakeEulerAnglesFromQ(Quaternion q);
532 inline Quaternion::Quaternion(void)
540 inline Quaternion::Quaternion(float e0, float e1, float e2, float e3)
548 inline float Quaternion::Magnitude(void)
550 return (float) sqrt(n * n + v.x * v.x + v.y * v.y + v.z * v.z);
553 inline Vector Quaternion::GetVector(void)
555 return Vector(v.x, v.y, v.z);
558 inline float Quaternion::GetScalar(void)
563 inline Quaternion Quaternion::operator+=(Quaternion q)
572 inline Quaternion Quaternion::operator-=(Quaternion q)
581 inline Quaternion Quaternion::operator*=(float s)
590 inline Quaternion Quaternion::operator/=(float s)
599 /*inline Quaternion Quaternion::operator~()
601 return Quaternion(n, -v.x, -v.y, -v.z);
604 inline Quaternion operator+(Quaternion q1, Quaternion q2)
606 return Quaternion( q1.n + q2.n,
612 inline Quaternion operator-(Quaternion q1, Quaternion q2)
614 return Quaternion( q1.n - q2.n,
620 inline Quaternion operator*(Quaternion q1, Quaternion q2)
622 return Quaternion( q1.n * q2.n - q1.v.x * q2.v.x - q1.v.y * q2.v.y - q1.v.z * q2.v.z,
623 q1.n * q2.v.x + q1.v.x * q2.n + q1.v.y * q2.v.z - q1.v.z * q2.v.y,
624 q1.n * q2.v.y + q1.v.y * q2.n + q1.v.z * q2.v.x - q1.v.x * q2.v.z,
625 q1.n * q2.v.z + q1.v.z * q2.n + q1.v.x * q2.v.y - q1.v.y * q2.v.x);
628 inline Quaternion operator*(Quaternion q, float s)
630 return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
633 inline Quaternion operator*(float s, Quaternion q)
635 return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
638 inline Quaternion operator*(Quaternion q, Vector v)
640 return Quaternion( -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
641 q.n * v.x + q.v.y * v.z - q.v.z * v.y,
642 q.n * v.y + q.v.z * v.x - q.v.x * v.z,
643 q.n * v.z + q.v.x * v.y - q.v.y * v.x);
646 inline Quaternion operator*(Vector v, Quaternion q)
648 return Quaternion( -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
649 q.n * v.x + q.v.z * v.y - q.v.y * v.z,
650 q.n * v.y + q.v.x * v.z - q.v.z * v.x,
651 q.n * v.z + q.v.y * v.x - q.v.x * v.y);
654 inline Quaternion operator/(Quaternion q, float s)
656 return Quaternion(q.n / s, q.v.x / s, q.v.y / s, q.v.z / s);
659 inline float QGetAngle(Quaternion q)
661 return (float) (2 * acosf(q.n));
664 inline Vector QGetAxis(Quaternion q)
678 inline Quaternion QRotate(Quaternion q1, Quaternion q2)
680 return q1 * q2 * (~q1);
683 inline Vector QVRotate(Quaternion q, Vector v)
690 return t.GetVector();
693 inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
696 double roll = DegreesToRadians(x);
697 double pitch = DegreesToRadians(y);
698 double yaw = DegreesToRadians(z);
700 double cyaw, cpitch, croll, syaw, spitch, sroll;
701 double cyawcpitch, syawspitch, cyawspitch, syawcpitch;
703 cyaw = cos(0.5f * yaw);
704 cpitch = cos(0.5f * pitch);
705 croll = cos(0.5f * roll);
706 syaw = sin(0.5f * yaw);
707 spitch = sin(0.5f * pitch);
708 sroll = sin(0.5f * roll);
710 cyawcpitch = cyaw * cpitch;
711 syawspitch = syaw * spitch;
712 cyawspitch = cyaw * spitch;
713 syawcpitch = syaw * cpitch;
715 q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
716 q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
717 q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
718 q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);
723 inline Vector MakeEulerAnglesFromQ(Quaternion q)
725 double r11, r21, r31, r32, r33;
726 double q00, q11, q22, q33;
735 r11 = q00 + q11 - q22 - q33;
736 r21 = 2 * (q.v.x * q.v.y + q.n * q.v.z);
737 r31 = 2 * (q.v.x * q.v.z - q.n * q.v.y);
738 r32 = 2 * (q.v.y * q.v.z + q.n * q.v.x);
739 r33 = q00 - q11 - q22 + q33;
742 if (tmp > 0.999999) {
743 double r12 = 2 * (q.v.x * q.v.y - q.n * q.v.z);
744 double r13 = 2 * (q.v.x * q.v.z + q.n * q.v.y);
746 u.x = RadiansToDegrees(0.0f); //roll
747 u.y = RadiansToDegrees((float) (-(pi / 2) * r31 / tmp)); // pitch
748 u.z = RadiansToDegrees((float) atan2(-r12, -r31 * r13)); // yaw
752 u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll
753 u.y = RadiansToDegrees((float) asinf(-r31)); // pitch
754 u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw