X-Git-Url: https://git.jsancho.org/?a=blobdiff_plain;f=Source%2FPhysicsMath.h;h=0777f38c26d654b470f272c541024630bb3d5adb;hb=25e3d5e35a7fd7efdffc420efe3c41a91cefe43f;hp=fdf7d40c7fc2528a510657c658a45abe6c73711c;hpb=87ff660bf58766ab705a749e72e04154ccf5a737;p=lugaru.git diff --git a/Source/PhysicsMath.h b/Source/PhysicsMath.h index fdf7d40..0777f38 100644 --- a/Source/PhysicsMath.h +++ b/Source/PhysicsMath.h @@ -30,24 +30,24 @@ Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. // Misc. Constants //------------------------------------------------------------------------// -float const pi = 3.14159265f; -float const g = -32.174f; // acceleration due to gravity, ft/s^2 -float const rho = 0.0023769f; // desity of air at sea level, slugs/ft^3 -float const tol = 0.0000000001f; // float type tolerance +float const pi = 3.14159265f; +float const g = -32.174f; // acceleration due to gravity, ft/s^2 +float const rho = 0.0023769f; // desity of air at sea level, slugs/ft^3 +float const tol = 0.0000000001f; // float type tolerance //------------------------------------------------------------------------// // Misc. Functions //------------------------------------------------------------------------// -inline float DegreesToRadians(float deg); -inline float RadiansToDegrees(float rad); +inline float DegreesToRadians(float deg); +inline float RadiansToDegrees(float rad); -inline float DegreesToRadians(float deg) +inline float DegreesToRadians(float deg) { return deg * pi / 180.0f; } -inline float RadiansToDegrees(float rad) +inline float RadiansToDegrees(float rad) { return rad * 180.0f / pi; } @@ -69,23 +69,23 @@ public: void Normalize(void); void Reverse(void); - Vector& operator+=(Vector u); // vector addition - Vector& operator-=(Vector u); // vector subtraction - Vector& operator*=(float s); // scalar multiply - Vector& operator/=(float s); // scalar divide + Vector& operator+=(Vector u); // vector addition + Vector& operator-=(Vector u); // vector subtraction + Vector& operator*=(float s); // scalar multiply + Vector& operator/=(float s); // scalar divide Vector operator-(void); }; -inline Vector operator+(Vector u, Vector v); -inline Vector operator-(Vector u, Vector v); -inline Vector operator^(Vector u, Vector v); -inline float operator*(Vector u, Vector v); -inline Vector operator*(float s, Vector u); -inline Vector operator*(Vector u, float s); -inline Vector operator/(Vector u, float s); -inline float TripleScalarProduct(Vector u, Vector v, Vector w); +inline Vector operator+(Vector u, Vector v); +inline Vector operator-(Vector u, Vector v); +inline Vector operator^(Vector u, Vector v); +inline float operator*(Vector u, Vector v); +inline Vector operator*(float s, Vector u); +inline Vector operator*(Vector u, float s); +inline Vector operator/(Vector u, float s); +inline float TripleScalarProduct(Vector u, Vector v, Vector w); /* float fast_sqrt2 (register float arg); float fast_sqrt2 (register float arg) @@ -96,7 +96,7 @@ register float result; if (arg == 0.0) return 0.0; asm { -frsqrte result,arg // Calculate Square root +frsqrte result,arg // Calculate Square root } // Newton Rhapson iterations. @@ -120,12 +120,12 @@ inline Vector::Vector(float xi, float yi, float zi) z = zi; } -inline float Vector::Magnitude(void) +inline float Vector::Magnitude(void) { return (float) sqrt(x * x + y * y + z * z); } -inline void Vector::Normalize(void) +inline void Vector::Normalize(void) { float m = (float) sqrt(x * x + y * y + z * z); if (m <= tol) @@ -142,7 +142,7 @@ inline void Vector::Normalize(void) z = 0.0f; } -inline void Vector::Reverse(void) +inline void Vector::Reverse(void) { x = -x; y = -y; @@ -157,7 +157,7 @@ inline Vector& Vector::operator+=(Vector u) return *this; } -inline Vector& Vector::operator-=(Vector u) +inline Vector& Vector::operator-=(Vector u) { x -= u.x; y -= u.y; @@ -165,7 +165,7 @@ inline Vector& Vector::operator-=(Vector u) return *this; } -inline Vector& Vector::operator*=(float s) +inline Vector& Vector::operator*=(float s) { x *= s; y *= s; @@ -173,7 +173,7 @@ inline Vector& Vector::operator*=(float s) return *this; } -inline Vector& Vector::operator/=(float s) +inline Vector& Vector::operator/=(float s) { x /= s; y /= s; @@ -181,55 +181,55 @@ inline Vector& Vector::operator/=(float s) return *this; } -inline Vector Vector::operator-(void) +inline Vector Vector::operator-(void) { return Vector(-x, -y, -z); } -inline Vector operator+(Vector u, Vector v) +inline Vector operator+(Vector u, Vector v) { return Vector(u.x + v.x, u.y + v.y, u.z + v.z); } -inline Vector operator-(Vector u, Vector v) +inline Vector operator-(Vector u, Vector v) { return Vector(u.x - v.x, u.y - v.y, u.z - v.z); } // Vector cross product (u cross v) -inline Vector operator^(Vector u, Vector v) +inline Vector operator^(Vector u, Vector v) { - return Vector( u.y * v.z - u.z * v.y, + return Vector( u.y * v.z - u.z * v.y, -u.x * v.z + u.z * v.x, u.x * v.y - u.y * v.x ); } // Vector dot product -inline float operator*(Vector u, Vector v) +inline float operator*(Vector u, Vector v) { return (u.x * v.x + u.y * v.y + u.z * v.z); } -inline Vector operator*(float s, Vector u) +inline Vector operator*(float s, Vector u) { return Vector(u.x * s, u.y * s, u.z * s); } -inline Vector operator*(Vector u, float s) +inline Vector operator*(Vector u, float s) { return Vector(u.x * s, u.y * s, u.z * s); } -inline Vector operator/(Vector u, float s) +inline Vector operator/(Vector u, float s) { return Vector(u.x / s, u.y / s, u.z / s); } // triple scalar product (u dot (v cross w)) -inline float TripleScalarProduct(Vector u, Vector v, Vector w) +inline float TripleScalarProduct(Vector u, Vector v, Vector w) { - return float( (u.x * (v.y * w.z - v.z * w.y)) + + return float( (u.x * (v.y * w.z - v.z * w.y)) + (u.y * (-v.x * w.z + v.z * w.x)) + (u.z * (v.x * w.y - v.y * w.x)) ); //return u*(v^w); @@ -246,16 +246,16 @@ class Matrix3x3 { public: // elements eij: i -> row, j -> column - float e11, e12, e13, e21, e22, e23, e31, e32, e33; + float e11, e12, e13, e21, e22, e23, e31, e32, e33; Matrix3x3(void); - Matrix3x3( float r1c1, float r1c2, float r1c3, + Matrix3x3( float r1c1, float r1c2, float r1c3, float r2c1, float r2c2, float r2c3, float r3c1, float r3c2, float r3c3 ); - float det(void); - Matrix3x3 Transpose(void); - Matrix3x3 Inverse(void); + float det(void); + Matrix3x3 Transpose(void); + Matrix3x3 Inverse(void); Matrix3x3& operator+=(Matrix3x3 m); Matrix3x3& operator-=(Matrix3x3 m); @@ -263,20 +263,20 @@ public: Matrix3x3& operator/=(float s); }; -inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2); -inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2); -inline Matrix3x3 operator/(Matrix3x3 m, float s); -inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2); -inline Matrix3x3 operator*(Matrix3x3 m, float s); -inline Matrix3x3 operator*(float s, Matrix3x3 m); -inline Vector operator*(Matrix3x3 m, Vector u); -inline Vector operator*(Vector u, Matrix3x3 m); +inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2); +inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2); +inline Matrix3x3 operator/(Matrix3x3 m, float s); +inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2); +inline Matrix3x3 operator*(Matrix3x3 m, float s); +inline Matrix3x3 operator*(float s, Matrix3x3 m); +inline Vector operator*(Matrix3x3 m, Vector u); +inline Vector operator*(Vector u, Matrix3x3 m); -inline Matrix3x3::Matrix3x3(void) +inline Matrix3x3::Matrix3x3(void) { e11 = 0; e12 = 0; @@ -289,7 +289,7 @@ inline Matrix3x3::Matrix3x3(void) e33 = 0; } -inline Matrix3x3::Matrix3x3( float r1c1, float r1c2, float r1c3, +inline Matrix3x3::Matrix3x3( float r1c1, float r1c2, float r1c3, float r2c1, float r2c2, float r2c3, float r3c1, float r3c2, float r3c3 ) { @@ -304,9 +304,9 @@ inline Matrix3x3::Matrix3x3( float r1c1, float r1c2, float r1c3, e33 = r3c3; } -inline float Matrix3x3::det(void) +inline float Matrix3x3::det(void) { - return e11 * e22 * e33 - + return e11 * e22 * e33 - e11 * e32 * e23 + e21 * e32 * e13 - e21 * e12 * e33 + @@ -314,14 +314,14 @@ inline float Matrix3x3::det(void) e31 * e22 * e13; } -inline Matrix3x3 Matrix3x3::Transpose(void) +inline Matrix3x3 Matrix3x3::Transpose(void) { return Matrix3x3(e11, e21, e31, e12, e22, e32, e13, e23, e33); } -inline Matrix3x3 Matrix3x3::Inverse(void) +inline Matrix3x3 Matrix3x3::Inverse(void) { - float d = e11 * e22 * e33 - + float d = e11 * e22 * e33 - e11 * e32 * e23 + e21 * e32 * e13 - e21 * e12 * e33 + @@ -331,7 +331,7 @@ inline Matrix3x3 Matrix3x3::Inverse(void) if (d == 0) d = 1; - return Matrix3x3( (e22 * e33 - e23 * e32) / d, + return Matrix3x3( (e22 * e33 - e23 * e32) / d, -(e12 * e33 - e13 * e32) / d, (e12 * e23 - e13 * e22) / d, -(e21 * e33 - e23 * e31) / d, @@ -342,7 +342,7 @@ inline Matrix3x3 Matrix3x3::Inverse(void) (e11 * e22 - e12 * e21) / d ); } -inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m) +inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m) { e11 += m.e11; e12 += m.e12; @@ -356,7 +356,7 @@ inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m) return *this; } -inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m) +inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m) { e11 -= m.e11; e12 -= m.e12; @@ -370,7 +370,7 @@ inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m) return *this; } -inline Matrix3x3& Matrix3x3::operator*=(float s) +inline Matrix3x3& Matrix3x3::operator*=(float s) { e11 *= s; e12 *= s; @@ -384,7 +384,7 @@ inline Matrix3x3& Matrix3x3::operator*=(float s) return *this; } -inline Matrix3x3& Matrix3x3::operator/=(float s) +inline Matrix3x3& Matrix3x3::operator/=(float s) { e11 /= s; e12 /= s; @@ -398,9 +398,9 @@ inline Matrix3x3& Matrix3x3::operator/=(float s) return *this; } -inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2) +inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2) { - return Matrix3x3( m1.e11 + m2.e11, + return Matrix3x3( m1.e11 + m2.e11, m1.e12 + m2.e12, m1.e13 + m2.e13, m1.e21 + m2.e21, @@ -411,9 +411,9 @@ inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2) m1.e33 + m2.e33); } -inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2) +inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2) { - return Matrix3x3( m1.e11 - m2.e11, + return Matrix3x3( m1.e11 - m2.e11, m1.e12 - m2.e12, m1.e13 - m2.e13, m1.e21 - m2.e21, @@ -424,9 +424,9 @@ inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2) m1.e33 - m2.e33); } -inline Matrix3x3 operator/(Matrix3x3 m, float s) +inline Matrix3x3 operator/(Matrix3x3 m, float s) { - return Matrix3x3( m.e11 / s, + return Matrix3x3( m.e11 / s, m.e12 / s, m.e13 / s, m.e21 / s, @@ -437,9 +437,9 @@ inline Matrix3x3 operator/(Matrix3x3 m, float s) m.e33 / s); } -inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2) +inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2) { - return Matrix3x3( m1.e11 * m2.e11 + m1.e12 * m2.e21 + m1.e13 * m2.e31, + return Matrix3x3( m1.e11 * m2.e11 + m1.e12 * m2.e21 + m1.e13 * m2.e31, m1.e11 * m2.e12 + m1.e12 * m2.e22 + m1.e13 * m2.e32, m1.e11 * m2.e13 + m1.e12 * m2.e23 + m1.e13 * m2.e33, m1.e21 * m2.e11 + m1.e22 * m2.e21 + m1.e23 * m2.e31, @@ -450,9 +450,9 @@ inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2) m1.e31 * m2.e13 + m1.e32 * m2.e23 + m1.e33 * m2.e33 ); } -inline Matrix3x3 operator*(Matrix3x3 m, float s) +inline Matrix3x3 operator*(Matrix3x3 m, float s) { - return Matrix3x3( m.e11 * s, + return Matrix3x3( m.e11 * s, m.e12 * s, m.e13 * s, m.e21 * s, @@ -463,9 +463,9 @@ inline Matrix3x3 operator*(Matrix3x3 m, float s) m.e33 * s); } -inline Matrix3x3 operator*(float s, Matrix3x3 m) +inline Matrix3x3 operator*(float s, Matrix3x3 m) { - return Matrix3x3( m.e11 * s, + return Matrix3x3( m.e11 * s, m.e12 * s, m.e13 * s, m.e21 * s, @@ -476,16 +476,16 @@ inline Matrix3x3 operator*(float s, Matrix3x3 m) m.e33 * s); } -inline Vector operator*(Matrix3x3 m, Vector u) +inline Vector operator*(Matrix3x3 m, Vector u) { - return Vector( m.e11 * u.x + m.e12 * u.y + m.e13 * u.z, + return Vector( m.e11 * u.x + m.e12 * u.y + m.e13 * u.z, m.e21 * u.x + m.e22 * u.y + m.e23 * u.z, m.e31 * u.x + m.e32 * u.y + m.e33 * u.z); } -inline Vector operator*(Vector u, Matrix3x3 m) +inline Vector operator*(Vector u, Matrix3x3 m) { - return Vector( u.x * m.e11 + u.y * m.e21 + u.z * m.e31, + return Vector( u.x * m.e11 + u.y * m.e21 + u.z * m.e31, u.x * m.e12 + u.y * m.e22 + u.z * m.e32, u.x * m.e13 + u.y * m.e23 + u.z * m.e33); } @@ -497,41 +497,41 @@ inline Vector operator*(Vector u, Matrix3x3 m) class Quaternion { public: - float n; // number (scalar) part - Vector v; // vector part: v.x, v.y, v.z + float n; // number (scalar) part + Vector v; // vector part: v.x, v.y, v.z Quaternion(void); Quaternion(float e0, float e1, float e2, float e3); - float Magnitude(void); - Vector GetVector(void); - float GetScalar(void); - Quaternion operator+=(Quaternion q); - Quaternion operator-=(Quaternion q); + float Magnitude(void); + Vector GetVector(void); + float GetScalar(void); + Quaternion operator+=(Quaternion q); + Quaternion operator-=(Quaternion q); Quaternion operator*=(float s); Quaternion operator/=(float s); - Quaternion operator~(void) const { + Quaternion operator~(void) const { return Quaternion(n, -v.x, -v.y, -v.z); } }; -inline Quaternion operator+(Quaternion q1, Quaternion q2); -inline Quaternion operator-(Quaternion q1, Quaternion q2); -inline Quaternion operator*(Quaternion q1, Quaternion q2); -inline Quaternion operator*(Quaternion q, float s); -inline Quaternion operator*(float s, Quaternion q); -inline Quaternion operator*(Quaternion q, Vector v); -inline Quaternion operator*(Vector v, Quaternion q); -inline Quaternion operator/(Quaternion q, float s); -inline float QGetAngle(Quaternion q); -inline Vector QGetAxis(Quaternion q); -inline Quaternion QRotate(Quaternion q1, Quaternion q2); -inline Vector QVRotate(Quaternion q, Vector v); -inline Quaternion MakeQFromEulerAngles(float x, float y, float z); -inline Vector MakeEulerAnglesFromQ(Quaternion q); +inline Quaternion operator+(Quaternion q1, Quaternion q2); +inline Quaternion operator-(Quaternion q1, Quaternion q2); +inline Quaternion operator*(Quaternion q1, Quaternion q2); +inline Quaternion operator*(Quaternion q, float s); +inline Quaternion operator*(float s, Quaternion q); +inline Quaternion operator*(Quaternion q, Vector v); +inline Quaternion operator*(Vector v, Quaternion q); +inline Quaternion operator/(Quaternion q, float s); +inline float QGetAngle(Quaternion q); +inline Vector QGetAxis(Quaternion q); +inline Quaternion QRotate(Quaternion q1, Quaternion q2); +inline Vector QVRotate(Quaternion q, Vector v); +inline Quaternion MakeQFromEulerAngles(float x, float y, float z); +inline Vector MakeEulerAnglesFromQ(Quaternion q); -inline Quaternion::Quaternion(void) +inline Quaternion::Quaternion(void) { n = 0; v.x = 0; @@ -539,7 +539,7 @@ inline Quaternion::Quaternion(void) v.z = 0; } -inline Quaternion::Quaternion(float e0, float e1, float e2, float e3) +inline Quaternion::Quaternion(float e0, float e1, float e2, float e3) { n = e0; v.x = e1; @@ -547,22 +547,22 @@ inline Quaternion::Quaternion(float e0, float e1, float e2, float e3) v.z = e3; } -inline float Quaternion::Magnitude(void) +inline float Quaternion::Magnitude(void) { return (float) sqrt(n * n + v.x * v.x + v.y * v.y + v.z * v.z); } -inline Vector Quaternion::GetVector(void) +inline Vector Quaternion::GetVector(void) { return Vector(v.x, v.y, v.z); } -inline float Quaternion::GetScalar(void) +inline float Quaternion::GetScalar(void) { return n; } -inline Quaternion Quaternion::operator+=(Quaternion q) +inline Quaternion Quaternion::operator+=(Quaternion q) { n += q.n; v.x += q.v.x; @@ -571,7 +571,7 @@ inline Quaternion Quaternion::operator+=(Quaternion q) return *this; } -inline Quaternion Quaternion::operator-=(Quaternion q) +inline Quaternion Quaternion::operator-=(Quaternion q) { n -= q.n; v.x -= q.v.x; @@ -580,7 +580,7 @@ inline Quaternion Quaternion::operator-=(Quaternion q) return *this; } -inline Quaternion Quaternion::operator*=(float s) +inline Quaternion Quaternion::operator*=(float s) { n *= s; v.x *= s; @@ -589,7 +589,7 @@ inline Quaternion Quaternion::operator*=(float s) return *this; } -inline Quaternion Quaternion::operator/=(float s) +inline Quaternion Quaternion::operator/=(float s) { n /= s; v.x /= s; @@ -598,72 +598,72 @@ inline Quaternion Quaternion::operator/=(float s) return *this; } -/*inline Quaternion Quaternion::operator~() +/*inline Quaternion Quaternion::operator~() { return Quaternion(n, -v.x, -v.y, -v.z); }*/ -inline Quaternion operator+(Quaternion q1, Quaternion q2) +inline Quaternion operator+(Quaternion q1, Quaternion q2) { - return Quaternion( q1.n + q2.n, + return Quaternion( q1.n + q2.n, q1.v.x + q2.v.x, q1.v.y + q2.v.y, q1.v.z + q2.v.z); } -inline Quaternion operator-(Quaternion q1, Quaternion q2) +inline Quaternion operator-(Quaternion q1, Quaternion q2) { - return Quaternion( q1.n - q2.n, + return Quaternion( q1.n - q2.n, q1.v.x - q2.v.x, q1.v.y - q2.v.y, q1.v.z - q2.v.z); } -inline Quaternion operator*(Quaternion q1, Quaternion q2) +inline Quaternion operator*(Quaternion q1, Quaternion q2) { - return Quaternion( q1.n * q2.n - q1.v.x * q2.v.x - q1.v.y * q2.v.y - q1.v.z * q2.v.z, + return Quaternion( q1.n * q2.n - q1.v.x * q2.v.x - q1.v.y * q2.v.y - q1.v.z * q2.v.z, q1.n * q2.v.x + q1.v.x * q2.n + q1.v.y * q2.v.z - q1.v.z * q2.v.y, q1.n * q2.v.y + q1.v.y * q2.n + q1.v.z * q2.v.x - q1.v.x * q2.v.z, q1.n * q2.v.z + q1.v.z * q2.n + q1.v.x * q2.v.y - q1.v.y * q2.v.x); } -inline Quaternion operator*(Quaternion q, float s) +inline Quaternion operator*(Quaternion q, float s) { - return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s); + return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s); } -inline Quaternion operator*(float s, Quaternion q) +inline Quaternion operator*(float s, Quaternion q) { - return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s); + return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s); } -inline Quaternion operator*(Quaternion q, Vector v) +inline Quaternion operator*(Quaternion q, Vector v) { - return Quaternion( -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z), + return Quaternion( -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z), q.n * v.x + q.v.y * v.z - q.v.z * v.y, q.n * v.y + q.v.z * v.x - q.v.x * v.z, q.n * v.z + q.v.x * v.y - q.v.y * v.x); } -inline Quaternion operator*(Vector v, Quaternion q) +inline Quaternion operator*(Vector v, Quaternion q) { - return Quaternion( -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z), + return Quaternion( -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z), q.n * v.x + q.v.z * v.y - q.v.y * v.z, q.n * v.y + q.v.x * v.z - q.v.z * v.x, q.n * v.z + q.v.y * v.x - q.v.x * v.y); } -inline Quaternion operator/(Quaternion q, float s) +inline Quaternion operator/(Quaternion q, float s) { - return Quaternion(q.n / s, q.v.x / s, q.v.y / s, q.v.z / s); + return Quaternion(q.n / s, q.v.x / s, q.v.y / s, q.v.z / s); } -inline float QGetAngle(Quaternion q) +inline float QGetAngle(Quaternion q) { - return (float) (2 * acosf(q.n)); + return (float) (2 * acosf(q.n)); } -inline Vector QGetAxis(Quaternion q) +inline Vector QGetAxis(Quaternion q) { Vector v; float m; @@ -677,30 +677,30 @@ inline Vector QGetAxis(Quaternion q) return v / m; } -inline Quaternion QRotate(Quaternion q1, Quaternion q2) +inline Quaternion QRotate(Quaternion q1, Quaternion q2) { - return q1 * q2 * (~q1); + return q1 * q2 * (~q1); } -inline Vector QVRotate(Quaternion q, Vector v) +inline Vector QVRotate(Quaternion q, Vector v) { Quaternion t; t = q * v * (~q); - return t.GetVector(); + return t.GetVector(); } -inline Quaternion MakeQFromEulerAngles(float x, float y, float z) +inline Quaternion MakeQFromEulerAngles(float x, float y, float z) { - Quaternion q; - double roll = DegreesToRadians(x); - double pitch = DegreesToRadians(y); - double yaw = DegreesToRadians(z); + Quaternion q; + double roll = DegreesToRadians(x); + double pitch = DegreesToRadians(y); + double yaw = DegreesToRadians(z); - double cyaw, cpitch, croll, syaw, spitch, sroll; - double cyawcpitch, syawspitch, cyawspitch, syawcpitch; + double cyaw, cpitch, croll, syaw, spitch, sroll; + double cyawcpitch, syawspitch, cyawspitch, syawcpitch; cyaw = cos(0.5f * yaw); cpitch = cos(0.5f * pitch); @@ -722,12 +722,12 @@ inline Quaternion MakeQFromEulerAngles(float x, float y, float z) return q; } -inline Vector MakeEulerAnglesFromQ(Quaternion q) +inline Vector MakeEulerAnglesFromQ(Quaternion q) { - double r11, r21, r31, r32, r33; - double q00, q11, q22, q33; - double tmp; - Vector u; + double r11, r21, r31, r32, r33; + double q00, q11, q22, q33; + double tmp; + Vector u; q00 = q.n * q.n; q11 = q.v.x * q.v.x; @@ -752,7 +752,7 @@ inline Vector MakeEulerAnglesFromQ(Quaternion q) } u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll - u.y = RadiansToDegrees((float) asinf(-r31)); // pitch + u.y = RadiansToDegrees((float) asinf(-r31)); // pitch u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw return u;