// 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;
}
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)
if (arg == 0.0) return 0.0;
asm {
-frsqrte result,arg // Calculate Square root
+frsqrte result,arg // Calculate Square root
}
// Newton Rhapson iterations.
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)
z = 0.0f;
}
-inline void Vector::Reverse(void)
+inline void Vector::Reverse(void)
{
x = -x;
y = -y;
return *this;
}
-inline Vector& Vector::operator-=(Vector u)
+inline Vector& Vector::operator-=(Vector u)
{
x -= u.x;
y -= u.y;
return *this;
}
-inline Vector& Vector::operator*=(float s)
+inline Vector& Vector::operator*=(float s)
{
x *= s;
y *= s;
return *this;
}
-inline Vector& Vector::operator/=(float s)
+inline Vector& Vector::operator/=(float s)
{
x /= s;
y /= 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);
{
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);
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;
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 )
{
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 +
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 +
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,
(e11 * e22 - e12 * e21) / d );
}
-inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m)
+inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m)
{
e11 += m.e11;
e12 += m.e12;
return *this;
}
-inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m)
+inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m)
{
e11 -= m.e11;
e12 -= m.e12;
return *this;
}
-inline Matrix3x3& Matrix3x3::operator*=(float s)
+inline Matrix3x3& Matrix3x3::operator*=(float s)
{
e11 *= s;
e12 *= s;
return *this;
}
-inline Matrix3x3& Matrix3x3::operator/=(float s)
+inline Matrix3x3& Matrix3x3::operator/=(float s)
{
e11 /= s;
e12 /= 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,
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,
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,
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,
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,
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,
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);
}
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;
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;
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;
return *this;
}
-inline Quaternion Quaternion::operator-=(Quaternion q)
+inline Quaternion Quaternion::operator-=(Quaternion q)
{
n -= q.n;
v.x -= q.v.x;
return *this;
}
-inline Quaternion Quaternion::operator*=(float s)
+inline Quaternion Quaternion::operator*=(float s)
{
n *= s;
v.x *= s;
return *this;
}
-inline Quaternion Quaternion::operator/=(float s)
+inline Quaternion Quaternion::operator/=(float s)
{
n /= s;
v.x /= 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;
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);
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;
}
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;
projects / lugaru.git / blobdiff