#ifndef VECTORS_H
#define VECTORS_H

#include <math.h>
#include <string.h>

#ifndef PI
#define PI				3.14159265358979323846		// matches value in gcc v2 math.h
#endif

typedef float vec_t;

#define EQUAL_EPSILON (1/65536.f)


#define DEG2RAD(d) ((d)*PI/180.f)
#define RAD2DEG(r) ((r)*180.f/PI)

struct Vector3;
struct EulerRotation;
struct Angle;

struct Vector2
{
	vec_t X, Y;

	Vector2 ()
	{
	}

	Vector2 (vec_t a, vec_t b)
		: X(a), Y(b)
	{
	}

	Vector2 (const Vector2 &other)
		: X(other.X), Y(other.Y)
	{
	}

	Vector2 (const Vector3 &other);	// Copy the X and Y from the 3D vector and discard the Z

	void Zero()
	{
		Y = X = 0;
	}

	Vector2 &operator= (const Vector2 &other)
	{
		// This might seem backwards, but this helps produce smaller code when a newly
		// created vector is assigned, because the components can just be popped off
		// the FPU stack in order without the need for fxch. For platforms with a
		// more sensible registered-based FPU, of course, the order doesn't matter.
		// (And, yes, I know fxch can improve performance in the right circumstances,
		// but this isn't one of those times. Here, it's little more than a no-op that
		// makes the exe 2 bytes larger whenever you assign one vector to another.)
		Y = other.Y, X = other.X;
		return *this;
	}

	// Access X and Y as an array
	vec_t &operator[] (int index)
	{
		return *(&X + index);
	}

	const vec_t &operator[] (int index) const
	{
		return *(&X + index);
	}

	// Test for equality
	bool operator== (const Vector2 &other) const
	{
		return fabs(X - other.X) < EQUAL_EPSILON && fabs(Y - other.Y) < EQUAL_EPSILON;
	}

	// Test for inequality
	bool operator!= (const Vector2 &other) const
	{
		return fabs(X - other.X) >= EQUAL_EPSILON && fabs(Y - other.Y) >= EQUAL_EPSILON;
	}

	// Unary negation
	Vector2 operator- () const
	{
		return Vector2(-X, -Y);
	}

	// Scalar addition
	Vector2 &operator+= (vec_t scalar)
	{
		X += scalar, Y += scalar;
		return *this;
	}

	friend Vector2 operator+ (const Vector2 &v, vec_t scalar)
	{
		return Vector2(v.X + scalar, v.Y + scalar);
	}

	friend Vector2 operator+ (vec_t scalar, const Vector2 &v)
	{
		return Vector2(v.X + scalar, v.Y + scalar);
	}

	// Scalar subtraction
	Vector2 &operator-= (vec_t scalar)
	{
		X -= scalar, Y -= scalar;
		return *this;
	}

	Vector2 operator- (vec_t scalar) const
	{
		return Vector2(X - scalar, Y - scalar);
	}

	// Scalar multiplication
	Vector2 &operator*= (vec_t scalar)
	{
		X *= scalar, Y *= scalar;
		return *this;
	}

	friend Vector2 operator* (const Vector2 &v, vec_t scalar)
	{
		return Vector2(v.X * scalar, v.Y * scalar);
	}

	friend Vector2 operator* (vec_t scalar, const Vector2 &v)
	{
		return Vector2(v.X * scalar, v.Y * scalar);
	}

	// Scalar division
	Vector2 &operator/= (vec_t scalar)
	{
		scalar = 1 / scalar, X *= scalar, Y *= scalar;
		return *this;
	}

	Vector2 operator/ (vec_t scalar) const
	{
		scalar = 1 / scalar;
		return Vector2(X * scalar, Y * scalar);
	}

	// Vector addition
	Vector2 &operator+= (const Vector2 &other)
	{
		X += other.X, Y += other.Y;
		return *this;
	}

	Vector2 operator+ (const Vector2 &other) const
	{
		return Vector2(X + other.X, Y + other.Y);
	}

