/*
===========================================================================
Copyright (C) 2015 the OpenMoHAA team

This file is part of OpenMoHAA source code.

OpenMoHAA source code is free software; you can redistribute it
and/or modify it under the terms of the GNU General Public License as
published by the Free Software Foundation; either version 2 of the License,
or (at your option) any later version.

OpenMoHAA source code is distributed in the hope that it will be
useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with OpenMoHAA source code; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
===========================================================================
*/

// vector.h: C++ vector class.

#pragma once

#include <cmath>
#include <cstdio>
#include "qcommon.h"

//#define X 0
//#define Y 1
//#define Z 2
//#define W 3

#ifdef __Q_FABS__
#    define VECTOR_FABS Q_fabs
#else
#    define VECTOR_FABS fabs
#endif

static float vrsqrt(float number)
{
    union {
        float f;
        int   i;
    } t;

    float       x2, y;
    const float threehalfs = 1.5F;

    x2  = number * 0.5F;
    t.f = number;
    t.i = 0x5f3759df - (t.i >> 1); // what the fuck?
    y   = t.f;
    y   = y * (threehalfs - (x2 * y * y)); // 1st iteration
    return y;
}

class Vector
{
public:
    float x;
    float y;
    float z;

    Vector();
    Vector(const vec3_t src);
    Vector(const float x, const float y, const float z);
    explicit Vector(const char *text);

    operator float *();
    operator const float *() const;

    float          pitch(void) const;
    float          yaw(void) const;
    float          roll(void) const;
    float          operator[](const int index) const;
    float        & operator[](const int index);
    void           copyTo(vec3_t vec) const;
    void           setPitch(const float x);
    void           setYaw(const float y);
    void           setRoll(const float z);
    void           setXYZ(const float x, const float y, const float z);
    const Vector & operator=(const Vector &a);
    const Vector & operator=(vec3_t a);
    const Vector & operator=(const char *a);
    friend Vector  operator+(const Vector &a, const Vector &b);
    friend Vector  operator+(vec3_t a, const Vector &b);
    friend Vector  operator+(const Vector &a, vec3_t b);
    const Vector & operator+=(const Vector &a);
    const Vector & operator+=(vec3_t a);
    friend Vector  operator-(const Vector &a, const Vector &b);
    friend Vector  operator-(vec3_t a, const Vector &b);
    friend Vector  operator-(const Vector &a, vec3_t b);
    const Vector & operator-=(const Vector &a);
    const Vector & operator-=(vec3_t a);
    friend Vector  operator*(const Vector &a, const float b);
    friend Vector  operator*(const float a, const Vector &b);
    friend float   operator*(const Vector  &a, const Vector  &b);
    friend float   operator*(vec3_t a, const Vector  &b);
    friend float   operator*(const Vector  &a, vec3_t b);
    const Vector & operator*=(const float a);
    friend Vector  operator/(const Vector &a, const float b);
    friend Vector  operator/(const float a, const Vector &b);
    friend float   operator/(const Vector  &a, const Vector  &b);
    friend float   operator/(vec3_t a, const Vector  &b);
    friend float   operator/(const Vector  &a, vec3_t b);
    const Vector & operator/=(const float a);
    friend int     operator==(const Vector    &a, const Vector    &b);
    friend int     operator==(vec3_t a, const Vector    &b);
    friend int     operator==(const Vector    &a, vec3_t b);
    friend int     operator!=(const Vector    &a, const Vector    &b);
    friend int     operator!=(vec3_t a, const Vector    &b);
    friend int     operator!=(const Vector    &a, vec3_t b);
    int            FuzzyEqual(const Vector           &b, const float epsilon) const;
    int            FuzzyEqual(vec3_t b, const float epsilon) const;
    const Vector & CrossProduct(const Vector a, const Vector b);
    const Vector & CrossProduct(vec3_t a, const Vector b);
    const Vector & CrossProduct(const Vector a, vec3_t b);
    float          length(void) const;
    float          lengthfast(void) const;
    float          lengthSquared(void) const;
    float          lengthXY() const;
    float          lengthXYSquared() const;
    float          normalize(void);
    void           normalizefast(void);
    void           EulerNormalize(void);
    void           EulerNormalize360(void);
    static Vector  Clamp(const Vector &value, const Vector &min, const Vector &max);
    static void    Clamp(Vector   &value, const Vector   &min, const Vector   &max);
    static Vector  Cross(const Vector &vector1, const Vector &vector2);
    static float   Dot(const Vector  &vector1, const Vector  &vector2);
    static float   Dot(vec3_t a, const Vector  &b);
    static float   Dot(const Vector  &a, vec3_t b);
    static float   Distance(const Vector  &vector1, const Vector  &vector2);
    static float   DistanceSquared(const Vector  &vector1, const Vector  &vector2);
    static float   DistanceXY(const Vector  &vector1, const Vector  &vector2);
    static Vector  AnglesBetween(const Vector &vector1, const Vector &vector2);
    static float   AngleBetween(const Vector  &vector1, const Vector  &vector2);
    static bool    CloseEnough(const Vector   &vector1, const Vector   &vector2, const float epsilon = Vector::Epsilon());
    static bool    SmallEnough(const Vector   &vector, const float epsilon = Vector::Epsilon());
    static float   Epsilon(void);
    static Vector& Identity(void);
    Vector         operator-(void) const;
    friend Vector  fabs(const Vector &a);
    float          toYaw(void) const;
    float          toPitch(void) const;
    Vector         toAngles(void) const;
    Vector         AnglesMod(void) const;
    void           AngleVectors(Vector *forward, Vector *right = NULL, Vector *up = NULL) const;
    void           AngleVectorsLeft(Vector *forward, Vector *right = NULL, Vector *up = NULL) const;
    friend Vector  LerpVector(const Vector &w1, const Vector &w2, const float t);
    friend float   MaxValue(const Vector  &a);
    Vector         GetRotatedX(float angle) const;
    void           RotateX(double angle);
    Vector         GetRotatedY(float angle) const;
    void           RotateY(double angle);
    Vector         GetRotatedZ(float angle) const;
    void           RotateZ(float angle);
    void           PackTo01();
    Vector         GetPackedTo01() const;
};

