1 #ifndef _PHYSICSMATH_H_
2 #define _PHYSICSMATH_H_
10 //------------------------------------------------------------------------//
12 //------------------------------------------------------------------------//
14 float const pi = 3.14159265f;
15 float const g = -32.174f; // acceleration due to gravity, ft/s^2
16 float const rho = 0.0023769f; // desity of air at sea level, slugs/ft^3
17 float const tol = 0.0000000001f; // float type tolerance
20 //------------------------------------------------------------------------//
22 //------------------------------------------------------------------------//
23 inline float DegreesToRadians(float deg);
24 inline float RadiansToDegrees(float rad);
26 inline float DegreesToRadians(float deg)
28 return deg * pi / 180.0f;
31 inline float RadiansToDegrees(float rad)
33 return rad * 180.0f / pi;
36 //------------------------------------------------------------------------//
37 // Vector Class and vector functions
38 //------------------------------------------------------------------------//
46 Vector(float xi, float yi, float zi);
48 float Magnitude(void);
52 Vector& operator+=(Vector u); // vector addition
53 Vector& operator-=(Vector u); // vector subtraction
54 Vector& operator*=(float s); // scalar multiply
55 Vector& operator/=(float s); // scalar divide
57 Vector operator-(void);
61 inline Vector operator+(Vector u, Vector v);
62 inline Vector operator-(Vector u, Vector v);
63 inline Vector operator^(Vector u, Vector v);
64 inline float operator*(Vector u, Vector v);
65 inline Vector operator*(float s, Vector u);
66 inline Vector operator*(Vector u, float s);
67 inline Vector operator/(Vector u, float s);
68 inline float TripleScalarProduct(Vector u, Vector v, Vector w);
70 float fast_sqrt2 (register float arg);
71 float fast_sqrt2 (register float arg)
73 // Can replace with slower return std::sqrt(arg);
74 register float result;
76 if (arg == 0.0) return 0.0;
79 frsqrte result,arg // Calculate Square root
82 // Newton Rhapson iterations.
83 result = result + 0.5 * result * (1.0 - arg * result * result);
84 result = result + 0.5 * result * (1.0 - arg * result * result);
89 inline Vector::Vector(void)
96 inline Vector::Vector(float xi, float yi, float zi)
103 inline float Vector::Magnitude(void)
105 return (float) sqrt(x*x + y*y + z*z);
108 inline void Vector::Normalize(void)
110 float m = (float) sqrt(x*x + y*y + z*z);
116 if (fabs(x) < tol) x = 0.0f;
117 if (fabs(y) < tol) y = 0.0f;
118 if (fabs(z) < tol) z = 0.0f;
121 inline void Vector::Reverse(void)
128 inline Vector& Vector::operator+=(Vector u)
136 inline Vector& Vector::operator-=(Vector u)
144 inline Vector& Vector::operator*=(float s)
152 inline Vector& Vector::operator/=(float s)
160 inline Vector Vector::operator-(void)
162 return Vector(-x, -y, -z);
166 inline Vector operator+(Vector u, Vector v)
168 return Vector(u.x + v.x, u.y + v.y, u.z + v.z);
171 inline Vector operator-(Vector u, Vector v)
173 return Vector(u.x - v.x, u.y - v.y, u.z - v.z);
176 // Vector cross product (u cross v)
177 inline Vector operator^(Vector u, Vector v)
179 return Vector( u.y*v.z - u.z*v.y,
184 // Vector dot product
185 inline float operator*(Vector u, Vector v)
187 return (u.x*v.x + u.y*v.y + u.z*v.z);
190 inline Vector operator*(float s, Vector u)
192 return Vector(u.x*s, u.y*s, u.z*s);
195 inline Vector operator*(Vector u, float s)
197 return Vector(u.x*s, u.y*s, u.z*s);
200 inline Vector operator/(Vector u, float s)
202 return Vector(u.x/s, u.y/s, u.z/s);
205 // triple scalar product (u dot (v cross w))
206 inline float TripleScalarProduct(Vector u, Vector v, Vector w)
208 return float( (u.x * (v.y*w.z - v.z*w.y)) +
209 (u.y * (-v.x*w.z + v.z*w.x)) +
210 (u.z * (v.x*w.y - v.y*w.x)) );
217 //------------------------------------------------------------------------//
218 // Matrix Class and matrix functions
219 //------------------------------------------------------------------------//
223 // elements eij: i -> row, j -> column
224 float e11, e12, e13, e21, e22, e23, e31, e32, e33;
227 Matrix3x3( float r1c1, float r1c2, float r1c3,
228 float r2c1, float r2c2, float r2c3,
229 float r3c1, float r3c2, float r3c3 );
232 Matrix3x3 Transpose(void);
233 Matrix3x3 Inverse(void);
235 Matrix3x3& operator+=(Matrix3x3 m);
236 Matrix3x3& operator-=(Matrix3x3 m);
237 Matrix3x3& operator*=(float s);
238 Matrix3x3& operator/=(float s);
241 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2);
242 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2);
243 inline Matrix3x3 operator/(Matrix3x3 m, float s);
244 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2);
245 inline Matrix3x3 operator*(Matrix3x3 m, float s);
246 inline Matrix3x3 operator*(float s, Matrix3x3 m);
247 inline Vector operator*(Matrix3x3 m, Vector u);
248 inline Vector operator*(Vector u, Matrix3x3 m);
254 inline Matrix3x3::Matrix3x3(void)
267 inline Matrix3x3::Matrix3x3( float r1c1, float r1c2, float r1c3,
268 float r2c1, float r2c2, float r2c3,
269 float r3c1, float r3c2, float r3c3 )
282 inline float Matrix3x3::det(void)
292 inline Matrix3x3 Matrix3x3::Transpose(void)
294 return Matrix3x3(e11,e21,e31,e12,e22,e32,e13,e23,e33);
297 inline Matrix3x3 Matrix3x3::Inverse(void)
299 float d = e11*e22*e33 -
308 return Matrix3x3( (e22*e33-e23*e32)/d,
309 -(e12*e33-e13*e32)/d,
311 -(e21*e33-e23*e31)/d,
313 -(e11*e23-e13*e21)/d,
315 -(e11*e32-e12*e31)/d,
316 (e11*e22-e12*e21)/d );
319 inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m)
333 inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m)
347 inline Matrix3x3& Matrix3x3::operator*=(float s)
361 inline Matrix3x3& Matrix3x3::operator/=(float s)
375 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2)
377 return Matrix3x3( m1.e11+m2.e11,
388 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2)
390 return Matrix3x3( m1.e11-m2.e11,
401 inline Matrix3x3 operator/(Matrix3x3 m, float s)
403 return Matrix3x3( m.e11/s,
414 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2)
416 return Matrix3x3( m1.e11*m2.e11 + m1.e12*m2.e21 + m1.e13*m2.e31,
417 m1.e11*m2.e12 + m1.e12*m2.e22 + m1.e13*m2.e32,
418 m1.e11*m2.e13 + m1.e12*m2.e23 + m1.e13*m2.e33,
419 m1.e21*m2.e11 + m1.e22*m2.e21 + m1.e23*m2.e31,
420 m1.e21*m2.e12 + m1.e22*m2.e22 + m1.e23*m2.e32,
421 m1.e21*m2.e13 + m1.e22*m2.e23 + m1.e23*m2.e33,
422 m1.e31*m2.e11 + m1.e32*m2.e21 + m1.e33*m2.e31,
423 m1.e31*m2.e12 + m1.e32*m2.e22 + m1.e33*m2.e32,
424 m1.e31*m2.e13 + m1.e32*m2.e23 + m1.e33*m2.e33 );
427 inline Matrix3x3 operator*(Matrix3x3 m, float s)
429 return Matrix3x3( m.e11*s,
440 inline Matrix3x3 operator*(float s, Matrix3x3 m)
442 return Matrix3x3( m.e11*s,
453 inline Vector operator*(Matrix3x3 m, Vector u)
455 return Vector( m.e11*u.x + m.e12*u.y + m.e13*u.z,
456 m.e21*u.x + m.e22*u.y + m.e23*u.z,
457 m.e31*u.x + m.e32*u.y + m.e33*u.z);
460 inline Vector operator*(Vector u, Matrix3x3 m)
462 return Vector( u.x*m.e11 + u.y*m.e21 + u.z*m.e31,
463 u.x*m.e12 + u.y*m.e22 + u.z*m.e32,
464 u.x*m.e13 + u.y*m.e23 + u.z*m.e33);
467 //------------------------------------------------------------------------//
468 // Quaternion Class and Quaternion functions
469 //------------------------------------------------------------------------//
473 float n; // number (scalar) part
474 Vector v; // vector part: v.x, v.y, v.z
477 Quaternion(float e0, float e1, float e2, float e3);
479 float Magnitude(void);
480 Vector GetVector(void);
481 float GetScalar(void);
482 Quaternion operator+=(Quaternion q);
483 Quaternion operator-=(Quaternion q);
484 Quaternion operator*=(float s);
485 Quaternion operator/=(float s);
486 Quaternion operator~(void) const { return Quaternion(n, -v.x, -v.y, -v.z);}
489 inline Quaternion operator+(Quaternion q1, Quaternion q2);
490 inline Quaternion operator-(Quaternion q1, Quaternion q2);
491 inline Quaternion operator*(Quaternion q1, Quaternion q2);
492 inline Quaternion operator*(Quaternion q, float s);
493 inline Quaternion operator*(float s, Quaternion q);
494 inline Quaternion operator*(Quaternion q, Vector v);
495 inline Quaternion operator*(Vector v, Quaternion q);
496 inline Quaternion operator/(Quaternion q, float s);
497 inline float QGetAngle(Quaternion q);
498 inline Vector QGetAxis(Quaternion q);
499 inline Quaternion QRotate(Quaternion q1, Quaternion q2);
500 inline Vector QVRotate(Quaternion q, Vector v);
501 inline Quaternion MakeQFromEulerAngles(float x, float y, float z);
502 inline Vector MakeEulerAnglesFromQ(Quaternion q);
505 inline Quaternion::Quaternion(void)
513 inline Quaternion::Quaternion(float e0, float e1, float e2, float e3)
521 inline float Quaternion::Magnitude(void)
523 return (float) sqrt(n*n + v.x*v.x + v.y*v.y + v.z*v.z);
526 inline Vector Quaternion::GetVector(void)
528 return Vector(v.x, v.y, v.z);
531 inline float Quaternion::GetScalar(void)
536 inline Quaternion Quaternion::operator+=(Quaternion q)
545 inline Quaternion Quaternion::operator-=(Quaternion q)
554 inline Quaternion Quaternion::operator*=(float s)
563 inline Quaternion Quaternion::operator/=(float s)
572 /*inline Quaternion Quaternion::operator~()
574 return Quaternion(n, -v.x, -v.y, -v.z);
577 inline Quaternion operator+(Quaternion q1, Quaternion q2)
579 return Quaternion( q1.n + q2.n,
585 inline Quaternion operator-(Quaternion q1, Quaternion q2)
587 return Quaternion( q1.n - q2.n,
593 inline Quaternion operator*(Quaternion q1, Quaternion q2)
595 return Quaternion( q1.n*q2.n - q1.v.x*q2.v.x - q1.v.y*q2.v.y - q1.v.z*q2.v.z,
596 q1.n*q2.v.x + q1.v.x*q2.n + q1.v.y*q2.v.z - q1.v.z*q2.v.y,
597 q1.n*q2.v.y + q1.v.y*q2.n + q1.v.z*q2.v.x - q1.v.x*q2.v.z,
598 q1.n*q2.v.z + q1.v.z*q2.n + q1.v.x*q2.v.y - q1.v.y*q2.v.x);
601 inline Quaternion operator*(Quaternion q, float s)
603 return Quaternion(q.n*s, q.v.x*s, q.v.y*s, q.v.z*s);
606 inline Quaternion operator*(float s, Quaternion q)
608 return Quaternion(q.n*s, q.v.x*s, q.v.y*s, q.v.z*s);
611 inline Quaternion operator*(Quaternion q, Vector v)
613 return Quaternion( -(q.v.x*v.x + q.v.y*v.y + q.v.z*v.z),
614 q.n*v.x + q.v.y*v.z - q.v.z*v.y,
615 q.n*v.y + q.v.z*v.x - q.v.x*v.z,
616 q.n*v.z + q.v.x*v.y - q.v.y*v.x);
619 inline Quaternion operator*(Vector v, Quaternion q)
621 return Quaternion( -(q.v.x*v.x + q.v.y*v.y + q.v.z*v.z),
622 q.n*v.x + q.v.z*v.y - q.v.y*v.z,
623 q.n*v.y + q.v.x*v.z - q.v.z*v.x,
624 q.n*v.z + q.v.y*v.x - q.v.x*v.y);
627 inline Quaternion operator/(Quaternion q, float s)
629 return Quaternion(q.n/s, q.v.x/s, q.v.y/s, q.v.z/s);
632 inline float QGetAngle(Quaternion q)
634 return (float) (2*acosf(q.n));
637 inline Vector QGetAxis(Quaternion q)
651 inline Quaternion QRotate(Quaternion q1, Quaternion q2)
656 inline Vector QVRotate(Quaternion q, Vector v)
663 return t.GetVector();
666 inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
669 double roll = DegreesToRadians(x);
670 double pitch = DegreesToRadians(y);
671 double yaw = DegreesToRadians(z);
673 double cyaw, cpitch, croll, syaw, spitch, sroll;
674 double cyawcpitch, syawspitch, cyawspitch, syawcpitch;
676 cyaw = cos(0.5f * yaw);
677 cpitch = cos(0.5f * pitch);
678 croll = cos(0.5f * roll);
679 syaw = sin(0.5f * yaw);
680 spitch = sin(0.5f * pitch);
681 sroll = sin(0.5f * roll);
683 cyawcpitch = cyaw*cpitch;
684 syawspitch = syaw*spitch;
685 cyawspitch = cyaw*spitch;
686 syawcpitch = syaw*cpitch;
688 q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
689 q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
690 q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
691 q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);
696 inline Vector MakeEulerAnglesFromQ(Quaternion q)
698 double r11, r21, r31, r32, r33, r12, r13;
699 double q00, q11, q22, q33;
708 r11 = q00 + q11 - q22 - q33;
709 r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z);
710 r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y);
711 r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x);
712 r33 = q00 - q11 - q22 + q33;
717 r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
718 r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);
720 u.x = RadiansToDegrees(0.0f); //roll
721 u.y = RadiansToDegrees((float) (-(pi/2) * r31/tmp)); // pitch
722 u.z = RadiansToDegrees((float) atan2(-r12, -r31*r13)); // yaw
726 u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll
727 u.y = RadiansToDegrees((float) asinf(-r31)); // pitch
728 u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw