Vector.h

#pragma once

#ifdef NDEBUG
#define Assert( _exp ) ((void)0)
#else
#define Assert(x)
#endif

#include "Definitions.h"
#include <sstream>

#define CHECK_VALID( _v ) 0

#define FastSqrt(x)	(sqrt)(x)

#define deg(a) a * 57.295779513082

#define M_PI		3.14159265358979323846	// matches value in gcc v2 math.h

#define M_PI_2      (M_PI * 2.f)

#define M_PI_F		((float)(M_PI))	// Shouldn't collide with anything.

#define M_PHI		1.61803398874989484820 // golden ratio

// NJS: Inlined to prevent floats from being autopromoted to doubles, as with the old system.
#ifndef RAD2DEG
#define RAD2DEG(x)  ((float)(x) * (float)(180.f / M_PI_F))
#endif

#ifndef DEG2RAD
#define DEG2RAD(x)  ((float)(x) * (float)(M_PI_F / 180.f))
#endif

// MOVEMENT INFO
enum
{
    PITCH = 0,	// up / down
    YAW,		// left / right
    ROLL		// fall over
};

// decls for aligning data


#define ALIGN16 __declspec(align(16))
#define VALVE_RAND_MAX 0x7fff
#define VectorExpand(v) (v).x, (v).y, (v).z

typedef float vec_t;

class Vector
{
public:
    float x, y, z;
    Vector(void);
    Vector(float X, float Y, float Z);
    void Init(float ix = 0.0f, float iy = 0.0f, float iz = 0.0f);
    bool IsValid() const;
    float operator[](int i) const;
    float& operator[](int i);
    inline void Zero();
    bool operator==(const Vector& v) const;
    bool operator!=(const Vector& v) const;
    inline Vector&	operator+=(const Vector &v);
    inline Vector&	operator-=(const Vector &v);
    inline Vector&	operator*=(const Vector &v);
    inline Vector&	operator*=(float s);
    inline Vector&	operator/=(const Vector &v);
    inline Vector&	operator/=(float s);
    inline Vector&	operator+=(float fl);
    inline Vector&	operator-=(float fl);
    inline float	Length() const;
    inline float LengthSqr(void) const
    {
        CHECK_VALID(*this);
        return (x*x + y*y + z*z);
    }
    bool IsZero(float tolerance = 0.01f) const
    {
        return (x > -tolerance && x < tolerance &&
            y > -tolerance && y < tolerance &&
            z > -tolerance && z < tolerance);
    }
	Vector  Clamp();
    Vector	Normalize();
    float	NormalizeInPlace();
    inline float	DistTo(const Vector &vOther) const;
    inline float	DistToSqr(const Vector &vOther) const;
    float	Dot(const Vector& vOther) const;
    float	Length2D(void) const;
    float	Length2DSqr(void) const;
    void	MulAdd(const Vector& a, const Vector& b, float scalar);
	Vector	Cross(const Vector &vOther) const;
    Vector& operator=(const Vector &vOther);
    Vector	operator-(void) const;
    Vector	operator+(const Vector& v) const;
    Vector	operator-(const Vector& v) const;
    Vector	operator*(const Vector& v) const;
    Vector	operator/(const Vector& v) const;
    Vector	operator*(float fl) const;
    Vector	operator/(float fl) const;
    // Base address...
    float* Base();
    float const* Base() const;

	auto clear() -> void
	{
		x = 0.f;
		y = 0.f;
		z = 0.f;
	}

	inline Vector Angle(Vector* up = 0)
	{
		if (!x && !y)
			return Vector(0, 0, 0);

		float roll = 0;

		if (up)
		{
			Vector left = (*up).Cross(*this);
			roll = deg(atan2f(left.z, (left.y * x) - (left.x * y)));
		}
		return Vector(deg(atan2f(-z, sqrtf(x*x + y * y))), deg(atan2f(y, x)), roll);
	}
	float Area(){
		return x * x + y * y + z * z;
	}
	float Magnitude() {
		return sqrt(Area());
	}
	float AngleTo(Vector& vOther) {
		return this->Dot(vOther) / (Magnitude() * vOther.Magnitude());
	}
};

//===============================================
inline void Vector::Init(float ix, float iy, float iz)
{
    x = ix; y = iy; z = iz;
    CHECK_VALID(*this);
}
//===============================================
inline Vector::Vector(float X, float Y, float Z)
{
    x = X; y = Y; z = Z;
    CHECK_VALID(*this);
}
//===============================================
inline Vector::Vector(void) { }
//===============================================
inline void Vector::Zero()
{
    x = y = z = 0.0f;
}
//===============================================
inline void VectorClear(Vector& a)
{
    a.x = a.y = a.z = 0.0f;
}
//===============================================
inline Vector& Vector::operator=(const Vector &vOther)
{
    CHECK_VALID(vOther);
    x = vOther.x; y = vOther.y; z = vOther.z;
    return *this;
}
//===============================================
inline float& Vector::operator[](int i)
{
    Assert((i >= 0) && (i < 3));
    return ((float*)this)[i];
}
//===============================================
inline float Vector::operator[](int i) const
{
    Assert((i >= 0) && (i < 3));
    return ((float*)this)[i];
}
//===============================================
inline bool Vector::operator==(const Vector& src) const
{
    CHECK_VALID(src);
    CHECK_VALID(*this);
    return (src.x == x) && (src.y == y) && (src.z == z);
}
//===============================================
inline bool Vector::operator!=(const Vector& src) const
{
    CHECK_VALID(src);
    CHECK_VALID(*this);
    return (src.x != x) || (src.y != y) || (src.z != z);
}
//===============================================
inline void VectorCopy(const Vector& src, Vector& dst)
{
    CHECK_VALID(src);
    dst.x = src.x;
    dst.y = src.y;
    dst.z = src.z;
}
//===============================================
inline  Vector& Vector::operator+=(const Vector& v)
{
    CHECK_VALID(*this);
    CHECK_VALID(v);
    x += v.x; y += v.y; z += v.z;
    return *this;
}
//===============================================
inline  Vector& Vector::operator-=(const Vector& v)
{
    CHECK_VALID(*this);
    CHECK_VALID(v);
    x -= v.x; y -= v.y; z -= v.z;
    return *this;
}
//===============================================
inline  Vector& Vector::operator*=(float fl)
{
    x *= fl;
    y *= fl;
    z *= fl;
    CHECK_VALID(*this);
    return *this;
}
//===============================================
inline  Vector& Vector::operator*=(const Vector& v)
{
    CHECK_VALID(v);
    x *= v.x;
    y *= v.y;
    z *= v.z;
    CHECK_VALID(*this);
    return *this;
}
//===============================================
inline Vector&	Vector::operator+=(float fl)
{
    x += fl;
    y += fl;
    z += fl;
    CHECK_VALID(*this);
    return *this;
}
//===============================================
inline Vector&	Vector::operator-=(float fl)
{
    x -= fl;
    y -= fl;
    z -= fl;
    CHECK_VALID(*this);
    return *this;
}
//===============================================
inline  Vector& Vector::operator/=(float fl)
{
    Assert(fl != 0.0f);
    float oofl = 1.0f / fl;
    x *= oofl;
    y *= oofl;
    z *= oofl;
    CHECK_VALID(*this);
    return *this;
}
//===============================================
inline Vector Vector::Clamp()
{
	CHECK_VALID(*this);

	if (this->x < -89.0f)
		this->x = -89.0f;

	if (this->x > 89.0f)
		this->x = 89.0f;

	while (this->y < -180.0f)
		this->y += 360.0f;

	while (this->y > 180.0f)
		this->y -= 360.0f;

	this->z = 0.0f;

	return *this;
}
//===============================================
inline  Vector& Vector::operator/=(const Vector& v)
{
    CHECK_VALID(v);
    Assert(v.x != 0.0f && v.y != 0.0f && v.z != 0.0f);
    x /= v.x;
    y /= v.y;
    z /= v.z;
    CHECK_VALID(*this);
    return *this;
}
//===============================================
inline float Vector::Length(void) const
{
    CHECK_VALID(*this);

    float root = 0.0f;

    float sqsr = x*x + y*y + z*z;

    root = sqrt(sqsr);

    return root;
}
//===============================================
inline float Vector::Length2D(void) const
{
    CHECK_VALID(*this);

    float root = 0.0f;

    float sqst = x*x + y*y;

    root = sqrt(sqst);

    return root;
}
//===============================================
inline float Vector::Length2DSqr(void) const
{
    return (x*x + y*y);
}
//===============================================
inline Vector CrossProduct(const Vector& a, const Vector& b)
{
    return Vector(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);
}
//===============================================
inline Vector Vector::Cross(const Vector& vOther) const
{
	return CrossProduct(*this, vOther);
}
//===============================================
float Vector::DistTo(const Vector &vOther) const
{
    Vector delta;

    delta.x = x - vOther.x;
    delta.y = y - vOther.y;
    delta.z = z - vOther.z;

    return delta.Length();
}
float Vector::DistToSqr(const Vector &vOther) const
{
    Vector delta;

    delta.x = x - vOther.x;
    delta.y = y - vOther.y;
    delta.z = z - vOther.z;

    return delta.LengthSqr();
}
//===============================================
inline Vector Vector::Normalize()
{
    Vector vector;
    float length = this->Length();

    if (length != 0)
    {
        vector.x = x / length;
        vector.y = y / length;
        vector.z = z / length;
    }
    else
    {
        vector.x = vector.y = 0.0f; vector.z = 1.0f;
    }

    return vector;
}
//===============================================
// changed that to fit awall, paste from xaE
inline float Vector::NormalizeInPlace()
{
    float radius = FastSqrt(x * x + y * y + z * z);

    // FLT_EPSILON is added to the radius to eliminate the possibility of divide by zero.
    float iradius = 1.f / (radius + FLT_EPSILON);

    x *= iradius;
    y *= iradius;
    z *= iradius;

    return radius;
}
//===============================================
inline void Vector::MulAdd(const Vector& a, const Vector& b, float scalar)
{
    x = a.x + b.x * scalar;
    y = a.y + b.y * scalar;
    z = a.z + b.z * scalar;
}
//===============================================
inline float VectorNormalize(Vector& v)
{
    Assert(v.IsValid());
    float l = v.Length();
    if (l != 0.0f)
    {
        v /= l;
    }
    else
    {
        // FIXME:
        // Just copying the existing implemenation; shouldn't res.z == 0?
        v.x = v.y = 0.0f; v.z = 1.0f;
    }
    return l;
}
//===============================================
inline float VectorNormalize(float * v)
{
    return VectorNormalize(*(reinterpret_cast<Vector *>(v)));
}
//===============================================
inline Vector Vector::operator+(const Vector& v) const
{
    Vector res;
    res.x = x + v.x;
    res.y = y + v.y;
    res.z = z + v.z;
    return res;
}

//===============================================
inline Vector Vector::operator-(const Vector& v) const
{
    Vector res;
    res.x = x - v.x;
    res.y = y - v.y;
    res.z = z - v.z;
    return res;
}
//===============================================
inline Vector Vector::operator*(float fl) const
{
    Vector res;
    res.x = x * fl;
    res.y = y * fl;
    res.z = z * fl;
    return res;
}
//===============================================
inline Vector Vector::operator*(const Vector& v) const
{
    Vector res;
    res.x = x * v.x;
    res.y = y * v.y;
    res.z = z * v.z;
    return res;
}
//===============================================
inline Vector Vector::operator/(float fl) const
{
    Vector res;
    res.x = x / fl;
    res.y = y / fl;
    res.z = z / fl;
    return res;
}
//===============================================
inline Vector Vector::operator/(const Vector& v) const
{
    Vector res;
    res.x = x / v.x;
    res.y = y / v.y;
    res.z = z / v.z;
    return res;
}
inline float Vector::Dot(const Vector& vOther) const
{
    const Vector& a = *this;

    return(a.x*vOther.x + a.y*vOther.y + a.z*vOther.z);
}

FORCEINLINE vec_t DotProduct(const Vector& a, const Vector& b)
{
	return (a.x * b.x + a.y * b.y + a.z * b.z);
}

//-----------------------------------------------------------------------------
// length
//-----------------------------------------------------------------------------

inline float VectorLength(const Vector& v)
{
    CHECK_VALID(v);
    return (float)FastSqrt(v.x*v.x + v.y*v.y + v.z*v.z);
}

//VECTOR SUBTRAC
inline void VectorSubtract(const Vector& a, const Vector& b, Vector& c)
{
    CHECK_VALID(a);
    CHECK_VALID(b);
    c.x = a.x - b.x;
    c.y = a.y - b.y;
    c.z = a.z - b.z;
}

//VECTORADD
inline void VectorAdd(const Vector& a, const Vector& b, Vector& c)
{
    CHECK_VALID(a);
    CHECK_VALID(b);
    c.x = a.x + b.x;
    c.y = a.y + b.y;
    c.z = a.z + b.z;
}

//-----------------------------------------------------------------------------
// Base address...
//-----------------------------------------------------------------------------
inline float* Vector::Base()
{
    return (float*)this;
}

inline float const* Vector::Base() const
{
    return (float const*)this;
}

inline void VectorMAInline(const float* start, float scale, const float* direction, float* dest)
{
    dest[0] = start[0] + direction[0] * scale;
    dest[1] = start[1] + direction[1] * scale;
    dest[2] = start[2] + direction[2] * scale;
}

inline void VectorMAInline(const Vector& start, float scale, const Vector& direction, Vector& dest)
{
    dest.x = start.x + direction.x*scale;
    dest.y = start.y + direction.y*scale;
    dest.z = start.z + direction.z*scale;
}

inline void VectorMA(const Vector& start, float scale, const Vector& direction, Vector& dest)
{
    VectorMAInline(start, scale, direction, dest);
}

inline void VectorMA(const float * start, float scale, const float *direction, float *dest)
{
    VectorMAInline(start, scale, direction, dest);
}


class ALIGN16 VectorAligned : public Vector
{
public:
    inline VectorAligned(void) {};
    inline VectorAligned(float X, float Y, float Z)
    {
        Init(X, Y, Z);
    }

#ifdef VECTOR_NO_SLOW_OPERATIONS

private:
    // No copy constructors allowed if we're in optimal mode
    VectorAligned(const VectorAligned& vOther);
    VectorAligned(const Vector &vOther);

#else
public:
    explicit VectorAligned(const Vector &vOther)
    {
        Init(vOther.x, vOther.y, vOther.z);
    }

    VectorAligned& operator=(const Vector &vOther)
    {
        Init(vOther.x, vOther.y, vOther.z);
        return *this;
    }

#endif
    float w;	// this space is used anyway
};


inline unsigned long& FloatBits(float& f)
{
    return *reinterpret_cast<unsigned long*>(&f);
}

inline bool IsFinite(float f)
{
    return ((FloatBits(f) & 0x7F800000) != 0x7F800000);
}

//=========================================================
// 2D Vector2D
//=========================================================

class Vector2D
{
public:
    // Members
    float x, y;

    // Construction/destruction
    Vector2D(void);
    Vector2D(float X, float Y);
    Vector2D(const float *pFloat);

    // Initialization
    void Init(float ix = 0.0f, float iy = 0.0f);

    // Got any nasty NAN's?
    bool IsValid() const;

    // array access...
    float  operator[](int i) const;
    float& operator[](int i);

    // Base address...
    float*       Base();
    float const* Base() const;

    // Initialization methods
    void Random(float minVal, float maxVal);

    // equality
    bool operator==(const Vector2D& v) const;
    bool operator!=(const Vector2D& v) const;

    // arithmetic operations
    Vector2D& operator+=(const Vector2D& v);
    Vector2D& operator-=(const Vector2D& v);
    Vector2D& operator*=(const Vector2D& v);
    Vector2D& operator*=(float           s);
    Vector2D& operator/=(const Vector2D& v);
    Vector2D& operator/=(float           s);
	Vector2D Normalized() const
	{
		return *this / Length();
	}
    // negate the Vector2D components
    void Negate();

    // Get the Vector2D's magnitude.
    float Length() const;

    // Get the Vector2D's magnitude squared.
    float LengthSqr(void) const;

    // return true if this vector is (0,0) within tolerance
    bool IsZero(float tolerance = 0.01f) const
    {
        return (x > -tolerance && x < tolerance &&
            y > -tolerance && y < tolerance);
    }

    float Normalize();

    // Normalize in place and return the old length.
    float NormalizeInPlace();

    // Compare length.
    bool IsLengthGreaterThan(float val) const;
    bool IsLengthLessThan(float    val) const;

    // Get the distance from this Vector2D to the other one.
    float DistTo(const Vector2D& vOther) const;

    // Get the distance from this Vector2D to the other one squared.
    float DistToSqr(const Vector2D& vOther) const;

    // Copy
    void CopyToArray(float* rgfl) const;

    // Multiply, add, and assign to this (ie: *this = a + b * scalar). This
    // is about 12% faster than the actual Vector2D equation (because it's done per-component
    // rather than per-Vector2D).
    void MulAdd(const Vector2D& a, const Vector2D& b, float scalar);

    // Dot product.
    float Dot(const Vector2D& vOther) const;

    // assignment
    Vector2D& operator=(const Vector2D& vOther);

#ifndef VECTOR_NO_SLOW_OPERATIONS
    // copy constructors
    Vector2D(const Vector2D &vOther);

    // arithmetic operations
    Vector2D operator-(void) const;

    Vector2D operator+(const Vector2D& v) const;
    Vector2D operator-(const Vector2D& v) const;
    Vector2D operator*(const Vector2D& v) const;
    Vector2D operator/(const Vector2D& v) const;
    Vector2D operator+(const int       i1) const;
    Vector2D operator+(const float     fl) const;
    Vector2D operator*(const float     fl) const;
    Vector2D operator/(const float     fl) const;

    // Cross product between two vectors.
    Vector2D Cross(const Vector2D& vOther) const;

    // Returns a Vector2D with the min or max in X, Y, and Z.
    Vector2D Min(const Vector2D& vOther) const;
    Vector2D Max(const Vector2D& vOther) const;

#else

private:
    // No copy constructors allowed if we're in optimal mode
    Vector2D(const Vector2D& vOther);
#endif
};

//-----------------------------------------------------------------------------

const Vector2D vec2_origin(0, 0);
//const Vector2D vec2_invalid(3.40282347E+38F, 3.40282347E+38F);

//-----------------------------------------------------------------------------
// Vector2D related operations
//-----------------------------------------------------------------------------

// Vector2D clear
void Vector2DClear(Vector2D& a);

// Copy
void Vector2DCopy(const Vector2D& src, Vector2D& dst);

// Vector2D arithmetic
void Vector2DAdd(const Vector2D&      a, const Vector2D& b, Vector2D&       result);
void Vector2DSubtract(const Vector2D& a, const Vector2D& b, Vector2D&       result);
void Vector2DMultiply(const Vector2D& a, float           b, Vector2D&       result);
void Vector2DMultiply(const Vector2D& a, const Vector2D& b, Vector2D&       result);
void Vector2DDivide(const Vector2D&   a, float           b, Vector2D&       result);
void Vector2DDivide(const Vector2D&   a, const Vector2D& b, Vector2D&       result);
void Vector2DMA(const Vector2D&       start, float       s, const Vector2D& dir, Vector2D& result);

// Store the min or max of each of x, y, and z into the result.
void Vector2DMin(const Vector2D& a, const Vector2D& b, Vector2D& result);
void Vector2DMax(const Vector2D& a, const Vector2D& b, Vector2D& result);

#define Vector2DExpand( v ) (v).x, (v).y

// Normalization
float Vector2DNormalize(Vector2D& v);

// Length
float Vector2DLength(const Vector2D& v);

// Dot Product
float DotProduct2D(const Vector2D& a, const Vector2D& b);

// Linearly interpolate between two vectors
void Vector2DLerp(const Vector2D& src1, const Vector2D& src2, float t, Vector2D& dest);


//-----------------------------------------------------------------------------
//
// Inlined Vector2D methods
//
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
// constructors
//-----------------------------------------------------------------------------
class vec2_t {
public:
	float x, y;

	vec2_t() {
		x = 0; y = 0;
};
	vec2_t(float X, float Y) {
		x = X; y = Y;
	};
	vec2_t(Vector vec) {
		x = vec.x; y = vec.y;
	}

	inline vec2_t operator*(const float n) const {
		return vec2_t(x * n, y * n);
	}
	inline vec2_t operator/(const float n) const {
		return vec2_t(x / n, y / n);
	}
	inline vec2_t operator+(const vec2_t & v) const {
		return vec2_t(x + v.x, y + v.y);
	}
	inline vec2_t operator-(const vec2_t & v) const {
		return vec2_t(x - v.x, y - v.y);
	}
	inline void operator+=(const vec2_t & v) {
		x += v.x;
		y += v.y;
	}
	inline void operator-=(const vec2_t & v) {
		x -= v.x;
		y -= v.y;
	}

	bool operator==(const vec2_t & v) const {
		return (v.x == x && v.y == y);
	}
	bool operator!=(const vec2_t & v) const {
		return (v.x != x || v.y != y);
	}

	inline float length() {
		return std::sqrt((x * x) + (y * y));
	}
};
inline Vector2D::Vector2D(void)
{
#ifdef _DEBUG
    // Initialize to NAN to catch errors
    //x = y = float_NAN;
#endif
}

inline Vector2D::Vector2D(float X, float Y)
{
    x = X; y = Y;
    Assert(IsValid());
}

inline Vector2D::Vector2D(const float *pFloat)
{
    Assert(pFloat);
    x = pFloat[0]; y = pFloat[1];
    Assert(IsValid());
}


//-----------------------------------------------------------------------------
// copy constructor
//-----------------------------------------------------------------------------

inline Vector2D::Vector2D(const Vector2D &vOther)
{
    Assert(vOther.IsValid());
    x = vOther.x; y = vOther.y;
}

//-----------------------------------------------------------------------------
// initialization
//-----------------------------------------------------------------------------

inline void Vector2D::Init(float ix, float iy)
{
    x = ix; y = iy;
    Assert(IsValid());
}

inline void Vector2D::Random(float minVal, float maxVal)
{
    x = minVal + ((float)rand() / VALVE_RAND_MAX) * (maxVal - minVal);
    y = minVal + ((float)rand() / VALVE_RAND_MAX) * (maxVal - minVal);
}

inline void Vector2DClear(Vector2D& a)
{
    a.x = a.y = 0.0f;
}

//-----------------------------------------------------------------------------
// assignment
//-----------------------------------------------------------------------------

inline Vector2D& Vector2D::operator=(const Vector2D &vOther)
{
    Assert(vOther.IsValid());
    x = vOther.x; y = vOther.y;
    return *this;
}

//-----------------------------------------------------------------------------
// Array access
//-----------------------------------------------------------------------------

inline float& Vector2D::operator[](int i)
{
    Assert((i >= 0) && (i < 2));
    return ((float*)this)[i];
}

inline float Vector2D::operator[](int i) const
{
    Assert((i >= 0) && (i < 2));
    return ((float*)this)[i];
}

//-----------------------------------------------------------------------------
// Base address...
//-----------------------------------------------------------------------------

inline float* Vector2D::Base()
{
    return (float*)this;
}

inline float const* Vector2D::Base() const
{
    return (float const*)this;
}

//-----------------------------------------------------------------------------
// IsValid?
//-----------------------------------------------------------------------------

inline bool Vector2D::IsValid() const
{
    return IsFinite(x) && IsFinite(y);
}

//-----------------------------------------------------------------------------
// comparison
//-----------------------------------------------------------------------------

inline bool Vector2D::operator==(const Vector2D& src) const
{
    Assert(src.IsValid() && IsValid());
    return (src.x == x) && (src.y == y);
}

inline bool Vector2D::operator!=(const Vector2D& src) const
{
    Assert(src.IsValid() && IsValid());
    return (src.x != x) || (src.y != y);
}


//-----------------------------------------------------------------------------
// Copy
//-----------------------------------------------------------------------------

inline void Vector2DCopy(const Vector2D& src, Vector2D& dst)
{
    Assert(src.IsValid());
    dst.x = src.x;
    dst.y = src.y;
}

inline void	Vector2D::CopyToArray(float* rgfl) const
{
    Assert(IsValid());
    Assert(rgfl);
    rgfl[0] = x; rgfl[1] = y;
}

//-----------------------------------------------------------------------------
// standard math operations
//-----------------------------------------------------------------------------

inline void Vector2D::Negate()
{
    Assert(IsValid());
    x = -x; y = -y;
}

inline Vector2D& Vector2D::operator+=(const Vector2D& v)
{
    Assert(IsValid() && v.IsValid());
    x += v.x; y += v.y;
    return *this;
}

inline Vector2D& Vector2D::operator-=(const Vector2D& v)
{
    Assert(IsValid() && v.IsValid());
    x -= v.x; y -= v.y;
    return *this;
}

inline Vector2D& Vector2D::operator*=(float fl)
{
    x *= fl;
    y *= fl;
    Assert(IsValid());
    return *this;
}

inline Vector2D& Vector2D::operator*=(const Vector2D& v)
{
    x *= v.x;
    y *= v.y;
    Assert(IsValid());
    return *this;
}

inline Vector2D& Vector2D::operator/=(float fl)
{
    Assert(fl != 0.0f);
    float oofl = 1.0f / fl;
    x *= oofl;
    y *= oofl;
    Assert(IsValid());
    return *this;
}

inline Vector2D& Vector2D::operator/=(const Vector2D& v)
{
    Assert(v.x != 0.0f && v.y != 0.0f);
    x /= v.x;
    y /= v.y;
    Assert(IsValid());
    return *this;
}

inline void Vector2DAdd(const Vector2D& a, const Vector2D& b, Vector2D& c)
{
    Assert(a.IsValid() && b.IsValid());
    c.x = a.x + b.x;
    c.y = a.y + b.y;
}

inline void Vector2DAdd(const Vector2D& a, const int b, Vector2D& c)
{
    Assert(a.IsValid());
    c.x = a.x + b;
    c.y = a.y + b;
}

inline void Vector2DAdd(const Vector2D& a, const float b, Vector2D& c)
{
    Assert(a.IsValid());
    c.x = a.x + b;
    c.y = a.y + b;
}

inline void Vector2DSubtract(const Vector2D& a, const Vector2D& b, Vector2D& c)
{
    Assert(a.IsValid() && b.IsValid());
    c.x = a.x - b.x;
    c.y = a.y - b.y;
}

inline void Vector2DMultiply(const Vector2D& a, const float b, Vector2D& c)
{
    Assert(a.IsValid() && IsFinite(b));
    c.x = a.x * b;
    c.y = a.y * b;
}

inline void Vector2DMultiply(const Vector2D& a, const Vector2D& b, Vector2D& c)
{
    Assert(a.IsValid() && b.IsValid());
    c.x = a.x * b.x;
    c.y = a.y * b.y;
}


inline void Vector2DDivide(const Vector2D& a, const float b, Vector2D& c)
{
    Assert(a.IsValid());
    Assert(b != 0.0f);
    float oob = 1.0f / b;
    c.x = a.x * oob;
    c.y = a.y * oob;
}

inline void Vector2DDivide(const Vector2D& a, const Vector2D& b, Vector2D& c)
{
    Assert(a.IsValid());
    Assert((b.x != 0.0f) && (b.y != 0.0f));
    c.x = a.x / b.x;
    c.y = a.y / b.y;
}

inline void Vector2DMA(const Vector2D& start, float s, const Vector2D& dir, Vector2D& result)
{
    Assert(start.IsValid() && IsFinite(s) && dir.IsValid());
    result.x = start.x + s*dir.x;
    result.y = start.y + s*dir.y;
}

// FIXME: Remove
// For backwards compatability
inline void	Vector2D::MulAdd(const Vector2D& a, const Vector2D& b, float scalar)
{
    x = a.x + b.x * scalar;
    y = a.y + b.y * scalar;
}

inline void Vector2DLerp(const Vector2D& src1, const Vector2D& src2, float t, Vector2D& dest)
{
    dest[0] = src1[0] + (src2[0] - src1[0]) * t;
    dest[1] = src1[1] + (src2[1] - src1[1]) * t;
}

//-----------------------------------------------------------------------------
// dot, cross
//-----------------------------------------------------------------------------
inline float DotProduct2D(const Vector2D& a, const Vector2D& b)
{
    Assert(a.IsValid() && b.IsValid());
    return(a.x*b.x + a.y*b.y);
}

// for backwards compatability
inline float Vector2D::Dot(const Vector2D& vOther) const
{
    return DotProduct2D(*this, vOther);
}


//-----------------------------------------------------------------------------
// length
//-----------------------------------------------------------------------------
inline float Vector2DLength(const Vector2D& v)
{
    Assert(v.IsValid());
    return (float)FastSqrt(v.x*v.x + v.y*v.y);
}

inline float Vector2D::LengthSqr(void) const
{
    Assert(IsValid());
    return (x*x + y*y);
}

inline float Vector2D::NormalizeInPlace()
{
    return Vector2DNormalize(*this);
}

inline bool Vector2D::IsLengthGreaterThan(float val) const
{
    return LengthSqr() > val*val;
}

inline bool Vector2D::IsLengthLessThan(float val) const
{
    return LengthSqr() < val*val;
}

inline float Vector2D::Length(void) const
{
    return Vector2DLength(*this);
}


inline void Vector2DMin(const Vector2D &a, const Vector2D &b, Vector2D &result)
{
    result.x = (a.x < b.x) ? a.x : b.x;
    result.y = (a.y < b.y) ? a.y : b.y;
}


inline void Vector2DMax(const Vector2D &a, const Vector2D &b, Vector2D &result)
{
    result.x = (a.x > b.x) ? a.x : b.x;
    result.y = (a.y > b.y) ? a.y : b.y;
}


//-----------------------------------------------------------------------------
// Normalization
//-----------------------------------------------------------------------------
inline float Vector2DNormalize(Vector2D& v)
{
    Assert(v.IsValid());
    float l = v.Length();
    if (l != 0.0f)
    {
        v /= l;
    }
    else
    {
        v.x = v.y = 0.0f;
    }
    return l;
}


//-----------------------------------------------------------------------------
// Get the distance from this Vector2D to the other one
//-----------------------------------------------------------------------------
inline float Vector2D::DistTo(const Vector2D &vOther) const
{
    Vector2D delta;
    Vector2DSubtract(*this, vOther, delta);
    return delta.Length();
}

inline float Vector2D::DistToSqr(const Vector2D &vOther) const
{
    Vector2D delta;
    Vector2DSubtract(*this, vOther, delta);
    return delta.LengthSqr();
}


//-----------------------------------------------------------------------------
// Computes the closest point to vecTarget no farther than flMaxDist from vecStart
//-----------------------------------------------------------------------------
inline void ComputeClosestPoint2D(const Vector2D& vecStart, float flMaxDist, const Vector2D& vecTarget, Vector2D *pResult)
{
    Vector2D vecDelta;
    Vector2DSubtract(vecTarget, vecStart, vecDelta);
    float flDistSqr = vecDelta.LengthSqr();
    if (flDistSqr <= flMaxDist * flMaxDist)
    {
        *pResult = vecTarget;
    }
    else
    {
        vecDelta /= FastSqrt(flDistSqr);
        Vector2DMA(vecStart, flMaxDist, vecDelta, *pResult);
    }
}



//-----------------------------------------------------------------------------
//
// Slow methods
//
//-----------------------------------------------------------------------------

#ifndef VECTOR_NO_SLOW_OPERATIONS
#endif
//-----------------------------------------------------------------------------
// Returns a Vector2D with the min or max in X, Y, and Z.
//-----------------------------------------------------------------------------

inline Vector2D Vector2D::Min(const Vector2D &vOther) const
{
    return Vector2D(x < vOther.x ? x : vOther.x,
        y < vOther.y ? y : vOther.y);
}

inline Vector2D Vector2D::Max(const Vector2D &vOther) const
{
    return Vector2D(x > vOther.x ? x : vOther.x,
        y > vOther.y ? y : vOther.y);
}


//-----------------------------------------------------------------------------
// arithmetic operations
//-----------------------------------------------------------------------------

inline Vector2D Vector2D::operator-(void) const
{
    return Vector2D(-x, -y);
}

inline Vector2D Vector2D::operator+(const Vector2D& v) const
{
    Vector2D res;
    Vector2DAdd(*this, v, res);
    return res;
}

inline Vector2D Vector2D::operator-(const Vector2D& v) const
{
    Vector2D res;
    Vector2DSubtract(*this, v, res);
    return res;
}

inline Vector2D Vector2D::operator+(const int i1) const
{
    Vector2D res;
    Vector2DAdd(*this, i1, res);
    return res;
}

inline Vector2D Vector2D::operator+(const float fl) const
{
    Vector2D res;
    Vector2DAdd(*this, fl, res);
    return res;
}

inline Vector2D Vector2D::operator*(const float fl) const
{
    Vector2D res;
    Vector2DMultiply(*this, fl, res);
    return res;
}

inline Vector2D Vector2D::operator*(const Vector2D& v) const
{
    Vector2D res;
    Vector2DMultiply(*this, v, res);
    return res;
}

inline Vector2D Vector2D::operator/(const float fl) const
{
    Vector2D res;
    Vector2DDivide(*this, fl, res);
    return res;
}

inline Vector2D Vector2D::operator/(const Vector2D& v) const
{
    Vector2D res;
    Vector2DDivide(*this, v, res);
    return res;
}

inline Vector2D operator*(const float fl, const Vector2D& v)
{
    return v * fl;
}

class QAngleByValue;
class QAngle
{
public:
    // Members
    float x, y, z;

    // Construction/destruction
    QAngle(void);
    QAngle(float X, float Y, float Z);
    //      QAngle(RadianEuler const &angles);      // evil auto type promotion!!!

    // Allow pass-by-value
    operator QAngleByValue &() { return *((QAngleByValue *)(this)); }
    operator const QAngleByValue &() const { return *((const QAngleByValue *)(this)); }

    // Initialization
    void Init(float ix = 0.0f, float iy = 0.0f, float iz = 0.0f);
    void Random(float minVal, float maxVal);

    // Got any nasty NAN's?
    bool IsValid() const;
    void Invalidate();

    // array access...
    float operator[](int i) const;
    float& operator[](int i);

    // Base address...
    float* Base();
    float const* Base() const;

    // equality
    bool operator==(const QAngle& v) const;
    bool operator!=(const QAngle& v) const;

    // arithmetic operations
    QAngle& operator+=(const QAngle &v);
    QAngle& operator-=(const QAngle &v);
    QAngle& operator*=(float s);
    QAngle& operator/=(float s);

    // Get the vector's magnitude.
    float   Length() const;
    float   LengthSqr() const;

    // negate the QAngle components
    //void  Negate();

    // No assignment operators either...
    QAngle& operator=(const QAngle& src);

#ifndef VECTOR_NO_SLOW_OPERATIONS
    // copy constructors

    // arithmetic operations
    QAngle  operator-(void) const;

    QAngle  operator+(const QAngle& v) const;
    QAngle  operator-(const QAngle& v) const;
    QAngle  operator*(float fl) const;
    QAngle  operator*(const QAngle& v) const;
    QAngle  operator/(float fl) const;
#else

private:
    // No copy constructors allowed if we're in optimal mode
    QAngle(const QAngle& vOther);

#endif
};

//-----------------------------------------------------------------------------
// constructors
//-----------------------------------------------------------------------------
inline QAngle::QAngle(void)
{
#ifdef _DEBUG
#ifdef VECTOR_PARANOIA
    // Initialize to NAN to catch errors
    x = y = z = VEC_T_NAN;
#endif
#endif
}

inline QAngle::QAngle(float X, float Y, float Z)
{
    x = X; y = Y; z = Z;
    CHECK_VALID(*this);
}

//-----------------------------------------------------------------------------
// initialization
//-----------------------------------------------------------------------------
inline void QAngle::Init(float ix, float iy, float iz)
{
    x = ix; y = iy; z = iz;
    CHECK_VALID(*this);
}

inline void QAngle::Random(float minVal, float maxVal)
{
    x = minVal + ((float)rand() / RAND_MAX) * (maxVal - minVal);
    y = minVal + ((float)rand() / RAND_MAX) * (maxVal - minVal);
    z = minVal + ((float)rand() / RAND_MAX) * (maxVal - minVal);
    CHECK_VALID(*this);
}

//-----------------------------------------------------------------------------
// assignment
//-----------------------------------------------------------------------------
inline QAngle& QAngle::operator=(const QAngle &vOther)
{
    CHECK_VALID(vOther);
    x = vOther.x; y = vOther.y; z = vOther.z;
    return *this;
}

//-----------------------------------------------------------------------------
// comparison
//-----------------------------------------------------------------------------
inline bool QAngle::operator==(const QAngle& src) const
{
    CHECK_VALID(src);
    CHECK_VALID(*this);
    return (src.x == x) && (src.y == y) && (src.z == z);
}

inline bool QAngle::operator!=(const QAngle& src) const
{
    CHECK_VALID(src);
    CHECK_VALID(*this);
    return (src.x != x) || (src.y != y) || (src.z != z);
}

//-----------------------------------------------------------------------------
// standard math operations
//-----------------------------------------------------------------------------
inline QAngle& QAngle::operator+=(const QAngle& v)
{
    CHECK_VALID(*this);
    CHECK_VALID(v);
    x += v.x; y += v.y; z += v.z;
    return *this;
}

inline QAngle& QAngle::operator-=(const QAngle& v)
{
    CHECK_VALID(*this);
    CHECK_VALID(v);
    x -= v.x; y -= v.y; z -= v.z;
    return *this;
}

inline QAngle& QAngle::operator*=(float fl)
{
    x *= fl;
    y *= fl;
    z *= fl;
    CHECK_VALID(*this);
    return *this;
}

inline QAngle& QAngle::operator/=(float fl)
{
    Assert(fl != 0.0f);
    float oofl = 1.0f / fl;
    x *= oofl;
    y *= oofl;
    z *= oofl;
    CHECK_VALID(*this);
    return *this;
}

//-----------------------------------------------------------------------------
// Base address...
//-----------------------------------------------------------------------------
inline float* QAngle::Base()
{
    return (float*)this;
}

inline float const* QAngle::Base() const
{
    return (float const*)this;
}

//-----------------------------------------------------------------------------
// Array access
//-----------------------------------------------------------------------------
inline float& QAngle::operator[](int i)
{
    Assert((i >= 0) && (i < 3));
    return ((float*)this)[i];
}

inline float QAngle::operator[](int i) const
{
    Assert((i >= 0) && (i < 3));
    return ((float*)this)[i];
}

//-----------------------------------------------------------------------------
// length
//-----------------------------------------------------------------------------
inline float QAngle::Length() const
{
    CHECK_VALID(*this);
    return (float)FastSqrt(LengthSqr());
}


inline float QAngle::LengthSqr() const
{
    CHECK_VALID(*this);
    return x * x + y * y + z * z;
}


//-----------------------------------------------------------------------------
// arithmetic operations (SLOW!!)
//-----------------------------------------------------------------------------
#ifndef VECTOR_NO_SLOW_OPERATIONS

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

inline QAngle QAngle::operator+(const QAngle& v) const
{
    QAngle res;
    res.x = x + v.x;
    res.y = y + v.y;
    res.z = z + v.z;
    return res;
}

inline QAngle QAngle::operator-(const QAngle& v) const
{
    QAngle res;
    res.x = x - v.x;
    res.y = y - v.y;
    res.z = z - v.z;
    return res;
}

inline QAngle QAngle::operator*(float fl) const
{
    QAngle res;
    res.x = x * fl;
    res.y = y * fl;
    res.z = z * fl;
    return res;
}

inline QAngle QAngle::operator*(const QAngle& v) const
{
    QAngle res;
    res.x = x * v.x;
    res.y = y * v.y;
    res.z = z * v.z;
    return res;
}

inline QAngle QAngle::operator/(float fl) const
{
    QAngle res;
    res.x = x / fl;
    res.y = y / fl;
    res.z = z / fl;
    return res;
}

inline QAngle operator*(float fl, const QAngle& v)
{
    return v * fl;
}

#endif // VECTOR_NO_SLOW_OPERATIONS


//QANGLE SUBTRAC
inline void QAngleSubtract(const QAngle& a, const QAngle& b, QAngle& c)
{
    CHECK_VALID(a);
    CHECK_VALID(b);
    c.x = a.x - b.x;
    c.y = a.y - b.y;
    c.z = a.z - b.z;
}

//QANGLEADD
inline void QAngleAdd(const QAngle& a, const QAngle& b, QAngle& c)
{
    CHECK_VALID(a);
    CHECK_VALID(b);
    c.x = a.x + b.x;
    c.y = a.y + b.y;
    c.z = a.z + b.z;
}