static Vector vec_origin = Vector(0, 0, 0);
static Vector vec_zero   = Vector(0, 0, 0);
static Vector g_vEyeDir  = Vector(0, 0, 0);

inline float Vector::pitch(void) const
{
    return x;
}

inline float Vector::yaw(void) const
{
    return y;
}

inline float Vector::roll(void) const
{
    return z;
}

inline void Vector::setPitch(float pitch)
{
    x = pitch;
}

inline void Vector::setYaw(float yaw)
{
    y = yaw;
}

inline void Vector::setRoll(float roll)
{
    z = roll;
}

inline void Vector::copyTo(vec3_t vec) const
{
    vec[0] = x;
    vec[1] = y;
    vec[2] = z;
}

inline float Vector::operator[](const int index) const
{
    assert((index >= 0) && (index < 3));
    return (&x)[index];
}

inline float& Vector::operator[](const int index)
{
    assert((index >= 0) && (index < 3));
    return (&x)[index];
}

inline void Vector::setXYZ(const float new_x, const float new_y, const float new_z)
{
    x = new_x;
    y = new_y;
    z = new_z;
}

inline Vector::Vector()
    : x(0)
    , y(0)
    , z(0)
{}

inline Vector::Vector(const vec3_t src)
    : x(src[0])
    , y(src[1])
    , z(src[2])
{}

inline Vector::Vector(const float init_x, const float init_y, const float init_z)
    : x(init_x)
    , y(init_y)
    , z(init_z)
{}

inline Vector::Vector(const char *text)
    : x(0)
    , y(0)
    , z(0)
{
    if (text) {
        if (text[0] == '"') {
            sscanf(text, "\"%f %f %f\"", &x, &y, &z);
        } else {
            sscanf(text, "%f %f %f", &x, &y, &z);
        }
    }
}

inline Vector::operator float *(void)
{
    return &x;
}

inline Vector::operator const float *(void) const
{
    return &x;
}

inline const Vector& Vector::operator=(const Vector& a)
{
    x = a.x;
    y = a.y;
    z = a.z;

    return *this;
}

inline const Vector& Vector::operator=(vec3_t a)
{
    x = a[0];
    y = a[1];
    z = a[2];

    return *this;
}

inline const Vector& Vector::operator=(const char *a)
{
    if (a) {
        if (a[0] == '"') {
            sscanf(a, "\"%f %f %f\"", &x, &y, &z);
        } else {
            sscanf(a, "%f %f %f", &x, &y, &z);
        }
    }

    return *this;
}

inline Vector operator+(const Vector& a, const Vector& b)
{
    return Vector(a.x + b.x, a.y + b.y, a.z + b.z);
}

inline Vector operator+(vec3_t a, const Vector& b)
{
    return Vector(a[0] + b.x, a[1] + b.y, a[2] + b.z);
}

inline Vector operator+(const Vector& a, vec3_t b)
{
    return Vector(a.x + b[0], a.y + b[1], a.z + b[2]);
}

inline const Vector& Vector::operator+=(const Vector& a)
{
    x += a.x;
    y += a.y;
    z += a.z;

    return *this;
}

inline const Vector& Vector::operator+=(vec3_t a)
{
    x += a[0];
    y += a[1];
    z += a[2];

    return *this;
}

inline Vector operator-(const Vector& a, const Vector& b)
{
    return Vector(a.x - b.x, a.y - b.y, a.z - b.z);
}

inline Vector operator-(vec3_t a, const Vector& b)
{
    return Vector(a[0] - b.x, a[1] - b.y, a[2] - b.z);
}

inline Vector operator-(const Vector& a, vec3_t b)
{
    return Vector(a.x - b[0], a.y - b[1], a.z - b[2]);
}

inline const Vector& Vector::operator-=(const Vector& a)
{
    x -= a.x;
    y -= a.y;
    z -= a.z;

    return *this;
}

inline const Vector& Vector::operator-=(vec3_t a)
{
    x -= a[0];
    y -= a[1];
    z -= a[2];

    return *this;
}

inline Vector operator*(const Vector& a, const float b)
{
    return Vector(a.x * b, a.y * b, a.z * b);
}

inline Vector operator*(const float a, const Vector& b)
{
    return b * a;
}

inline float operator*(const Vector& a, const Vector& b)
{
    return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
}

inline float operator*(vec3_t a, const Vector& b)
{
    return (a[0] * b.x) + (a[1] * b.y) + (a[2] * b.z);
}

inline float operator*(const Vector& a, vec3_t b)
{
    return (a.x * b[0]) + (a.y * b[1]) + (a.z * b[2]);
}

inline const Vector& Vector::operator*=(const float a)
{
    x *= a;
    y *= a;
    z *= a;

    return *this;
}

inline Vector operator/(const Vector& a, const float b)
{
    return Vector(a.x / b, a.y / b, a.z / b);
}

inline Vector operator/(const float a, const Vector& b)
{
    return b / a;
}

inline float operator/(const Vector& a, const Vector& b)
{
    return (a.x / b.x) + (a.y / b.y) + (a.z / b.z);
}

inline float operator/(vec3_t a, const Vector& b)
{
    return (a[0] / b.x) + (a[1] / b.y) + (a[2] / b.z);
}

inline float operator/(const Vector& a, vec3_t b)
{
    return (a.x / b[0]) + (a.y / b[1]) + (a.z / b[2]);
}

inline const Vector& Vector::operator/=(const float a)
{
    *this = *this / a;
    return *this;
}

inline int Vector::FuzzyEqual(const Vector& b, const float epsilon) const
{
    return ((VECTOR_FABS(x - b.x) < epsilon) && (VECTOR_FABS(y - b.y) < epsilon) && (VECTOR_FABS(z - b.z) < epsilon));
}

inline int Vector::FuzzyEqual(vec3_t b, const float epsilon) const
{
    return (
        (VECTOR_FABS(x - b[0]) < epsilon) && (VECTOR_FABS(y - b[1]) < epsilon) && (VECTOR_FABS(z - b[2]) < epsilon)
    );
}

inline int operator==(const Vector& a, const Vector& b)
{
    return ((a.x == b.x) && (a.y == b.y) && (a.z == b.z));
}

inline int operator==(vec3_t a, const Vector& b)
{
    return ((a[0] == b.x) && (a[1] == b.y) && (a[2] == b.z));
}

inline int operator==(const Vector& a, vec3_t b)
{
    return ((a.x == b[0]) && (a.y == b[1]) && (a.z == b[2]));
}

inline int operator!=(const Vector& a, const Vector& b)
{
    return ((a.x != b.x) || (a.y != b.y) || (a.z != b.z));
}

inline int operator!=(vec3_t a, const Vector& b)
{
    return ((a[0] != b.x) || (a[1] != b.y) || (a[2] != b.z));
}

inline int operator!=(const Vector& a, vec3_t b)
{
    return ((a.x != b[0]) || (a.y != b[1]) || (a.z != b[2]));
}

inline const Vector& Vector::CrossProduct(const Vector a, const Vector b)
{
    x = (a.y * b.z) - (a.z * b.y);
    y = (a.z * b.x) - (a.x * b.z);
    z = (a.x * b.y) - (a.y * b.x);

    return *this;
}

inline const Vector& Vector::CrossProduct(vec3_t a, const Vector b)
{
    x = (a[1] * b.z) - (a[2] * b.y);
    y = (a[2] * b.x) - (a[0] * b.z);
    z = (a[0] * b.y) - (a[1] * b.x);

    return *this;
}

inline const Vector& Vector::CrossProduct(const Vector a, vec3_t b)
{
    x = (a.y * b[2]) - (a.z * b[1]);
    y = (a.z * b[0]) - (a.x * b[2]);
    z = (a.x * b[1]) - (a.y * b[0]);

    return *this;
}

inline Vector Vector::Clamp(const Vector& value, const Vector& minimum, const Vector& maximum)
{
    Vector clamped(value);
    Vector::Clamp(clamped, minimum, maximum);
    return clamped;
}

inline void Vector::Clamp(Vector& value, const Vector& minimum, const Vector& maximum)
{
    for (int i = 0; i < 3; i++) {
        Q_clamp(value[i], minimum[i], maximum[i]);
    }
}

inline Vector Vector::Cross(const Vector& vector1, const Vector& vector2)
{
    const Vector result(
        (vector1.y * vector2.z) - (vector1.z * vector2.y),
        (vector1.z * vector2.x) - (vector1.x * vector2.z),
        (vector1.x * vector2.y) - (vector1.y * vector2.x)
    );

    return result;
}

inline float Vector::Dot(const Vector& vector1, const Vector& vector2)
{
    return vector1 * vector2;
}

inline float Vector::Dot(vec3_t vector1, const Vector& vector2)
{
    return vector1 * vector2;
}

inline float Vector::Dot(const Vector& vector1, vec3_t vector2)
{
    return vector1 * vector2;
}

//----------------------------------------------------------------
// Name:				lengthSquared
// Class:			Vector
//
// Description:	Returns squared length of the vector
//
// Parameters:		None
//
// Returns:			float - squared length
//----------------------------------------------------------------
inline float Vector::lengthSquared(void) const
{
    return (x * x) + (y * y) + (z * z);
}

inline float Vector::length(void) const
{
    return sqrt(lengthSquared());
}

inline float Vector::lengthfast(void) const
{
    return vrsqrt(lengthSquared());
}

//----------------------------------------------------------------
// Name:				lengthXY
// Class:			Vector
//
// Description:	Returns length of the vector (using only the x
//						and y components
//
// Parameters:		None
//
// Returns:			float - length of the vector in the xy plane
//----------------------------------------------------------------
inline float Vector::lengthXY() const
{
    return sqrt((x * x) + (y * y));
}

//----------------------------------------------------------------
// Name:				lengthXYSquared
// Class:			Vector
//
// Description:	Returns length of the vector squared (using only the x
//						and y components
//
// Parameters:		None
//
// Returns:			float - squared length of the vector in the xy plane
//----------------------------------------------------------------
inline float Vector::lengthXYSquared() const
{
    return (x * x) + (y * y);
}

//----------------------------------------------------------------
// Name:				normalize
// Class:			Vector
//
// Description:	unitizes the vector
//
// Parameters:		None
//
// Returns:			float - length of the vector before the function
//----------------------------------------------------------------
inline float Vector::normalize(void)
{
    return VectorNormalize(*this);
}

//----------------------------------------------------------------
// Name:				normalizefast
// Class:			Vector
//
// Description:	fast version of normalize
//
// Parameters:		None
//
// Returns:			float - length of the vector before the function
//----------------------------------------------------------------
inline void Vector::normalizefast(void)
{
    float ilength;

    ilength = this->lengthfast();

    x *= ilength;
    y *= ilength;
    z *= ilength;
}

//----------------------------------------------------------------
// Name:				EulerNormalize
// Class:			Vector
//
// Description:	forces each component of the vector into the
//						range (-180, +180) by adding or subtracting 360
//						This is useful when the Vector is being used as
//						EulerAngles to represent a rotational offset
//
// Parameters:		None
//
// Returns:			None
//----------------------------------------------------------------
inline void Vector::EulerNormalize(void)
{
    x = AngleNormalize180(x);
    y = AngleNormalize180(y);
    z = AngleNormalize180(z);
}

//----------------------------------------------------------------
// Name:				EulerNormalize360
// Class:			Vector
//
// Description:	forces each component of the vector into the
//						range (0, +360) by adding or subtracting 360
//						This is useful when the Vector is being used as
//						EulerAngles to represent a rotational direction
//
// Parameters:		None
//
// Returns:			None
//----------------------------------------------------------------
inline void Vector::EulerNormalize360(void)
{
    x = AngleNormalize360(x);
    y = AngleNormalize360(y);
    z = AngleNormalize360(z);
}

//----------------------------------------------------------------
// Name:				Epsilon
// Class:			Vector
//
// Description:	returns a standard 'small' value for the class
//
// Parameters:		None
//
// Returns:			float - the epsilon constant for the class
//----------------------------------------------------------------
inline float Vector::Epsilon(void)
{
    return 0.000000001f;
}

//----------------------------------------------------------------
// Name:				Identity
// Class:			Vector
//
// Description:	returns the additive identity for the class
//
// Parameters:		None
//
// Returns:			Vector - the identity for the class
//----------------------------------------------------------------
inline Vector& Vector::Identity(void)
{
    return vec_zero;
}

//----------------------------------------------------------------
// Name:				Distance
// Class:			Vector
//
// Description:	returns the distance between two vectors
//
// Parameters:
//						Vector - first vector
//						Vector - second vector
//
// Returns:			float - distance between the two vectors
//----------------------------------------------------------------
inline float Vector::Distance(const Vector& vector1, const Vector& vector2)
{
    return (vector1 - vector2).length();
}

//----------------------------------------------------------------
// Name:				DistanceSquared
// Class:			Vector
//
// Description:	returns the squared distance between two vectors
//
// Parameters:
//						Vector - first vector
//						Vector - second vector
//
// Returns:			float - distance between the two vectors squared
//----------------------------------------------------------------
inline float Vector::DistanceSquared(const Vector& vector1, const Vector& vector2)
{
    return (vector1 - vector2).lengthSquared();
}

//----------------------------------------------------------------
// Name:				DistanceXY
// Class:			Vector
//
// Description:	returns the distance between two vectors in the
//						xy plane
//
// Parameters:
//						Vector - first vector
//						Vector - second vector
//
// Returns:			float - distance between the two vectors in the
//						xy plane
//----------------------------------------------------------------
inline float Vector::DistanceXY(const Vector& vector1, const Vector& vector2)
{
    return (vector1 - vector2).lengthXY();
}

inline Vector Vector::toAngles(void) const
{
    float forward;
    float yaw, pitch;

    if ((x == 0.0f) && (y == 0.0f)) {
        yaw = 0.0f;
        if (z > 0.0f) {
            pitch = 90.0f;
        } else {
            pitch = 270.0f;
        }
    } else {
        yaw = atan2(y, x) * 180.0f / M_PI;
        if (yaw < 0.0f) {
            yaw += 360.0f;
        }

        forward = (float)sqrt(x * x + y * y);
        pitch   = atan2(z, forward) * 180.0f / M_PI;
        if (pitch < 0.0f) {
            pitch += 360.0f;
        }
    }

    return Vector(-pitch, yaw, 0.0f);
}

//----------------------------------------------------------------
// Name:				AnglesBetween
// Class:			Vector
//
// Description:	returns the smaller of the angles formed by the
//						two vectors
//
// Parameters:
//						Vector - first vector
//						Vector - second vector
//
// Returns:			Vector - angles between the vectors
//----------------------------------------------------------------
inline Vector Vector::AnglesBetween(const Vector& vector1, const Vector& vector2)
{
    Vector unitVector1(vector1);
    unitVector1.normalize();
    Vector unitVector2(vector2);
    unitVector2.normalize();
    Vector angles(unitVector1.toAngles() - unitVector2.toAngles());
    angles.EulerNormalize();

    return angles;
}

//----------------------------------------------------------------
// Name:				AngleBetween
// Class:			Vector
//
// Description:	returns the smaller of the angles formed by the
//						two vectors
//
// Parameters:
//						Vector - first vector
//						Vector - second vector
//
// Returns:			float - angle between the vectors
//----------------------------------------------------------------
inline float Vector::AngleBetween(const Vector& vector1, const Vector& vector2)
{
    Vector unitVector1(vector1);
    unitVector1.normalize();
    Vector unitVector2(vector2);
    unitVector2.normalize();

    return acos(Vector::Dot(unitVector1, unitVector2));
}

//----------------------------------------------------------------
// Name:				CloseEnough
// Class:			Vector
//
// Description:	tests to see if the two vectors are within
//						'epsilon' of each other
//
// Parameters:
//						Vector - first vector
//						Vector - second vector
//						float  - amount that each component of the
//						vectors can be apart
//
// Returns:			bool	 - the result of the test for closeness
//----------------------------------------------------------------
inline bool Vector::CloseEnough(const Vector& vector1, const Vector& vector2, const float epsilon)
{
    return Distance(vector1, vector2) < epsilon;
}

//----------------------------------------------------------------
// Name:				SmallEnough
// Class:			Vector
//
// Description:	tests to see if the vectors are within
//						'epsilon' of the origin
//
// Parameters:
//						Vector - vector
//						float  - amount that each component of the
//						vectors can be from the origin
//
// Returns:			bool	 - the result of the test for smallness
//----------------------------------------------------------------
inline bool Vector::SmallEnough(const Vector& vector, const float epsilon)
{
    return CloseEnough(vector, Vector::Identity(), epsilon);
}

inline Vector Vector::operator-() const
{
    return Vector(-x, -y, -z);
}

inline Vector fabs(const Vector& a)
{
    return Vector(VECTOR_FABS(a.x), VECTOR_FABS(a.y), VECTOR_FABS(a.z));
}

inline float MaxValue(const Vector& a)
{
    float maxy;
    float maxz;
    float max;

    max  = VECTOR_FABS(a.x);
    maxy = VECTOR_FABS(a.y);
    maxz = VECTOR_FABS(a.z);

    if (maxy > max) {
        max = maxy;
    }
    if (maxz > max) {
        max = maxz;
    }
    return max;
}

inline float Vector::toYaw(void) const
{
    float yaw;

    if ((y == 0.0f) && (x == 0.0f)) {
        yaw = 0.0f;
    } else {
        yaw = (float)((int)(atan2(y, x) * 180.0f / M_PI));
        if (yaw < 0.0f) {
            yaw += 360.0f;
        }
    }

    return yaw;
}

inline float Vector::toPitch(void) const
{
    float forward;
    float pitch;

    forward = (float)sqrt((x * x) + (y * y));
    pitch = (float)((int)(atan2(z, forward) * 180 / M_PI));
    if (pitch < 0.0f) {
        pitch += 360.0f;
    }

    return pitch;
}

inline Vector Vector::AnglesMod(void) const
{
    return Vector(AngleMod(x), AngleMod(y), AngleMod(z));
}

inline void Vector::AngleVectors(Vector *forward, Vector *right, Vector *up) const
{
    float        angle;
    static float sr, sp, sy, cr, cp, cy; // static to help MS compiler fp bugs

    angle = yaw() * (M_PI * 2.0f / 360.0f);
    sy    = sin(angle);
    cy    = cos(angle);

    angle = pitch() * (M_PI * 2.0f / 360.0f);
    sp    = sin(angle);
    cp    = cos(angle);

    angle = roll() * (M_PI * 2.0f / 360.0f);
    sr    = sin(angle);
    cr    = cos(angle);

    if (forward) {
        forward->setXYZ(cp * cy, cp * sy, -sp);
    }

    if (right) {
        right->setXYZ((-1 * sr * sp * cy) + (-1 * cr * -sy), (-1 * sr * sp * sy) + (-1 * cr * cy), -1 * sr * cp);
    }

    if (up) {
        up->setXYZ((cr * sp * cy) + (-sr * -sy), (cr * sp * sy) + (-sr * cy), cr * cp);
    }
}

inline void Vector::AngleVectorsLeft(Vector *forward, Vector *left, Vector *up) const
{
    float        angle;
    static float sr, sp, sy, cr, cp, cy; // static to help MS compiler fp bugs

    angle = yaw() * (M_PI * 2 / 360);
    sy    = sin(angle);
    cy    = cos(angle);

    angle = pitch() * (M_PI * 2 / 360);
    sp    = sin(angle);
    cp    = cos(angle);

    angle = roll() * (M_PI * 2 / 360);
    sr    = sin(angle);
    cr    = cos(angle);

    if (forward) {
        forward->setXYZ(cp * cy, cp * sy, -sp);
    }

    if (left) {
        left->setXYZ((sr * sp * cy) + (cr * -sy), (sr * sp * sy) + (cr * cy), sr * cp);
    }

    if (up) {
        up->setXYZ((cr * sp * cy) + (-sr * -sy), (cr * sp * sy) + (-sr * cy), cr * cp);
    }
}

#define LERP_DELTA 1e-6

inline Vector LerpVector(const Vector& vector1, const Vector& vector2, const float t)
{
    float omega, cosom, sinom, scale0, scale1;

    Vector w1(vector1);
    Vector w2(vector2);

    w1.normalize();
    w2.normalize();

    cosom = w1 * w2;
    if ((1.0f - cosom) > LERP_DELTA) {
        omega  = acos(cosom);
        sinom  = sin(omega);
        scale0 = sin((1.0f - t) * omega) / sinom;
        scale1 = sin(t * omega) / sinom;
    } else {
        scale0 = 1.0f - t;
        scale1 = t;
    }

    return ((w1 * scale0) + (w2 * scale1));
}

class Quat
{
public:
    float x;
    float y;
    float z;
    float w;

    Quat();
    Quat(Vector angles);
    Quat(float scrMatrix[3][3]);
    Quat(const float x, const float y, const float z, const float w);

    float      *vec4(void);
    float       operator[](const int index) const;
    float     & operator[](const int index);
    void        set(const float x, const float y, const float z, const float w);
    const Quat& operator=(const Quat& a);
    friend Quat operator+(const Quat& a, const Quat& b);
    const Quat& operator+=(const Quat& a);
    friend Quat operator-(const Quat& a, const Quat& b);
    const Quat& operator-=(const Quat& a);
    friend Quat operator*(const Quat& a, const float b);
    friend Quat operator*(const float a, const Quat& b);
    const Quat& operator*=(const float a);
    friend int  operator==(const Quat &a, const Quat &b);
    friend int  operator!=(const Quat &a, const Quat &b);
    float       length(void) const;
    float       lengthSquared(void) const;
    const Quat& normalize(void);
    Quat        operator-() const;
    Vector      toAngles(void);
};

inline Quat::Quat()
    : x(0)
    , y(0)
    , z(0)
    , w(0)
{}

inline Quat::Quat(Vector Angles)
{
    EulerToQuat(Angles, this->vec4());
}

inline Quat::Quat(float srcMatrix[3][3])
{
    MatToQuat(srcMatrix, this->vec4());
}

inline Quat::Quat(const float init_x, const float init_y, const float init_z, const float init_w)
    : x(init_x)
    , y(init_y)
    , z(init_z)
    , w(init_w)
{}

inline float Quat::operator[](const int index) const
{
    assert((index >= 0) && (index < 4));
    return (&x)[index];
}

inline float& Quat::operator[](const int index)
{
    assert((index >= 0) && (index < 4));
    return (&x)[index];
}

inline float *Quat::vec4(void)
{
    return &x;
}

inline void Quat::set(const float new_x, const float new_y, const float new_z, const float new_w)
{
    x = new_x;
    y = new_y;
    z = new_z;
    w = new_w;
}

inline const Quat& Quat::operator=(const Quat& a)
{
    x = a.x;
    y = a.y;
    z = a.z;
    w = a.w;

    return *this;
}

inline Quat operator+(const Quat& a, const Quat& b)
{
    return Quat(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
}

inline const Quat& Quat::operator+=(const Quat& a)
{
    *this = *this + a;

    return *this;
}

inline Quat operator-(const Quat& a, const Quat& b)
{
    return Quat(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
}

inline const Quat& Quat::operator-=(const Quat& a)
{
    *this = *this - a;

    return *this;
}

inline Quat operator*(const Quat& a, const float b)
{
    return Quat(a.x * b, a.y * b, a.z * b, a.w * b);
}

inline Quat operator*(const float a, const Quat& b)
{
    return b * a;
}

inline const Quat& Quat::operator*=(const float a)
{
    *this = *this * a;

    return *this;
}

inline int operator==(const Quat& a, const Quat& b)
{
    return ((a.x == b.x) && (a.y == b.y) && (a.z == b.z) && (a.w == b.w));
}

inline int operator!=(const Quat& a, const Quat& b)
{
    return (((a.x != b.x) || (a.y != b.y)) || ((a.z != b.z) && (a.w != b.w)));
}

inline float Quat::length(void) const
{
    float length;

    length = (x * x) + (y * y) + (z * z) + (w * w);
    return sqrt(length);
}

inline const Quat& Quat::normalize(void)
{
    float length, ilength;

    length = this->length();
    if (length) {
        ilength = 1.0f / length;
        *this *= ilength;
    }

    return *this;
}

inline Quat Quat::operator-() const
{
    return Quat(-x, -y, -z, -w);
}

inline Vector Quat::toAngles(void)
{
    float  m[3][3];
    vec3_t angles;

    QuatToMat(this->vec4(), m);
    MatrixToEulerAngles(m, angles);
    return Vector(angles);
}

inline Vector Vector::GetRotatedX(float angle) const
{
    if (angle == 0.0) {
        return (*this);
    }

    float sinAngle = (float)sin(M_PI * angle / 180);
    float cosAngle = (float)cos(M_PI * angle / 180);

    return Vector(x, y * cosAngle - z * sinAngle, y * sinAngle + z * cosAngle);
}

inline void Vector::RotateX(double angle)
{
    (*this) = GetRotatedX(angle);
}

inline Vector Vector::GetRotatedY(float angle) const
{
    if (angle == 0.0) {
        return (*this);
    }

    float sinAngle = (float)sin(M_PI * angle / 180);
    float cosAngle = (float)cos(M_PI * angle / 180);

    return Vector(x * cosAngle + z * sinAngle, y, -x * sinAngle + z * cosAngle);
}

inline void Vector::RotateY(double angle)
{
    (*this) = GetRotatedY(angle);
}

inline Vector Vector::GetRotatedZ(float angle) const
{
    if (angle == 0.0) {
        return (*this);
    }

    float sinAngle = (float)sin(M_PI * angle / 180);
    float cosAngle = (float)cos(M_PI * angle / 180);

    return Vector(x * cosAngle - y * sinAngle, x * sinAngle + y * cosAngle, z);
}

inline void Vector::RotateZ(float angle)
{
    (*this) = GetRotatedZ(angle);
}

inline void Vector::PackTo01()
{
    (*this) = GetPackedTo01();
}

inline Vector Vector::GetPackedTo01() const
{
    Vector temp(*this);

    temp.normalize();

    temp = temp * 0.5f + Vector(0.5f, 0.5f, 0.5f);

    return temp;
}

#undef VECTOR_FABS