	// Vector subtraction
	Vector2 &operator-= (const Vector2 &other)
	{
		X -= other.X, Y -= other.Y;
		return *this;
	}

	Vector2 operator- (const Vector2 &other) const
	{
		return Vector2(X - other.X, Y - other.Y);
	}

	// Vector length
	vec_t Length() const
	{
		return (vec_t)sqrt (X*X + Y*Y);
	}

	vec_t LengthSquared() const
	{
		return X*X + Y*Y;
	}

	// Return a unit vector facing the same direction as this one
	Vector2 Unit() const
	{
		vec_t len = Length();
		if (len != 0) len = 1 / len;
		return *this * len;
	}

	// Scales this vector into a unit vector
	void MakeUnit()
	{
		vec_t len = Length();
		if (len != 0) len = 1 / len;
		*this *= len;
	}

	// Dot product
	vec_t Dot (const Vector2 &other) const
	{
		return X*other.X + Y*other.Y;
	}

	// Returns the angle (in radians) that the ray (0,0)-(X,Y) faces
	vec_t Angle() const
	{
		return (vec_t)atan2 (X, Y);
	}

	// Returns a rotated vector. Angle is in radians.
	Vector2 Rotated (double angle)
	{
		double cosval = cos (angle);
		double sinval = sin (angle);
		return Vector2(vec_t(X*cosval - Y*sinval), vec_t(Y*cosval + X*sinval));
	}

	// Returns a vector rotated 90 degrees clockwise.
	Vector2 Rotated90CW()
	{
		return Vector2(Y, -X);
	}

	// Returns a vector rotated 90 degrees counterclockwise.
	Vector2 Rotated90CCW()
	{
		return Vector2(-Y, X);
	}
};

struct Vector3
{
	vec_t X, Y, Z;

	Vector3 ()
	{
	}

	Vector3 (vec_t a, vec_t b, vec_t c)
		: X(a), Y(b), Z(c)
	{
	}

	Vector3 (const Vector3 &other)
		: X(other.X), Y(other.Y), Z(other.Z)
	{
	}

	Vector3 (const Vector2 &xy, vec_t z)
		: X(xy.X), Y(xy.Y), Z(z)
	{
	}

	Vector3 (const EulerRotation &rot);

	void Zero()
	{
		Z = Y = X = 0;
	}

	Vector3 &operator= (const Vector3 &other)
	{
		Z = other.Z, Y = other.Y, X = other.X;
		return *this;
	}

	// Access X and Y and Z as an array
	vec_t &operator[] (int index)
	{
		return *(&X + index);
	}

	const vec_t &operator[] (int index) const
	{
		return *(&X + index);
	}

	// Test for equality
	bool operator== (const Vector3 &other) const
	{
		return fabs(X - other.X) < EQUAL_EPSILON && fabs(Y - other.Y) < EQUAL_EPSILON && fabs(Z - other.Z) < EQUAL_EPSILON;
	}

	// Test for inequality
	bool operator!= (const Vector3 &other) const
	{
		return fabs(X - other.X) >= EQUAL_EPSILON && fabs(Y - other.Y) >= EQUAL_EPSILON && fabs(Z - other.Z) >= EQUAL_EPSILON;
	}

	// Unary negation
	Vector3 operator- () const
	{
		return Vector3(-X, -Y, -Z);
	}

	// Scalar addition
	Vector3 &operator+= (vec_t scalar)
	{
		X += scalar, Y += scalar, Z += scalar;
		return *this;
	}

	friend Vector3 operator+ (const Vector3 &v, vec_t scalar)
	{
		return Vector3(v.X + scalar, v.Y + scalar, v.Z + scalar);
	}

	friend Vector3 operator+ (vec_t scalar, const Vector3 &v)
	{
		return Vector3(v.X + scalar, v.Y + scalar, v.Z + scalar);
	}

	// Scalar subtraction
	Vector3 &operator-= (vec_t scalar)
	{
		X -= scalar, Y -= scalar, Z -= scalar;
		return *this;
	}

	Vector3 operator- (vec_t scalar) const
	{
		return Vector3(X - scalar, Y - scalar, Z - scalar);
	}

	// Scalar multiplication
	Vector3 &operator*= (vec_t scalar)
	{
		X *= scalar, Y *= scalar, Z *= scalar;
		return *this;
	}

	friend Vector3 operator* (const Vector3 &v, vec_t scalar)
	{
		return Vector3(v.X * scalar, v.Y * scalar, v.Z * scalar);
	}

	friend Vector3 operator* (vec_t scalar, const Vector3 &v)
	{
		return Vector3(v.X * scalar, v.Y * scalar, v.Z * scalar);
	}

	// Scalar division
	Vector3 &operator/= (vec_t scalar)
	{
		scalar = 1 / scalar, X *= scalar, Y *= scalar, Z *= scalar;
		return *this;
	}

	Vector3 operator/ (vec_t scalar) const
	{
		scalar = 1 / scalar;
		return Vector3(X * scalar, Y * scalar, Z * scalar);
	}

	// Vector addition
	Vector3 &operator+= (const Vector3 &other)
	{
		X += other.X, Y += other.Y, Z += other.Z;
		return *this;
	}

	Vector3 operator+ (const Vector3 &other) const
	{
		return Vector3(X + other.X, Y + other.Y, Z + other.Z);
	}

	// Vector subtraction
	Vector3 &operator-= (const Vector3 &other)
	{
		X -= other.X, Y -= other.Y, Z - other.Z;
		return *this;
	}

	Vector3 operator- (const Vector3 &other) const
	{
		return Vector3(X - other.X, Y - other.Y, Z - other.Z);
	}

	// Add a 2D vector to this 3D vector, leaving Z unchanged.
	Vector3 &operator+= (const Vector2 &other)
	{
		X += other.X, Y += other.Y;
		return *this;
	}

	// Subtract a 2D vector from this 3D vector, leaving Z unchanged.
	Vector3 &operator-= (const Vector2 &other)
	{
		X -= other.X, Y -= other.Y;
		return *this;
	}

	// Add a 3D vector and a 2D vector.
	// Discards the Z component of the 3D vector and returns a 2D vector.
	friend Vector2 operator+ (const Vector3 &v3, const Vector2 &v2)
	{
		return Vector2(v3.X + v2.X, v3.Y + v2.Y);
	}

	friend Vector2 operator+ (const Vector2 &v2, const Vector3 &v3)
	{
		return Vector2(v2.X + v3.X, v2.Y + v3.Y);
	}

	// Subtract a 3D vector and a 2D vector.
	// Discards the Z component of the 3D vector and returns a 2D vector.
	friend Vector2 operator- (const Vector3 &v3, const Vector2 &v2)
	{
		return Vector2(v3.X - v2.X, v3.Y - v2.Y);
	}

	friend Vector2 operator- (const Vector2 &v2, const Vector3 &v3)
	{
		return Vector2(v2.X - v3.X, v2.Y - v3.Y);
	}

	// Vector length
	vec_t Length() const
	{
		return (vec_t)sqrt (X*X + Y*Y + Z*Z);
	}

	vec_t LengthSquared() const
	{
		return X*X + Y*Y + Z*Z;
	}

	// Return a unit vector facing the same direction as this one
	Vector3 Unit() const
	{
		vec_t len = Length();
		if (len != 0) len = 1 / len;
		return *this * len;
	}

	// Scales this vector into a unit vector
	void MakeUnit()
	{
		vec_t len = Length();
		if (len != 0) len = 1 / len;
		*this *= len;
	}

	// Resizes this vector to be the specified length (if it is not 0)
	Vector3 &Resize(vec_t len)
	{
		vec_t nowlen = Length();
		X *= (len /= nowlen);
		Y *= len;
		Z *= len;
		return *this;
	}

	// Dot product
	vec_t Dot (const Vector3 &other) const
	{
		return X*other.X + Y*other.Y + Z*other.Z;
	}

	// Cross product
	Vector3 Cross (const Vector3 &other) const
	{
		return Vector3(Y*other.Z - Z*other.Y,
					   Z*other.X - X*other.Z,
					   X*other.Y - Y*other.X);
	}
};

inline Vector2::Vector2 (const Vector3 &other)
: X(other.X), Y(other.Y)
{
}

struct Matrix3x3
{
	vec_t Cells[3][3];

	Matrix3x3()
	{
	}

	Matrix3x3(const Matrix3x3 &other)
	{
		(*this)[0] = other[0];
		(*this)[1] = other[1];
		(*this)[2] = other[2];
	}

	Matrix3x3(const Vector3 &row1, const Vector3 &row2, const Vector3 &row3)
	{
		(*this)[0] = row1;
		(*this)[1] = row2;
		(*this)[2] = row3;
	}

	// Construct a rotation matrix about an arbitrary axis.
	// (The axis vector must be normalized.)
	Matrix3x3(const Vector3 &axis, double radians)
	{
		double c = cos(radians), s = sin(radians), t = 1 - c;
/* In comments: A more readable version of the matrix setup.
This was found in Diana Gruber's article "The Mathematics of the
3D Rotation Matrix" at <http://www.makegames.com/3drotation/> and is
attributed to Graphics Gems (Glassner, Academic Press, 1990).

		Cells[0][0] = t*axis.X*axis.X + c;
		Cells[0][1] = t*axis.X*axis.Y - s*axis.Z;
		Cells[0][2] = t*axis.X*axis.Z + s*axis.Y;

		Cells[1][0] = t*axis.Y*axis.X + s*axis.Z;
		Cells[1][1] = t*axis.Y*axis.Y + c;
		Cells[1][2] = t*axis.Y*axis.Z - s*axis.X;

		Cells[2][0] = t*axis.Z*axis.X - s*axis.Y;
		Cells[2][1] = t*axis.Z*axis.Y + s*axis.X;
		Cells[2][2] = t*axis.Z*axis.Z + c;

Outside comments: A faster version with only 10 (not 24) multiplies.
*/
		double sx = s*axis.X, sy = s*axis.Y, sz = s*axis.Z;
		double tx, ty, txx, tyy, u, v;

		tx = t*axis.X;
		Cells[0][0] = vec_t( (txx=tx*axis.X) + c );
		Cells[0][1] = vec_t(   (u=tx*axis.Y) - sz);
		Cells[0][2] = vec_t(   (v=tx*axis.Z) + sy);

		ty = t*axis.Y;
		Cells[1][0] = vec_t(              u  + sz);
		Cells[1][1] = vec_t( (tyy=ty*axis.Y) + c );
		Cells[1][2] = vec_t(   (u=ty*axis.Z) - sx);

		Cells[2][0] = vec_t(              v  - sy);
		Cells[2][1] = vec_t(              u  + sx);
		Cells[2][2] = vec_t(     (t-txx-tyy) + c );
	}

	Matrix3x3(const Vector3 &axis, double c/*cosine*/, double s/*sine*/)
	{
		double t = 1 - c;
		double sx = s*axis.X, sy = s*axis.Y, sz = s*axis.Z;
		double tx, ty, txx, tyy, u, v;

		tx = t*axis.X;
		Cells[0][0] = vec_t( (txx=tx*axis.X) + c );
		Cells[0][1] = vec_t(   (u=tx*axis.Y) - sz);
		Cells[0][2] = vec_t(   (v=tx*axis.Z) + sy);

		ty = t*axis.Y;
		Cells[1][0] = vec_t(              u  + sz);
		Cells[1][1] = vec_t( (tyy=ty*axis.Y) + c );
		Cells[1][2] = vec_t(   (u=ty*axis.Z) - sx);

		Cells[2][0] = vec_t(              v  - sy);
		Cells[2][1] = vec_t(              u  + sx);
		Cells[2][2] = vec_t(     (t-txx-tyy) + c );
	}

	Matrix3x3(const Vector3 &axis, Angle degrees);

	void Zero()
	{
		memset (this, 0, sizeof *this);
	}

	void Identity()
	{
		Cells[0][0] = 1; Cells[0][1] = 0; Cells[0][2] = 0;
		Cells[1][0] = 0; Cells[1][1] = 1; Cells[1][2] = 0;
		Cells[2][0] = 0; Cells[2][1] = 0; Cells[2][2] = 1;
	}

	Vector3 &operator[] (int index)
	{
		return *((Vector3 *)&Cells[index]);
	}

	const Vector3 &operator[] (int index) const
	{
		return *((Vector3 *)&Cells[index]);
	}

	// Multiply a scalar
	Matrix3x3 &operator*= (vec_t scalar)
	{
		(*this)[0] *= scalar;
		(*this)[1] *= scalar;
		(*this)[2] *= scalar;
		return *this;
	}

	friend Matrix3x3 operator* (vec_t s, const Matrix3x3 &m)
	{
		return Matrix3x3(m[0]*s, m[1]*s, m[2]*s);
	}

	Matrix3x3 operator* (vec_t s) const
	{
		return Matrix3x3((*this)[0]*s, (*this)[1]*s, (*this)[2]*s);
	}

	// Divide a scalar
	Matrix3x3 &operator/= (vec_t scalar)
	{
		return *this *= 1 / scalar;
	}

	Matrix3x3 operator/ (vec_t s) const
	{
		return *this * (1 / s);
	}

	// Add two 3x3 matrices together
	Matrix3x3 &operator+= (const Matrix3x3 &o)
	{
		(*this)[0] += o[0];
		(*this)[1] += o[1];
		(*this)[2] += o[2];
		return *this;
	}

	Matrix3x3 operator+ (const Matrix3x3 &o) const
	{
		return Matrix3x3((*this)[0] + o[0], (*this)[1] + o[1], (*this)[2] + o[2]);
	}

	// Subtract two 3x3 matrices
	Matrix3x3 &operator-= (const Matrix3x3 &o)
	{
		(*this)[0] -= o[0];
		(*this)[1] -= o[1];
		(*this)[2] -= o[2];
		return *this;
	}

	Matrix3x3 operator- (const Matrix3x3 &o) const
	{
		return Matrix3x3((*this)[0] - o[0], (*this)[1] - o[1], (*this)[2] - o[2]);
	}

	// Concatenate two 3x3 matrices
	Matrix3x3 &operator*= (const Matrix3x3 &o)
	{
		return *this = *this * o;
	}

	Matrix3x3 operator* (const Matrix3x3 &o) const
	{
		return Matrix3x3(
			Vector3(Cells[0][0]*o[0][0] + Cells[0][1]*o[1][0] + Cells[0][2]*o[2][0],
					Cells[0][0]*o[0][1] + Cells[0][1]*o[1][1] + Cells[0][2]*o[2][1],
					Cells[0][0]*o[0][2] + Cells[0][1]*o[1][2] + Cells[0][2]*o[2][2]),
			Vector3(Cells[1][0]*o[0][0] + Cells[1][1]*o[1][0] + Cells[1][2]*o[2][0],
					Cells[1][0]*o[0][1] + Cells[1][1]*o[1][1] + Cells[1][2]*o[2][1],
					Cells[1][0]*o[0][2] + Cells[1][1]*o[1][2] + Cells[1][2]*o[2][2]),
			Vector3(Cells[2][0]*o[0][0] + Cells[2][1]*o[1][0] + Cells[2][2]*o[2][0],
					Cells[2][0]*o[0][1] + Cells[2][1]*o[1][1] + Cells[2][2]*o[2][1],
					Cells[2][0]*o[0][2] + Cells[2][1]*o[1][2] + Cells[2][2]*o[2][2]));
	}

	// Multiply a 3D vector by a rotation matrix
	friend Vector3 operator* (const Vector3 &v, const Matrix3x3 &m)
	{
		return Vector3(m[0].Dot(v), m[1].Dot(v), m[2].Dot(v));
	}

	friend Vector3 operator* (const Matrix3x3 &m, const Vector3 &v)
	{
		return Vector3(m[0].Dot(v), m[1].Dot(v), m[2].Dot(v));
	}
};

struct Angle
{
	vec_t Degrees;

	Angle ()
	{
	}

	Angle (float amt)
		: Degrees(amt)
	{
	}

	Angle (double amt)
		: Degrees(vec_t(amt))
	{
	}

	Angle (int amt)
		: Degrees(vec_t(amt))
	{
	}

	Angle (const Angle &other)
		: Degrees(other.Degrees)
	{
	}

	Angle &operator= (const Angle &other)
	{
		Degrees = other.Degrees;
		return *this;
	}

	Angle &operator= (vec_t other)
	{
		Degrees = other;
		return *this;
	}

	operator float() const { return Degrees; }
	operator double() const { return Degrees; }

	Angle operator- () const
	{
		return Angle(-Degrees);
	}

	Angle &operator+= (Angle other)
	{
		Degrees += other.Degrees;
		return *this;
	}

	Angle &operator-= (Angle other)
	{
		Degrees -= other.Degrees;
		return *this;
	}

	Angle &operator*= (Angle other)
	{
		Degrees *= other.Degrees;
		return *this;
	}

	Angle &operator/= (Angle other)
	{
		Degrees /= other.Degrees;
		return *this;
	}

	Angle operator+ (Angle other) const
	{
		return Degrees + other.Degrees;
	}

	Angle operator- (Angle other) const
	{
		return Degrees - other.Degrees;
	}

	Angle operator* (Angle other) const
	{
		return Degrees * other.Degrees;
	}

	Angle operator/ (Angle other) const
	{
		return Degrees / other.Degrees;
	}

	Angle &operator+= (double other)
	{
		Degrees += vec_t(other);
		return *this;
	}

	Angle &operator-= (double other)
	{
		Degrees -= vec_t(other);
		return *this;
	}

	Angle &operator*= (double other)
	{
		Degrees *= vec_t(other);
		return *this;
	}

	Angle &operator/= (double other)
	{
		Degrees /= vec_t(other);
		return *this;
	}

	Angle operator+ (double other) const
	{
		return Degrees + vec_t(other);
	}

	Angle operator- (double other) const
	{
		return Degrees - vec_t(other);
	}

	friend Angle operator- (double o1, Angle o2)
	{
		return Angle(vec_t(o1) - o2.Degrees);
	}

	Angle operator* (double other) const
	{
		return Degrees * vec_t(other);
	}

	Angle operator/ (double other) const
	{
		return Degrees / vec_t(other);
	}

	// Should the comparisons consider an epsilon value?
	bool operator< (Angle other) const
	{
		return Degrees < other.Degrees;
	}

	bool operator> (Angle other) const
	{
		return Degrees > other.Degrees;
	}

	bool operator<= (Angle other) const
	{
		return Degrees <= other.Degrees;
	}

	bool operator>= (Angle other) const
	{
		return Degrees >= other.Degrees;
	}

	bool operator== (Angle other) const
	{
		return Degrees == other.Degrees;
	}

	bool operator!= (Angle other) const
	{
		return Degrees != other.Degrees;
	}

	bool operator< (double other) const
	{
		return Degrees < other;
	}

	bool operator> (double other) const
	{
		return Degrees > other;
	}

	bool operator<= (double other) const
	{
		return Degrees <= other;
	}

	bool operator>= (double other) const
	{
		return Degrees >= other;
	}

	bool operator== (double other) const
	{
		return Degrees == other;
	}

	bool operator!= (double other) const
	{
		return Degrees != other;
	}

	// Ensure the angle is between [0.0,360.0) degrees
	Angle &Normalize360()
	{
		// Normalizing the angle converts it to a BAM, masks it, and converts it back to a float.

		// This could have been kept entirely in floating point using fmod(), but the MSVCRT has lots
		// of overhead for that function, despite the x87 offering the FPREM instruction which does
		// exactly what fmod() is supposed to do. So fmod ends up being an order of magnitude slower
		// than casting to and from an int.

		// Casting Degrees to a volatile ensures that the compiler will not try to evaluate an expression
		// such as "Angle a(360*100+24); a.Normalize360();" at compile time. Normally, it would see that
		// this expression is constant and attempt to remove the Normalize360() call entirely and store
		// the result of the function in the Angle directly. Unfortunately, it does not do the casting
		// properly and will overflow, producing an incorrect result. So we need to make sure it always
		// evaluates Normalize360 at run time and never at compile time. (This applies to VC++. I don't
		// know if other compilers suffer similarly).
		Degrees = vec_t((int(*(volatile vec_t *)&Degrees * ((1<<30)/360.0)) & ((1<<30)-1)) * (360.f/(1<<30)));
		return *this;
	}

	// Ensures the angle is between (-180.0,180.0] degrees
	Angle &Normalize180()
	{
		Degrees = vec_t((((int(*(volatile vec_t *)&Degrees * ((1<<30)/360.0))+(1<<29)-1) & ((1<<30)-1)) - (1<<29)+1) * (360.f/(1<<30)));
		return *this;
	}

	// Like Normalize360(), except the integer value is not converted back to a float.
	// The steps parameter must be a power of 2.
	int Quantize(int steps)
	{
		return int(*(volatile vec_t *)&Degrees * (steps/360.0)) & (steps-1);
	}
};

inline vec_t ToRadians (const Angle &deg)
{
	return vec_t(deg.Degrees * (PI / 180.0));
}

inline Angle ToDegrees (vec_t rad)
{
	return Angle(vec_t(rad * (180.0 / PI)));
}

inline vec_t cos (const Angle &deg)
{
	return (vec_t)cos(ToRadians(deg));
}

inline vec_t sin (const Angle &deg)
{
	return (vec_t)sin(ToRadians(deg));
}

inline vec_t tan (const Angle &deg)
{
	return (vec_t)tan(ToRadians(deg));
}

inline Angle fabs (const Angle &deg)
{
	return Angle(fabs(deg.Degrees));
}

inline Angle vectoyaw (const Vector2 &vec)
{
	return atan2(vec.Y, vec.X) * (180.0 / PI);
}

inline Angle vectoyaw (const Vector3 &vec)
{
	return atan2(vec.Y, vec.X) * (180.0 / PI);
}

// Much of this is copied from Vector3. Is all that functionality really appropriate?
struct EulerRotation
{
	Angle Pitch;	// up/down
	Angle Yaw;		// left/right
	Angle Roll;		// rotation about the forward axis

	EulerRotation ()
	{
	}

	EulerRotation (const Angle &p, const Angle &y, const Angle &r)
		: Pitch(p), Yaw(y), Roll(r)
	{
	}

	EulerRotation (const EulerRotation &other)
		: Pitch(other.Pitch), Yaw(other.Yaw), Roll(other.Roll)
	{
	}

	EulerRotation &operator= (const EulerRotation &other)
	{
		Roll = other.Roll, Yaw = other.Yaw, Pitch = other.Pitch;
		return *this;
	}

	// Access angles as an array
	Angle &operator[] (int index)
	{
		return *(&Pitch + index);
	}

	const Angle &operator[] (int index) const
	{
		return *(&Pitch + index);
	}

	// Test for equality
	bool operator== (const EulerRotation &other) const
	{
		return fabs(Pitch - other.Pitch) < Angle(EQUAL_EPSILON) && fabs(Yaw - other.Yaw) < Angle(EQUAL_EPSILON) && fabs(Roll - other.Roll) < Angle(EQUAL_EPSILON);
	}

	// Test for inequality
	bool operator!= (const EulerRotation &other) const
	{
		return fabs(Pitch - other.Pitch) >= Angle(EQUAL_EPSILON) && fabs(Yaw - other.Yaw) >= Angle(EQUAL_EPSILON) && fabs(Roll - other.Roll) >= Angle(EQUAL_EPSILON);
	}

	// Unary negation
	EulerRotation operator- () const
	{
		return EulerRotation(-Pitch, -Yaw, -Roll);
	}

	// Scalar addition
	EulerRotation &operator+= (const Angle &scalar)
	{
		Pitch += scalar, Yaw += scalar, Roll += scalar;
		return *this;
	}

	friend EulerRotation operator+ (const EulerRotation &v, const Angle &scalar)
	{
		return EulerRotation(v.Pitch + scalar, v.Yaw + scalar, v.Roll + scalar);
	}

	friend EulerRotation operator+ (const Angle &scalar, const EulerRotation &v)
	{
		return EulerRotation(v.Pitch + scalar, v.Yaw + scalar, v.Roll + scalar);
	}

	// Scalar subtraction
	EulerRotation &operator-= (const Angle &scalar)
	{
		Pitch -= scalar, Yaw -= scalar, Roll -= scalar;
		return *this;
	}

	EulerRotation operator- (const Angle &scalar) const
	{
		return EulerRotation(Pitch - scalar, Yaw - scalar, Roll - scalar);
	}

	// Scalar multiplication
	EulerRotation &operator*= (const Angle &scalar)
	{
		Pitch *= scalar, Yaw *= scalar, Roll *= scalar;
		return *this;
	}

	friend EulerRotation operator* (const EulerRotation &v, const Angle &scalar)
	{
		return EulerRotation(v.Pitch * scalar, v.Yaw * scalar, v.Roll * scalar);
	}

	friend EulerRotation operator* (const Angle &scalar, const EulerRotation &v)
	{
		return EulerRotation(v.Pitch * scalar, v.Yaw * scalar, v.Roll * scalar);
	}

	// Scalar division
	EulerRotation &operator/= (const Angle &scalar)
	{
		Angle mul(1 / scalar.Degrees);
		Pitch *= scalar, Yaw *= scalar, Roll *= scalar;
		return *this;
	}

	EulerRotation operator/ (const Angle &scalar) const
	{
		Angle mul(1 / scalar.Degrees);
		return EulerRotation(Pitch * mul, Yaw * mul, Roll * mul);
	}

	// Vector addition
	EulerRotation &operator+= (const EulerRotation &other)
	{
		Pitch += other.Pitch, Yaw += other.Yaw, Roll += other.Roll;
		return *this;
	}

	EulerRotation operator+ (const EulerRotation &other) const
	{
		return EulerRotation(Pitch + other.Pitch, Yaw + other.Yaw, Roll + other.Roll);
	}

	// Vector subtraction
	EulerRotation &operator-= (const EulerRotation &other)
	{
		Pitch -= other.Pitch, Yaw -= other.Yaw, Roll - other.Roll;
		return *this;
	}

	EulerRotation operator- (const EulerRotation &other) const
	{
		return EulerRotation(Pitch - other.Pitch, Yaw - other.Yaw, Roll - other.Roll);
	}

	// Normalize each component
	EulerRotation &Normalize180 ()
	{
		for (int i = -3; i; ++i)
		{
			(*this)[i+3].Normalize180();
		}
		return *this;
	}

	EulerRotation &Normalize360 ()
	{
		for (int i = -3; i; ++i)
		{
			(*this)[i+3].Normalize360();
		}
		return *this;
	}
};

// Create a forward vector from a rotation (ignoring roll)

inline Vector3::Vector3 (const EulerRotation &rot)
: X(cos(rot.Pitch)*cos(rot.Yaw)), Y(cos(rot.Pitch)*sin(rot.Yaw)), Z(-sin(rot.Pitch))
{
}

inline Matrix3x3::Matrix3x3(const Vector3 &axis, Angle degrees)
{
	vec_t c = cos(degrees), s = sin(degrees), t = 1 - c;
	vec_t sx = s*axis.X, sy = s*axis.Y, sz = s*axis.Z;
	vec_t tx, ty, txx, tyy, u, v;

	tx = t*axis.X;
	Cells[0][0] =  (txx=tx*axis.X) + c;
	Cells[0][1] =    (u=tx*axis.Y) - sz;
	Cells[0][2] =    (v=tx*axis.Z) + sy;

	ty = t*axis.Y;
	Cells[1][0] =               u  + sz;
	Cells[1][1] =  (tyy=ty*axis.Y) + c;
	Cells[1][2] =    (u=ty*axis.Z) - sx;

	Cells[2][0] =               v  - sy;
	Cells[2][1] =               u  + sx;
	Cells[2][2] =      (t-txx-tyy) + c;
}

#include "vectorq.h"

#endif
