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 //------------------------------------------------------------------------//
65 Vector(float xi, float yi, float zi);
67 float Magnitude(void);
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
76 Vector operator-(void);
80 inline Vector operator+(Vector u, Vector v);
81 inline Vector operator-(Vector u, Vector v);
82 inline Vector operator^(Vector u, Vector v);
83 inline float operator*(Vector u, Vector v);
84 inline Vector operator*(float s, Vector u);
85 inline Vector operator*(Vector u, float s);
86 inline Vector operator/(Vector u, float s);
87 inline float TripleScalarProduct(Vector u, Vector v, Vector w);
89 float fast_sqrt2 (register float arg);
90 float fast_sqrt2 (register float arg)
92 // Can replace with slower return std::sqrt(arg);
93 register float result;
95 if (arg == 0.0) return 0.0;
98 frsqrte result,arg // Calculate Square root
101 // Newton Rhapson iterations.
102 result = result + 0.5 * result * (1.0 - arg * result * result);
103 result = result + 0.5 * result * (1.0 - arg * result * result);
108 inline Vector::Vector(void)
115 inline Vector::Vector(float xi, float yi, float zi)
122 inline float Vector::Magnitude(void)
124 return (float) sqrt(x*x + y*y + z*z);
127 inline void Vector::Normalize(void)
129 float m = (float) sqrt(x*x + y*y + z*z);
135 if (fabs(x) < tol) x = 0.0f;
136 if (fabs(y) < tol) y = 0.0f;
137 if (fabs(z) < tol) z = 0.0f;
140 inline void Vector::Reverse(void)
147 inline Vector& Vector::operator+=(Vector u)
155 inline Vector& Vector::operator-=(Vector u)
163 inline Vector& Vector::operator*=(float s)
171 inline Vector& Vector::operator/=(float s)
179 inline Vector Vector::operator-(void)
181 return Vector(-x, -y, -z);
185 inline Vector operator+(Vector u, Vector v)
187 return Vector(u.x + v.x, u.y + v.y, u.z + v.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 // Vector cross product (u cross v)
196 inline Vector operator^(Vector u, Vector v)
198 return Vector( u.y*v.z - u.z*v.y,
203 // Vector dot product
204 inline float operator*(Vector u, Vector v)
206 return (u.x*v.x + u.y*v.y + u.z*v.z);
209 inline Vector operator*(float s, Vector u)
211 return Vector(u.x*s, u.y*s, u.z*s);
214 inline Vector operator*(Vector u, float s)
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 // triple scalar product (u dot (v cross w))
225 inline float TripleScalarProduct(Vector u, Vector v, Vector w)
227 return float( (u.x * (v.y*w.z - v.z*w.y)) +
228 (u.y * (-v.x*w.z + v.z*w.x)) +
229 (u.z * (v.x*w.y - v.y*w.x)) );
236 //------------------------------------------------------------------------//
237 // Matrix Class and matrix functions
238 //------------------------------------------------------------------------//
242 // elements eij: i -> row, j -> column
243 float e11, e12, e13, e21, e22, e23, e31, e32, e33;
246 Matrix3x3( float r1c1, float r1c2, float r1c3,
247 float r2c1, float r2c2, float r2c3,
248 float r3c1, float r3c2, float r3c3 );
251 Matrix3x3 Transpose(void);
252 Matrix3x3 Inverse(void);
254 Matrix3x3& operator+=(Matrix3x3 m);
255 Matrix3x3& operator-=(Matrix3x3 m);
256 Matrix3x3& operator*=(float s);
257 Matrix3x3& operator/=(float s);
260 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2);
261 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2);
262 inline Matrix3x3 operator/(Matrix3x3 m, float s);
263 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2);
264 inline Matrix3x3 operator*(Matrix3x3 m, float s);
265 inline Matrix3x3 operator*(float s, Matrix3x3 m);
266 inline Vector operator*(Matrix3x3 m, Vector u);
267 inline Vector operator*(Vector u, Matrix3x3 m);
273 inline Matrix3x3::Matrix3x3(void)
286 inline Matrix3x3::Matrix3x3( float r1c1, float r1c2, float r1c3,
287 float r2c1, float r2c2, float r2c3,
288 float r3c1, float r3c2, float r3c3 )
301 inline float Matrix3x3::det(void)
311 inline Matrix3x3 Matrix3x3::Transpose(void)
313 return Matrix3x3(e11,e21,e31,e12,e22,e32,e13,e23,e33);
316 inline Matrix3x3 Matrix3x3::Inverse(void)
318 float d = e11*e22*e33 -
327 return Matrix3x3( (e22*e33-e23*e32)/d,
328 -(e12*e33-e13*e32)/d,
330 -(e21*e33-e23*e31)/d,
332 -(e11*e23-e13*e21)/d,
334 -(e11*e32-e12*e31)/d,
335 (e11*e22-e12*e21)/d );
338 inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m)
352 inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m)
366 inline Matrix3x3& Matrix3x3::operator*=(float s)
380 inline Matrix3x3& Matrix3x3::operator/=(float s)
394 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2)
396 return Matrix3x3( m1.e11+m2.e11,
407 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2)
409 return Matrix3x3( m1.e11-m2.e11,
420 inline Matrix3x3 operator/(Matrix3x3 m, float s)
422 return Matrix3x3( m.e11/s,
433 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2)
435 return Matrix3x3( m1.e11*m2.e11 + m1.e12*m2.e21 + m1.e13*m2.e31,
436 m1.e11*m2.e12 + m1.e12*m2.e22 + m1.e13*m2.e32,
437 m1.e11*m2.e13 + m1.e12*m2.e23 + m1.e13*m2.e33,
438 m1.e21*m2.e11 + m1.e22*m2.e21 + m1.e23*m2.e31,
439 m1.e21*m2.e12 + m1.e22*m2.e22 + m1.e23*m2.e32,
440 m1.e21*m2.e13 + m1.e22*m2.e23 + m1.e23*m2.e33,
441 m1.e31*m2.e11 + m1.e32*m2.e21 + m1.e33*m2.e31,
442 m1.e31*m2.e12 + m1.e32*m2.e22 + m1.e33*m2.e32,
443 m1.e31*m2.e13 + m1.e32*m2.e23 + m1.e33*m2.e33 );
446 inline Matrix3x3 operator*(Matrix3x3 m, float s)
448 return Matrix3x3( m.e11*s,
459 inline Matrix3x3 operator*(float s, Matrix3x3 m)
461 return Matrix3x3( m.e11*s,
472 inline Vector operator*(Matrix3x3 m, Vector u)
474 return Vector( m.e11*u.x + m.e12*u.y + m.e13*u.z,
475 m.e21*u.x + m.e22*u.y + m.e23*u.z,
476 m.e31*u.x + m.e32*u.y + m.e33*u.z);
479 inline Vector operator*(Vector u, Matrix3x3 m)
481 return Vector( u.x*m.e11 + u.y*m.e21 + u.z*m.e31,
482 u.x*m.e12 + u.y*m.e22 + u.z*m.e32,
483 u.x*m.e13 + u.y*m.e23 + u.z*m.e33);
486 //------------------------------------------------------------------------//
487 // Quaternion Class and Quaternion functions
488 //------------------------------------------------------------------------//
492 float n; // number (scalar) part
493 Vector v; // vector part: v.x, v.y, v.z
496 Quaternion(float e0, float e1, float e2, float e3);
498 float Magnitude(void);
499 Vector GetVector(void);
500 float GetScalar(void);
501 Quaternion operator+=(Quaternion q);
502 Quaternion operator-=(Quaternion q);
503 Quaternion operator*=(float s);
504 Quaternion operator/=(float s);
505 Quaternion operator~(void) const { return Quaternion(n, -v.x, -v.y, -v.z);}
508 inline Quaternion operator+(Quaternion q1, Quaternion q2);
509 inline Quaternion operator-(Quaternion q1, Quaternion q2);
510 inline Quaternion operator*(Quaternion q1, Quaternion q2);
511 inline Quaternion operator*(Quaternion q, float s);
512 inline Quaternion operator*(float s, Quaternion q);
513 inline Quaternion operator*(Quaternion q, Vector v);
514 inline Quaternion operator*(Vector v, Quaternion q);
515 inline Quaternion operator/(Quaternion q, float s);
516 inline float QGetAngle(Quaternion q);
517 inline Vector QGetAxis(Quaternion q);
518 inline Quaternion QRotate(Quaternion q1, Quaternion q2);
519 inline Vector QVRotate(Quaternion q, Vector v);
520 inline Quaternion MakeQFromEulerAngles(float x, float y, float z);
521 inline Vector MakeEulerAnglesFromQ(Quaternion q);
524 inline Quaternion::Quaternion(void)
532 inline Quaternion::Quaternion(float e0, float e1, float e2, float e3)
540 inline float Quaternion::Magnitude(void)
542 return (float) sqrt(n*n + v.x*v.x + v.y*v.y + v.z*v.z);
545 inline Vector Quaternion::GetVector(void)
547 return Vector(v.x, v.y, v.z);
550 inline float Quaternion::GetScalar(void)
555 inline Quaternion Quaternion::operator+=(Quaternion q)
564 inline Quaternion Quaternion::operator-=(Quaternion q)
573 inline Quaternion Quaternion::operator*=(float s)
582 inline Quaternion Quaternion::operator/=(float s)
591 /*inline Quaternion Quaternion::operator~()
593 return Quaternion(n, -v.x, -v.y, -v.z);
596 inline Quaternion operator+(Quaternion q1, Quaternion q2)
598 return Quaternion( q1.n + q2.n,
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 - q1.v.x*q2.v.x - q1.v.y*q2.v.y - q1.v.z*q2.v.z,
615 q1.n*q2.v.x + q1.v.x*q2.n + q1.v.y*q2.v.z - q1.v.z*q2.v.y,
616 q1.n*q2.v.y + q1.v.y*q2.n + q1.v.z*q2.v.x - q1.v.x*q2.v.z,
617 q1.n*q2.v.z + q1.v.z*q2.n + q1.v.x*q2.v.y - q1.v.y*q2.v.x);
620 inline Quaternion operator*(Quaternion q, float s)
622 return Quaternion(q.n*s, q.v.x*s, q.v.y*s, q.v.z*s);
625 inline Quaternion operator*(float s, Quaternion q)
627 return Quaternion(q.n*s, q.v.x*s, q.v.y*s, q.v.z*s);
630 inline Quaternion operator*(Quaternion q, Vector v)
632 return Quaternion( -(q.v.x*v.x + q.v.y*v.y + q.v.z*v.z),
633 q.n*v.x + q.v.y*v.z - q.v.z*v.y,
634 q.n*v.y + q.v.z*v.x - q.v.x*v.z,
635 q.n*v.z + q.v.x*v.y - q.v.y*v.x);
638 inline Quaternion operator*(Vector v, Quaternion q)
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.z*v.y - q.v.y*v.z,
642 q.n*v.y + q.v.x*v.z - q.v.z*v.x,
643 q.n*v.z + q.v.y*v.x - q.v.x*v.y);
646 inline Quaternion operator/(Quaternion q, float s)
648 return Quaternion(q.n/s, q.v.x/s, q.v.y/s, q.v.z/s);
651 inline float QGetAngle(Quaternion q)
653 return (float) (2*acosf(q.n));
656 inline Vector QGetAxis(Quaternion q)
670 inline Quaternion QRotate(Quaternion q1, Quaternion q2)
675 inline Vector QVRotate(Quaternion q, Vector v)
682 return t.GetVector();
685 inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
688 double roll = DegreesToRadians(x);
689 double pitch = DegreesToRadians(y);
690 double yaw = DegreesToRadians(z);
692 double cyaw, cpitch, croll, syaw, spitch, sroll;
693 double cyawcpitch, syawspitch, cyawspitch, syawcpitch;
695 cyaw = cos(0.5f * yaw);
696 cpitch = cos(0.5f * pitch);
697 croll = cos(0.5f * roll);
698 syaw = sin(0.5f * yaw);
699 spitch = sin(0.5f * pitch);
700 sroll = sin(0.5f * roll);
702 cyawcpitch = cyaw*cpitch;
703 syawspitch = syaw*spitch;
704 cyawspitch = cyaw*spitch;
705 syawcpitch = syaw*cpitch;
707 q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
708 q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
709 q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
710 q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);
715 inline Vector MakeEulerAnglesFromQ(Quaternion q)
717 double r11, r21, r31, r32, r33, r12, r13;
718 double q00, q11, q22, q33;
727 r11 = q00 + q11 - q22 - q33;
728 r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z);
729 r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y);
730 r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x);
731 r33 = q00 - q11 - q22 + q33;
736 r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
737 r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);
739 u.x = RadiansToDegrees(0.0f); //roll
740 u.y = RadiansToDegrees((float) (-(pi/2) * r31/tmp)); // pitch
741 u.z = RadiansToDegrees((float) atan2(-r12, -r31*r13)); // yaw
745 u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll
746 u.y = RadiansToDegrees((float) asinf(-r31)); // pitch
747 u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw