openmohaa/code/qcommon/vector.h

/*
===========================================================================
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.

#ifndef __VECTOR_H__
#define __VECTOR_H__

#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 float const * () 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 float const * ( 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 )
{
	float	length, ilength;

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

	return length;
}

//----------------------------------------------------------------
// 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;

	if ( ( x == 0.0f ) && ( y == 0.0f ) )
	{
		if ( z > 0.0f )
		{
			pitch = 90.0f;
		}
		else
		{
			pitch = 270.0f;
		}
	}
	else
	{
		forward = ( float )sqrt( ( x * x ) + ( y * y ) );
		pitch = ( float )( ( int )( atan2( z, forward ) * 180.0f / 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.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 ( 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

#endif /* Vector.h */