// ------------------------------------------------------------
// Machinery project - math library
// ------------------------------------------------------------
// (c) 2000 by Sascha 'EvilOne' Salevsky
// E-mail: Sascha.Salevsky@Ibykus.de
// ------------------------------------------------------------

#include "Mathematics.h"

// ------------------------------------------------------------
// Vector2
// ------------------------------------------------------------

ostream& operator<<(ostream& os, Vector2& v)
{
  os << "(" << setprecision(4) << v.x << ", "  << v.y << ")";
  return os;
}

// ------------------------------------------------------------
// Vector3
// ------------------------------------------------------------

ostream& operator<<(ostream& os, Vector3& v)
{
  os << "(" << setprecision(4) << v.x << ", "  << v.y << ", "  << v.z << ")";
  return os;
}

// ------------------------------------------------------------
// Plane
// ------------------------------------------------------------

ostream& operator<<(ostream& os, Plane& p)
{
  os << "(" << setprecision(4) << p.a << ", " 
	 << p.b << ", "  << p.c << ", " << p.d << ")";
  return os;
}

// ------------------------------------------------------------
// Matrix3
// ------------------------------------------------------------

float Matrix3::Identity_Matrix[] =
{
	1.0f, 0.0f, 0.0f,
	0.0f, 1.0f, 0.0f,
	0.0f, 0.0f, 1.0f
};

ostream& operator<<(ostream& os, Matrix3 &m)
{
	os << setprecision(4);
	os << "(" << m.M[0]  << ", " << m.M[1]  << ", " << m.M[2] << ", " << endl
	   << " " << m.M[3]  << ", " << m.M[4]  << ", " << m.M[5] << ", " << endl
	   << " " << m.M[6]  << ", " << m.M[7]  << ", " << m.M[8] << ")";
	return os;
}

// ------------------------------------------------------------
// Matrix4
// ------------------------------------------------------------

float Matrix4::Identity_Matrix[] =
{
	1.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 1.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 1.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 1.0f
};

float Matrix4::Orientation_Switch_Matrix[] =
{
	1.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 1.0f, 0.0f, 0.0f,
	0.0f, 0.0f, -1.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 1.0f
};

float Matrix4::Perspective_Matrix[] =
{
	1.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 1.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 1.0f, -1.0f,
	0.0f, 0.0f, 0.0f, 0.0f
};

void Matrix4::Assign(const Quaternion& other)
{
    float xx = other.x * other.x;
    float xy = other.x * other.y;
    float xz = other.x * other.z;
    float xw = other.x * other.w;

    float yy = other.y * other.y;
    float yz = other.y * other.z;
    float yw = other.y * other.w;

    float zz = other.z * other.z;
    float zw = other.z * other.w;

    M[0]  = 1.0f - 2.0f * (yy + zz);
    M[1]  =        2.0f * (xy - zw);
    M[2]  =        2.0f * (xz + yw);

    M[4]  =        2.0f * (xy + zw);
    M[5]  = 1.0f - 2.0f * (xx + zz);
    M[6]  =        2.0f * (yz - xw);

    M[8]  =        2.0f * (xz - yw);
    M[9]  =        2.0f * (yz + xw);
    M[10] = 1.0f - 2.0f * (xx + yy);

    M[3]  = M[7] = M[11] = M[12] = M[13] = M[14] = 0.0f;
    M[15] = 1.0f;
}


Matrix4& Matrix4::SetRotate(float rx, float ry, float rz)
{
	float a = CosD(rx); float b = SinD(rx);
	float c = CosD(ry); float d = SinD(ry);
	float e = CosD(rz); float f = SinD(rz);
	float ad = a * d;
	float bd = b * d;

	M[0] =  c * e;
	M[1] = -c * f;
	M[2] = -d;

	M[4] = -bd * e + a * f;
	M[5] =  bd * f + a * e;
	M[6] = -b * c;

	M[8] =  ad * e + b * f;
	M[9] = -ad * f + b * e;
	M[10] =  a * c;

	M[3] = M[7] = M[11] = M[12] = M[13] = M[14] = 0.0f;
	M[15] = 1.0f;
	return *this;
}

Matrix4& Matrix4::SetRotate(float angle, float x, float y, float z)
{
	float xx, yy, zz, xy, yz, zx, xs, ys, zs, c_complement;
	float s = SinD(angle);
	float c = SinD(angle);
	float magnitude = (float)sqrt(x * x + y * y + z * z);
	float *data = M;
	if(magnitude == 0.0f)
	{
		SetIdentity();
		return *this;
	}
	x /= magnitude;
	y /= magnitude;
	z /= magnitude;
	xx = x * x;
	yy = y * y;
	zz = z * z;
	xy = x * y;
	yz = y * z;
	zx = z * x;
	xs = x * s;
	ys = y * s;
	zs = z * s;
	c_complement = 1.0F - c;
	data[0] = (c_complement * xx) + c;
	data[4] = (c_complement * xy) - zs;
	data[8] = (c_complement * zx) + ys;
	data[12] = 0.0F;
	data[1] = (c_complement * xy) + zs;
	data[5] = (c_complement * yy) + c;
	data[9] = (c_complement * yz) - xs;
	data[13] = 0.0F;
	data[2] = (c_complement * zx) - ys;
	data[6] = (c_complement * yz) + xs;
	data[10] = (c_complement * zz) + c;
	data[14] = 0.0F;
	data[3] = 0.0F;
	data[7] = 0.0F;
	data[11] = 0.0F;
	data[15] = 1.0F;
	return *this;
}

Matrix4& Matrix4::Transpose()
{
	Transpose(*this);
	return *this;
}

Matrix4& Matrix4::Transpose(const Matrix4& other)
{
	for(int i = 0; i < 4; i++)
	{
		for(int j = 0; j < 4; j++)
		M[i * 4 + j] = other.M[j * 4 + i];
	}
	return *this;
}

Matrix4& Matrix4::Adjoint()
{
	Matrix4 tmp;
	tmp.Assign(*this);
	Adjoint(tmp);
	return *this;
}

Matrix4& Matrix4::Adjoint(const Matrix4& other)
{
	float a1, a2, a3, a4, b1, b2, b3, b4;
	float c1, c2, c3, c4, d1, d2, d3, d4;
	const float *in = other.M;
	float *out = M;

	a1 = (in [(( 0 ) << 2) + (  0 )]) ; b1 = (in [(( 0 ) << 2) + (  1 )]) ;
	c1 = (in [(( 0 ) << 2) + (  2 )]) ; d1 = (in [(( 0 ) << 2) + (  3 )]) ;
	a2 = (in [(( 1 ) << 2) + (  0 )]) ; b2 = (in [(( 1 ) << 2) + (  1 )]) ;
	c2 = (in [(( 1 ) << 2) + (  2 )]) ; d2 = (in [(( 1 ) << 2) + (  3 )]) ;
	a3 = (in [(( 2 ) << 2) + (  0 )]) ; b3 = (in [(( 2 ) << 2) + (  1 )]) ;
	c3 = (in [(( 2 ) << 2) + (  2 )]) ; d3 = (in [(( 2 ) << 2) + (  3 )]) ;
	a4 = (in [(( 3 ) << 2) + (  0 )]) ; b4 = (in [(( 3 ) << 2) + (  1 )]) ;
	c4 = (in [(( 3 ) << 2) + (  2 )]) ; d4 = (in [(( 3 ) << 2) + (  3 )]) ;

	out[(( 0 ) << 2) + (  0 )]   =   Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
	out[(( 1 ) << 2) + (  0 )]   = - Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
	out[(( 2 ) << 2) + (  0 )]   =   Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
	out[(( 3 ) << 2) + (  0 )]   = - Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);

	out[(( 0 ) << 2) + (  1 )]   = - Det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
	out[(( 1 ) << 2) + (  1 )]   =   Det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
	out[(( 2 ) << 2) + (  1 )]   = - Det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
	out[(( 3 ) << 2) + (  1 )]   =   Det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);

	out[(( 0 ) << 2) + (  2 )]   =   Det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
	out[(( 1 ) << 2) + (  2 )]   = - Det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
	out[(( 2 ) << 2) + (  2 )]   =   Det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
	out[(( 3 ) << 2) + (  2 )]   = - Det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);

	out[(( 0 ) << 2) + (  3 )]   = - Det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
	out[(( 1 ) << 2) + (  3 )]   =   Det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
	out[(( 2 ) << 2) + (  3 )]   = - Det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
	out[(( 3 ) << 2) + (  3 )]   =   Det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
	return *this;
}

Matrix4& Matrix4::Invert()
{
	Matrix4 tmp;
	tmp.Assign(*this);
	Invert(tmp);
	return *this;
}

Matrix4& Matrix4::Invert(const Matrix4& other)
{
	int i;
	Adjoint(other);
	float d = other.Det();

	for (i = 0; i < 16; i++) M[i] = M[i] / d;
	return *this;
}

float Matrix4::Det() const
{
	float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
	a1 = (M [(( 0 ) << 2) + (  0 )]) ;
	b1 = (M [(( 0 ) << 2) + (  1 )]) ; 
	c1 = (M [(( 0 ) << 2) + (  2 )]) ;
	d1 = (M [(( 0 ) << 2) + (  3 )]) ;

	a2 = (M [(( 1 ) << 2) + (  0 )]) ;
	b2 = (M [(( 1 ) << 2) + (  1 )]) ; 
	c2 = (M [(( 1 ) << 2) + (  2 )]) ;
	d2 = (M [(( 1 ) << 2) + (  3 )]) ;

	a3 = (M [(( 2 ) << 2) + (  0 )]) ;
	b3 = (M [(( 2 ) << 2) + (  1 )]) ; 
	c3 = (M [(( 2 ) << 2) + (  2 )]) ;
	d3 = (M [(( 2 ) << 2) + (  3 )]) ;

	a4 = (M [(( 3 ) << 2) + (  0 )]) ;
	b4 = (M [(( 3 ) << 2) + (  1 )]) ; 
	c4 = (M [(( 3 ) << 2) + (  2 )]) ;
	d4 = (M [(( 3 ) << 2) + (  3 )]) ;

	return  a1 * Det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4) -
			b1 * Det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4) +
			c1 * Det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4) -
			d1 * Det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
}

Matrix4& Matrix4::SetProjection(float fov, float aspect, float znear, float zfar)
{
	float top = znear * TanD(fov);
	float bottom = -top;
	float left = bottom * aspect;
	float right = top * aspect;
	float x = (2.0f * znear) / (right - left);
	float y = (2.0f * znear) / (top - bottom);
	float a = (right + left) / (right - left);
	float b = (top + bottom) / (top - bottom);
	float c = -(zfar + znear) / (zfar - znear);
	float d = -(2.0f * zfar * znear) / (zfar - znear);
	M[0]  = x;     M[1]  = 0.0f;  M[2]  = 0.0f; M[3]  = 0.0f;
	M[4]  = 0.0f;  M[5]  = y;     M[6]  = 0.0f; M[7]  = 0.0f;
	M[8]  = a;     M[9]  = b;     M[10] = c;    M[11] = -1.0f;
	M[12] = 0.0f;  M[13] = 0.0f;  M[14] = d;    M[15] = 0.0f;
	return *this;
}

Matrix4& Matrix4::SetOthogonal(float znear, float zfar)
{
	float x, y, z;
	float tx, ty, tz;
	float small = 0.0f;
	x = 2.0f / (1.0f + small);
	y = 2.0f / (1.0f + small);
	z = -2.0f / (zfar - znear);
	tx = -(1.0f - small) / (1.0f + small);
	ty = -(1.0f - small) / (1.0f + small);
	tz = -(zfar + znear) / (zfar - znear);
	M[0] = x;    M[4] = 0.0f;  M[8]  = 0.0f;  M[12] = tx;
	M[1] = 0.0f; M[5] = y;     M[9]  = 0.0f;  M[13] = ty;
	M[2] = 0.0f; M[6] = 0.0f;  M[10] = z;     M[14] = tz;
	M[3] = 0.0f; M[7] = 0.0f;  M[11] = 0.0f;  M[15] = 1.0f;
	return *this;
}

float Matrix4::Det2x2(float a, float b, float c, float d)
{
  return a * d - b * c;
}
  
float Matrix4::Det3x3(float a1, float a2, float a3,
                   float b1, float b2, float b3,
                   float c1, float c2, float c3)
{
	return a1 * Det2x2(b2, b3, c2, c3)
		 - b1 * Det2x2(a2, a3, c2, c3)
		 + c1 * Det2x2(a2, a3, b2, b3);
}

ostream& operator<<(ostream& os, Matrix4 &m)
{
	os << setprecision(4);
	os << "(" << m.M[0]  << ", " << m.M[1]  << ", " << m.M[2]  << ", " << m.M[3]  << ", " << endl
	   << " " << m.M[4]  << ", " << m.M[5]  << ", " << m.M[6]  << ", " << m.M[7]  << ", " << endl
	   << " " << m.M[8]  << ", " << m.M[9]  << ", " << m.M[10] << ", " << m.M[11] << ", " << endl
	   << " " << m.M[12] << ", " << m.M[13] << ", " << m.M[14] << ", " << m.M[15] << ")";
	return os;
}

// ------------------------------------------------------------
// Quaternion
// ------------------------------------------------------------

void Quaternion::Slerp(const Quaternion& q0, const Quaternion& q1, float t)
{
	float cosom = q0.x * q1.x + q0.y * q1.y + q0.z * q1.z + q0.w * q1.w;
	float tmp0, tmp1, tmp2, tmp3;
	if (cosom < 0.0f)
	{
		cosom = -cosom;
		tmp0 = -q1.x;
		tmp1 = -q1.y;
		tmp2 = -q1.z;
		tmp3 = -q1.w;
	}
	else
	{
		tmp0 = q1.x;
		tmp1 = q1.y;
		tmp2 = q1.z;
		tmp3 = q1.w;
	}

	/* calc coeffs */
	float scale0, scale1;

	if ((1.0 - cosom) > 0.0000001f)
	{
		// standard case (slerp)
		float omega = (float) acos (cosom);
		float sinom = (float) sin (omega);
		scale0 = (float) sin ((1.0 - t) * omega) / sinom;
		scale1 = (float) sin (t * omega) / sinom;
	}
	else
	{
		/* just lerp */
		scale0 = 1.0f - t;
		scale1 = t;
	}

	x = scale0 * q0.x + scale1 * tmp0;
	y = scale0 * q0.y + scale1 * tmp1;
	z = scale0 * q0.z + scale1 * tmp2;
	w = scale0 * q0.w + scale1 * tmp3;
}

void Quaternion::Lerp(const Quaternion& q0, const Quaternion& q1, float t)
{
	float cosom = q0.x * q1.x + q0.y * q1.y + q0.z * q1.z + q0.w * q1.w;
	float tmp0, tmp1, tmp2, tmp3;
	if (cosom < 0.0f)
	{
		cosom = -cosom;
		tmp0 = -q1.x;
		tmp1 = -q1.y;
		tmp2 = -q1.z;
		tmp3 = -q1.w;
	}
	else
	{
		tmp0 = q1.x;
		tmp1 = q1.y;
		tmp2 = q1.z;
		tmp3 = q1.w;
	}

	/* just lerp */
	float scale0 = 1.0f - t;
	float scale1 = t;

	x = scale0 * q0.x + scale1 * tmp0;
	y = scale0 * q0.y + scale1 * tmp1;
	z = scale0 * q0.z + scale1 * tmp2;
	w = scale0 * q0.w + scale1 * tmp3;
}

ostream& operator<<(ostream& os, Quaternion& v)
{
  os << "(" << setprecision(4) << v.x << ", "  << v.y << ", "  << v.z << ", "  << v.w << ")";
  return os;
}
 

// ------------------------------------------------------------
// Machinery project - math library
// ------------------------------------------------------------
// (c) 2000 by Sascha 'EvilOne' Salevsky
// E-mail: Sascha.Salevsky@Ibykus.de
// ------------------------------------------------------------
//
// Erm, all angles are in degree(like in OGL), I hate this
// fuckin' radians...
//
// Hmmm, some optimizations can be done, mostly in Matrix4,
// but for the first version I want working code, not 100%
// optimal one, tuning comes later...
//
// Currently some camera-stuff is missing in Matrix4, by
// the way, this comes later ... at the time I need it ;-)
// I think an higher level camera class may be better?!?
//
// should I include defines like #define FTYPE float
// and use this abstract type, so that it can also be used
// with doubles?????
//
// Hmmmmm, I think about making this all a template-library...
//
//
// Hakuna Matata, EvilOne
// Bite me... hehe
// ------------------------------------------------------------

#ifndef _mathematics_h_
#define _mathematics_h_

#include <math.h>
#include <stdlib.h> 
#include <iostream.h>
#include <iomanip.h>

// ------------------------------------------------------------
// constants
// ------------------------------------------------------------

// PI & Co.

const float M_PI				= 3.14159265358979323846f;
const float M_DEG_TO_RAD		= 0.01745329251994329547f;
const float M_RAD_TO_DEG		= 57.29577951308232286465f;
const float M_PI_DIV_2			= 1.570796326794896558f;
const float M_PI_DIV_4			= 0.785398163397448279f;

// tolerance values

const float M_LOW_TOLERANCE		= 0.000001f;
const float M_TOLERANCE			= 0.000010f;
const float M_HIGH_TOLERANCE	= 0.000100f;

// ------------------------------------------------------------
// basic functions
// ------------------------------------------------------------

inline float Clamp(float f, float min, float max)
{
	return min > f ? min : (max > f ? f : max);
}

inline float Lerp(float f, float from, float to)
{
	return from + (to - from) * f;
}

inline float Min(float f1, float f2)
{
	return f1 < f2 ? f1 : f2;
}

inline float Max(float f1, float f2)
{
	return f1 > f2 ? f1 : f2;
}

inline float SinD(float f)
{
	return (float)sin(f * M_DEG_TO_RAD);
}

inline float CosD(float f)
{
	return (float)cos(f * M_DEG_TO_RAD);
}

inline float TanD(float f)
{
	return (float)tan(f * M_DEG_TO_RAD);
}

inline float AtanD(float f)
{
	return (float)atan(f * M_DEG_TO_RAD);
}

inline float Degree(float f)
{
	return f * M_DEG_TO_RAD;
}

inline float Radians(float f)
{
	return f * M_RAD_TO_DEG;
}

inline bool IsZeroL(float f)
{
	return fabs(f) > M_LOW_TOLERANCE;
}

inline bool IsZero(float f)
{
	return fabs(f) > M_TOLERANCE;
}

inline bool IsZeroH(float f)
{
	return fabs(f) > M_HIGH_TOLERANCE;
}

inline float Rand(float max)
{
	return (float(rand()) / float(RAND_MAX)) * max;
}

inline float Rand(float min, float max)
{
	return Rand(max - min) + min; 
}

// ------------------------------------------------------------
// Linear algebra classes
// ------------------------------------------------------------

class Vector2;
class Matrix3;
class Vector3;
class Plane;
class Matrix4;
class Quaternion;

// ------------------------------------------------------------
// Vector2
// ------------------------------------------------------------

class Vector2
{
public:

	// Members

	float x, y;

public:

	// Constructors

	Vector2();
	Vector2(const float* other);
	Vector2(float _x, float _y);
	Vector2(const Vector2& other);

	// Assign

	Vector2& operator=(const Vector2& other);
	void Assign(float _x, float _y);
	void Assign(const Vector2& other);
	void Assign(float* other);

	// Swap

	void Swap(Vector2& other);

	// Casting

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

	// Math

	// this += other
	void Add(const Vector2& other);
	// this = v1 + v2
	void Add(const Vector2& v1, const Vector2& v2);
	// this -= other
	void Sub(const Vector2& other);
	// this = v1 - v2
	void Sub(const Vector2& v1, const Vector2& v2);
	// this *= f
	void Mul(float f);
	// this = other * f
	void Mul(const Vector2& other, float f);
	// this /= f
	void Div(float f);
	// this = other / f
	void Div(const Vector2& other, float f);
	// this = -this
	void Invert();
	// this = -other
	void Invert(const Vector2& other);

	// Linear combination

	// this += other * s
	void Combine(const Vector2& other, float s);
	// this = this * s + other * t
	void Combine(float s, const Vector2& other, float t);
	// this = v1 * s + v2 * t
	void Combine(const Vector2& v1, float s, const Vector2& v2, float t);

	// operators

	Vector2& operator+=(const Vector2& other);
	Vector2& operator-=(const Vector2& other);
	Vector2& operator*=(float f);
	Vector2& operator/=(float f);
	Vector2& operator+=(float f);
	Vector2& operator-=(float f);
	Vector2& operator-();
	Vector2 operator+(const Vector2& other) const;
	Vector2 operator-(const Vector2& other) const;
	Vector2 operator*(float f) const;
	Vector2 operator/(float f) const;
	Vector2 operator+(float f) const;
	Vector2 operator-(float f) const;

	// Linear Algebra

	float Length() const;
	float LengthSqr() const;
	float Distance(const Vector2& other) const;
	float DistanceSqr(const Vector2& other) const;
	void Normalize();
	void Normalize(const Vector2& other);

	void Rotate(float angle);
	void Rotate(Vector2 other, float angle);

	// Others

	void Clamp(float min, float max);
	// this = v1 + (v2 - v1) * s
	void Lerp(const Vector2& v1, const Vector2& v2, float s);

	// Actions with Matrix3

	void Transform(const Matrix3& mat, const Vector2& other);
	void Transform(const Matrix3& mat);

	// friend operators

	friend ostream& operator<<(ostream& os, Vector2& v);
};

// ------------------------------------------------------------
// Matrix3
// ------------------------------------------------------------

class Matrix3
{
public:

	// members

	union
	{
		float M[9];
		float m[3][3];
	};

	static float Identity_Matrix[];

public:

	// Constructors

	Matrix3();
	Matrix3(const float* other);
	Matrix3(const Matrix3& other);

	// Assignment

	Matrix3& operator=(const Matrix3& other);
	void Assign(const float* other);
	void Assign(const Matrix3& other);

	// Casting

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

	// Simple methods

	Matrix3& SetIdentity();
	Matrix3& SetZero();

	// this *= other
	Matrix3& Mul(const Matrix3& other);
	// this = m1 * m2
	Matrix3& Mul(const Matrix3& m1, const Matrix3& m2);
	// this *= f
	Matrix3& Mul(float f);
	// this = other * f
	Matrix3& Mul(const Matrix3& other, float f);
	// this /= f
	Matrix3& Div(float f);
	// this = other / f
	Matrix3& Div(const Matrix3& other, float f);

	// Operators

	Matrix3& operator*=(const Matrix3& other);
	Matrix3 operator*(const Matrix3& other) const;

	// Transformation methods

	Matrix3& Translate(float tx, float ty);
	Matrix3& SetTranslate(float tx, float ty);
	Matrix3& Rotate(float r);
	Matrix3& SetRotate(float r);
	Matrix3& Scale(float sx, float sy);
	Matrix3& SetScale(float sx, float sy);

	// friends

	friend ostream& operator<<(ostream& os, Matrix3 &m);
};

// ------------------------------------------------------------
// Vector3
// ------------------------------------------------------------

class Vector3
{
public:

	// Member variables

	float x, y, z;

public:

	// Constructors

	Vector3();
	Vector3(float _x, float _y, float _z);
	Vector3(Vector3& other);
	Vector3(const float* other);

	// assignemt

	Vector3& operator=(const Vector3& other);
	void Assign(float _x, float _y, float _z);
	void Assign(const Vector3& other);
	void Assign(float* other);

	// Swap

	void Swap(Vector3& other);

	// Casting

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

	// Math

	// this += other
	void Add(const Vector3& other);
	// this = v1 + v2
	void Add(const Vector3& v1, const Vector3& v2);
	// this -= other
	void Sub(const Vector3& other);
	// this = v1 - v2
	void Sub(const Vector3& v1, const Vector3& v2);
	// this *= f
	void Mul(float f);
	// this = other * f
	void Mul(const Vector3& other, float f);
	// this /= f
	void Div(float f);
	// this = other / f
	void Div(const Vector3& other, float f);
	// this = -this
	void Invert();
	// this = -other
	void Invert(const Vector3& other);

	// Linear combination

	// this += other * s
	void Combine(const Vector3& other, float s);
	// this = this * s + other * t
	void Combine(float s, const Vector3& other, float t);
	// this = v1 * s + v2 * t
	void Combine(const Vector3& v1, float s, const Vector3& v2, float t);

	// EvilOne's beloved overloads

	Vector3& operator+=(const Vector3& other);
	Vector3& operator-=(const Vector3& other);
	Vector3& operator*=(float f);
	Vector3& operator/=(float f);
	Vector3& operator+=(float f);
	Vector3& operator-=(float f);
	Vector3& operator-();
	Vector3 operator+(const Vector3& other) const;
	Vector3 operator-(const Vector3& other) const;
	Vector3 operator*(float f) const;
	Vector3 operator/(float f) const;
	Vector3 operator+(float f) const;
	Vector3 operator-(float f) const;

	// Linear Algebra

	float Dot(const Vector3& v) const;
	// this = v1 x v2
	void Cross(const Vector3& v);
	void Cross(const Vector3& v1, const Vector3& v2);
	float Length() const;
	float LengthSqr() const;
	float Distance(const Vector3& other) const;
	float DistanceSqr(const Vector3& other) const;
	void Normalize();
	void Normalize(const Vector3& other);

	// Other usefull funtions

	void Clamp(float min, float max);
	// this = v1 + (v2 - v1) * s
	void Lerp(const Vector3& v1, const Vector3& v2, float s);

	// Operations with a plane

	void Intersect(const Vector3& start, float s, const Vector3& end, float e);
	void Intersect(const Plane& p, const Vector3& start, const Vector3& end);
	void Reflect(const Plane& p, const Vector3& v);

	// Operations with Matrix

	void Transform(const Matrix4& mat, const Vector3& other);
	void Transform(const Matrix4& mat);
	void TransformTranspose(const Matrix4& mat, const Vector3& other);
	void TransformTranspose(const Matrix4& mat);

	// friend operators

	friend ostream& operator<<(ostream& os, Vector3& v);
};

// ------------------------------------------------------------
// Plane
// ------------------------------------------------------------

class Plane
{
public:

	// members 

	union
	{
		struct
		{
			float a, b, c, d;
		};

		struct
		{
			Vector3 n;
			float d;
		};
	};

public:

	// Constructors

	Plane();
	Plane(const Vector3& _n, float _d);
	Plane(float _a, float _b, float _c, float _d);
	Plane(float* other);
	Plane(const Plane& other);

	// Assignment

	Plane& operator=(const Plane& other);
	void Assign(const Vector3& _n, float _d);
	void Assign(float _a, float _b, float _c, float _d);
	void Assign(float* other);
	void Assign(const Plane& other);

	// Functions

	float Distance(const Vector3& v) const;

	// Interaction with matrix

	void Transform(const Matrix4 &mat);
	void Transform(const Matrix4 &mat, const Plane& other);

	// friend operators

	friend ostream& operator<<(ostream& os, Plane& p);
};

// ------------------------------------------------------------
// Matrix4
// ------------------------------------------------------------

class Matrix4
{
public:

	// members

	union
	{
		float M[16];
		float m[4][4];
	};

	static float Identity_Matrix[];
	static float Orientation_Switch_Matrix[];
	static float Perspective_Matrix[];

public:

	// Constructors

	Matrix4();
	Matrix4(const float* other);
	Matrix4(const Matrix4& other);
	Matrix4(const Quaternion& other);

	// Assignment

	Matrix4& operator=(const Matrix4& other);
	Matrix4& operator=(const Quaternion& other);
	void Assign(const Quaternion& other);
	void Assign(const float* other);
	void Assign(const Matrix4& other);

	// Casting

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

	// Simple methods

	Matrix4& SetIdentity();
	Matrix4& SetZero();
	Matrix4& SetPerspective();
	Matrix4& SetSwitchOrientation();

	// this *= other
	Matrix4& Mul(const Matrix4& other);
	// this = m1 * m2
	Matrix4& Mul(const Matrix4& m1, const Matrix4& m2);
	// this *= f
	Matrix4& Mul(float f);
	// this = other * f
	Matrix4& Mul(const Matrix4& other, float f);
	// this /= f
	Matrix4& Div(float f);
	// this = other / f
	Matrix4& Div(const Matrix4& other, float f);

	// Operators

	Matrix4& operator*=(const Matrix4& other);
	Matrix4 operator*(const Matrix4& other) const;

	// Transformation methods

	Matrix4& SetTranslate(float tx, float ty, float tz);
	Matrix4& Translate(float tx, float ty, float tz);
	Matrix4& SetScale(float sx, float sy, float sz);
	Matrix4& Scale(float sx, float sy, float sz);

	// rotation around three euler-angles

	Matrix4& SetRotate(const Vector3& r);
	Matrix4& SetRotate(float rx, float ry, float rz);
	Matrix4& Rotate(const Vector3& r);
	Matrix4& Rotate(float rx, float ry, float rz);

	// rotation euler-angle around axis
	
	Matrix4& SetRotate(float angle, const Vector3& r);
	Matrix4& SetRotate(float angle, float x, float y, float z);
	Matrix4& Rotate(float angle, const Vector3& r);
	Matrix4& Rotate(float angle, float x, float y, float z);

	// Invert/Transpose

	Matrix4& Adjoint();
	Matrix4& Adjoint(const Matrix4& other);

	Matrix4& Transpose();
	Matrix4& Transpose(const Matrix4& other);

	Matrix4& Invert();
	Matrix4& Invert(const Matrix4& other);

	float Det() const;

	// Perpsective

	Matrix4& SetProjection(float fov, float aspect, float znear, float zfar);
	Matrix4& SetOthogonal(float znear, float zfar);

	// static

	static float Det2x2(float a, float b, float c, float d);
	static float Det3x3(float a1, float a2, float a3,
						float b1, float b2, float b3,
						float c1, float c2, float c3);

	// friends

	friend ostream& operator<<(ostream& os, Matrix4 &m);
};

// ------------------------------------------------------------
// Quaternion
// ------------------------------------------------------------

class Quaternion
{
public:

	// Members

	float x, y, z, w;

public:

	// Constructors

	Quaternion();
	Quaternion(const Quaternion& other);
	Quaternion(const float* other);
	Quaternion(float _x, float _y, float _z, float _w);

	// Assignment

	Quaternion& operator=(const Quaternion& other);
	void Assign(const Quaternion& other);
	void Assign(const float* other);
	void Assign(float _x, float _y, float _z, float _w);

	// Simple math

	void Invert();
	void Invert(const Quaternion& other);
	Quaternion& operator-();

	Quaternion& Mul(const Quaternion& other);
	Quaternion& Mul(const Quaternion& q1, const Quaternion& q2);
	Quaternion operator*(const Quaternion& other) const;
	Quaternion& operator*=(const Quaternion& other);

	float Length() const;
	float LengthSqr() const;

	void Normalize();
	void Normalize(const Quaternion& other);

	// Rotation around euler-angle and axis

	Quaternion& SetRotate(float angle, const Vector3& v);
	Quaternion& Rotate(float angle, const Vector3& v);
	Quaternion& SetRotate(float angle, float _x, float _y, float _z);
	Quaternion& Rotate(float angle, float _x, float _y, float _z);

	// rotation around three euler-angles

	Quaternion& SetRotate(const Vector3& v);
	Quaternion& Rotate(const Vector3& v);
	Quaternion& SetRotate(float rx, float ry, float rz);
	Quaternion& Rotate(float rx, float ry, float rz);

	// Interpolation

	void Lerp(const Quaternion& q0, const Quaternion& q1, float t);
	void Slerp(const Quaternion& q0, const Quaternion& q1, float t);

	// friend operators

	friend ostream& operator<<(ostream& os, Quaternion& v);
};

// ------------------------------------------------------------
// Include implementations
// ------------------------------------------------------------

#include "Vector2.h"
#include "Matrix3.h"
#include "Vector3.h"
#include "Plane.h"
#include "Matrix4.h"
#include "Quaternion.h"

#endif
 

// ------------------------------------------------------------
// Machinery project - math library
// ------------------------------------------------------------
// (c) 2000 by Sascha 'EvilOne' Salevsky
// E-mail: Sascha.Salevsky@Ibykus.de
// ------------------------------------------------------------

// Constructors

inline Matrix3::Matrix3()
{}

inline Matrix3::Matrix3(const float* other)
{
	for(int i = 0; i < 9; i++) M[i] = other[i];
}

inline Matrix3::Matrix3(const Matrix3& other)
{
	for(int i = 0; i < 9; i++) M[i] = other.M[i];
}

// Assignment

inline Matrix3& Matrix3::operator=(const Matrix3& other)
{
	for(int i = 0; i < 9; i++) M[i] = other.M[i];
	return *this;
}

inline void Matrix3::Assign(const Matrix3& other)
{
	for(int i = 0; i < 9; i++) M[i] = other.M[i];
}

inline void Matrix3::Assign(const float* other)
{
	for(int i = 0; i < 9; i++) M[i] = other[i];
}

// Casting

inline Matrix3::operator const float*() const
{
	return M;
}

inline Matrix3::operator float*()
{
	return M;
}

// Simple Methods

inline Matrix3& Matrix3::SetIdentity()
{
	for(int i = 0; i < 9; i++) M[i] = Matrix3::Identity_Matrix[i];
	return *this;
}

inline Matrix3& Matrix3::SetZero()
{
	for(int i = 0; i < 9; i++) M[i] = 0.0f;
	return *this;
}

inline Matrix3& Matrix3::Mul(const Matrix3& other)
{
	Matrix3 tmp;
	tmp.Assign(*this);
	Mul(tmp, other);
	return *this;
}

inline Matrix3& Matrix3::Mul(const Matrix3& m1, const Matrix3& m2)
{
	const float *b = m1.M;
	const float *a = m2.M;
	float *prod = M;

	for(int i = 0; i < 3; i++)
	{
		float a0 = a[i];
		float a1 = a[i + 3];
		float a2 = a[i + 6];
		prod[i]     = a0 * b[0] + a1 * b[1] + a2 * b[2];
		prod[i + 3] = a0 * b[3] + a1 * b[4] + a2 * b[5];
		prod[i + 6] = a0 * b[6] + a1 * b[7] + a2 * b[8];
	}
	return *this;
}

inline Matrix3& Matrix3::Mul(float f)
{
	for(int i = 0; i < 9; i++) M[i] *=f;
	return *this;
}

inline Matrix3& Matrix3::Mul(const Matrix3& other, float f)
{
	const float* m = other.M;
	for(int i = 0; i < 9; i++) M[i] = m[i] * f;
	return *this;
}

inline Matrix3& Matrix3::Div(float f)
{
	f = 1.0f / f;
	for(int i = 0; i < 9; i++) M[i] *=f;
	return *this;
}

inline Matrix3& Matrix3::Div(const Matrix3& other, float f)
{
	f = 1.0f / f;
	const float* m = other.M;
	for(int i = 0; i < 9; i++) M[i] = m[i] * f;
	return *this;
}

// Operators

inline Matrix3& Matrix3::operator*=(const Matrix3& other)
{
	Matrix3 tmp;
	tmp.Assign(*this);
	Mul(tmp, other);
	return *this;
}

inline Matrix3 Matrix3::operator*(const Matrix3& other) const
{
	Matrix3 tmp;
	tmp.Mul(*this, other);
	return tmp;
}

// Transformations

inline Matrix3& Matrix3::Translate(float tx, float ty)
{
	M[6] = M[0] * tx + M[3] * ty + M[6];
	M[7] = M[1] * tx + M[4] * ty + M[7];
	M[8] = M[2] * tx + M[5] * ty + M[8];
	return *this;
}

inline Matrix3& Matrix3::SetTranslate(float tx, float ty)
{
	SetIdentity();
	M[6] = tx;
	M[7] = ty;
	return *this;
}

inline Matrix3& Matrix3::Scale(float sx, float sy)
{
	M[0] *= sx; M[3] *= sy;
	M[1] *= sx; M[4] *= sy;
	M[2] *= sx; M[5] *= sy;
	return *this;
}

inline Matrix3& Matrix3::SetScale(float sx, float sy)
{
	M[0] = sx; M[4] = sy; M[8] = 1.0f;
	M[1] = M[2] = M[3] = M[5] = M[6] = M[7] = 0.0f;
	return *this;
}

inline Matrix3& Matrix3::Rotate(float r)
{
	float c = CosD(r), s = SinD(r);
	Matrix3 tmp;
	tmp.SetIdentity();
	tmp.M[0] = c;
	tmp.M[1] = -s;
	tmp.M[3] = s;
	tmp.M[4] = c;
	Mul(tmp);
	return *this;
}

inline Matrix3& Matrix3::SetRotate(float r)
{
	float c = CosD(r), s = SinD(r);
	Matrix3 tmp;
	SetIdentity();
	M[0] = c;
	M[1] = -s;
	M[3] = s;
	M[4] = c;
	return *this;
}
 

// ------------------------------------------------------------
// Machinery project - math library
// ------------------------------------------------------------
// (c) 2000 by Sascha 'EvilOne' Salevsky
// E-mail: Sascha.Salevsky@Ibykus.de
// ------------------------------------------------------------

#ifndef _mathematics_h_
#error "Dont include Matrix4 directly!!! Use Mathematics.h instead."
#endif

// Constructors

inline Matrix4::Matrix4()
{}

inline Matrix4::Matrix4(const Matrix4& other)
{
	for(int i = 0; i < 16; i++) M[i] = other.M[i];
}

inline Matrix4::Matrix4(const float* other)
{
	for(int i = 0; i < 16; i++) M[i] = other[i];
}

inline Matrix4::Matrix4(const Quaternion& other)
{
	Assign(other);
}

// Assignment

inline Matrix4& Matrix4::operator=(const Matrix4& other)
{
	for(int i = 0; i < 16; i++) M[i] = other.M[i];
	return *this;
}

inline void Matrix4::Assign(const Matrix4& other)
{
	for(int i = 0; i < 16; i++) M[i] = other.M[i];
}

inline void Matrix4::Assign(const float* other)
{
	for(int i = 0; i < 16; i++) M[i] = other[i];
}

inline Matrix4& Matrix4::operator=(const Quaternion& other)
{
	Assign(other);
	return *this;
}

// Casting

inline Matrix4::operator const float*() const
{
	return M;
}

inline Matrix4::operator float*()
{
	return M;
}

// Simple methods

inline Matrix4& Matrix4::SetIdentity()
{
	for(int i = 0; i < 16; i++) M[i] = Matrix4::Identity_Matrix[i];
	return *this;
}

inline Matrix4& Matrix4::SetZero()
{
	for(int i = 0; i < 16; i++) M[i] = 0.0f;
	return *this;
}

inline Matrix4& Matrix4::SetSwitchOrientation()
{
	for(int i = 0; i < 16; i++) M[i] = Matrix4::Orientation_Switch_Matrix[i];
	return *this;
}

inline Matrix4& Matrix4::SetPerspective()
{
	for(int i = 0; i < 16; i++) M[i] = Matrix4::Perspective_Matrix[i];
	return *this;
}

inline Matrix4& Matrix4::Mul(const Matrix4& other)
{
	Matrix4 tmp;
	tmp.Assign(*this);
	Mul(tmp, other);
	return *this;
}

inline Matrix4& Matrix4::Mul(const Matrix4& m1, const Matrix4& m2)
{
	const float *b = m1.M;
	const float *a = m2.M;
	float *prod = M;

	for(int i = 0; i < 4; i++)
	{
		float a0 = a[i];
		float a1 = a[i + 4];
		float a2 = a[i + 8];
		float a3 = a[i + 12];
		prod[i] = a0 * b[0] + a1 * b[1] + a2 * b[2] + a3 * b[3];
		prod[i + 4] = a0 * b[4] + a1 * b[5] + a2 * b[6] + a3 * b[7];
		prod[i + 8] = a0 * b[8] + a1 * b[9] + a2 * b[10] + a3 * b[11];
		prod[i + 12] = a0 * b[12] + a1 * b[13] + a2 * b[14] + a3 * b[15];
	}
	return *this;
}

inline Matrix4& Matrix4::Mul(float f)
{
	for(int i = 0; i < 16; i++) M[i] *=f;
	return *this;
}

inline Matrix4& Matrix4::Mul(const Matrix4& other, float f)
{
	const float* m = other.M;
	for(int i = 0; i < 16; i++) M[i] = m[i] * f;
	return *this;
}

inline Matrix4& Matrix4::Div(float f)
{
	f = 1.0f / f;
	for(int i = 0; i < 16; i++) M[i] *=f;
	return *this;
}

inline Matrix4& Matrix4::Div(const Matrix4& other, float f)
{
	f = 1.0f / f;
	const float* m = other.M;
	for(int i = 0; i < 16; i++) M[i] = m[i] * f;
	return *this;
}

// Operators

inline Matrix4& Matrix4::operator*=(const Matrix4& other)
{
	Matrix4 tmp;
	tmp.Assign(*this);
	Mul(tmp, other);
	return *this;
}

inline Matrix4 Matrix4::operator*(const Matrix4& other) const
{
	Matrix4 tmp;
	tmp.Mul(*this, other);
	return tmp;
}

// Transformation

inline Matrix4& Matrix4::Translate(float tx, float ty, float tz)
{
	M[12] = M[0] * tx + M[4] * ty + M[8]  * tz + M[12];
	M[13] = M[1] * tx + M[5] * ty + M[9]  * tz + M[13];
	M[14] = M[2] * tx + M[6] * ty + M[10] * tz + M[14];
	M[15] = M[3] * tx + M[7] * ty + M[11] * tz + M[15];
	return *this;
}

inline Matrix4& Matrix4::SetTranslate(float tx, float ty, float tz)
{
	SetIdentity();
	return Translate(tx, ty, tz);
}

inline Matrix4& Matrix4::Scale(float sx, float sy, float sz)
{
	M[0] *= sx; M[4] *= sy; M[8]  *= sz;
	M[1] *= sx; M[5] *= sy; M[9]  *= sz;
	M[2] *= sx; M[6] *= sy; M[10] *= sz;
	M[3] *= sx; M[7] *= sy; M[11] *= sz;
	return *this;
}

inline Matrix4& Matrix4::SetScale(float sx, float sy, float sz)
{
	SetIdentity();
	return Scale(sx, sy, sz);
}

inline Matrix4& Matrix4::Rotate(float rx, float ry, float rz)
{
	Matrix4 tmp;
	tmp.SetRotate(rx, ry, rz);
	Mul(tmp);
	return *this;
}

inline Matrix4& Matrix4::Rotate(float angle, float x, float y, float z)
{
	Matrix4 tmp;
	tmp.SetRotate(angle, x, y, z);
	Mul(tmp);
	return *this;
}

inline Matrix4& Matrix4::SetRotate(const Vector3& r)
{
	return SetRotate(r.x, r.y, r.z);
}

inline Matrix4& Matrix4::Rotate(const Vector3& r)
{
	return Rotate(r.x, r.y, r.z);
}

inline Matrix4& Matrix4::SetRotate(float angle, const Vector3& r)
{
	return SetRotate(angle, r.x, r.y, r.z);
}

inline Matrix4& Matrix4::Rotate(float angle, const Vector3& r)
{
	return Rotate(angle, r.x, r.y, r.z);
}
 

// ------------------------------------------------------------
// Machinery project - math library
// ------------------------------------------------------------
// (c) 2000 by Sascha 'EvilOne' Salevsky
// E-mail: Sascha.Salevsky@Ibykus.de
// ------------------------------------------------------------

#ifndef _mathematics_h_
#error "Dont include Plane directly!!! Use Mathematics.h instead."
#endif

inline Plane::Plane()
{}

// copy by hand avoids call to Vector3::operator=(...)
inline Plane::Plane(const Vector3& _n, float _d)
{
	a = _n.x; b = _n.y; c = _n.z; d = _d;
}

inline Plane::Plane(float _a, float _b, float _c, float _d)
{
	a = _a; b = _b; c = _c; d = _d;
}

inline Plane::Plane(float* other)
{
	a = other[0]; b = other[1]; c = other[2]; d = other[3];
}

inline Plane::Plane(const Plane& other)
{
	a = other.a; b = other.b; c = other.c; d = other.d;
}

// Assignment

inline Plane& Plane::operator=(const Plane& other)
{
	a = other.a; b = other.b; c = other.c; d = other.d;
	return *this;
}

inline void Plane::Assign(const Vector3& _n, float _d)
{
	a = _n.x; b = _n.y; c = _n.z; d = _d;
}

inline void Plane::Assign(float _a, float _b, float _c, float _d)
{
	a = _a; b = _b; c = _c; d = _d;
}

inline void Plane::Assign(float* other)
{
	a = other[0]; b = other[1]; c = other[2]; d = other[3];
}

inline void Plane::Assign(const Plane& other)
{
	a = other.a; b = other.b; c = other.c; d = other.d;
}

// Functions

inline float Plane::Distance(const Vector3& v) const
{
	return n.Dot(v) - d;
}

inline void Plane::Transform(const Matrix4& mat)
{
  const float* m = mat.M;
  float fx = a;
  float fy = b;
  float fz = c;
  float fw = d;
  a = m[0] * fx + m[4] * fy + m[8] * fz + m[12] * fw;
  b = m[1] * fx + m[5] * fy + m[9] * fz + m[13] * fw;
  c = m[2] * fx + m[6] * fy + m[10] * fz + m[14] * fw;
  d = m[3] * fx + m[7] * fy + m[11] * fz + m[15] * fw;
}

inline void Plane::Transform(const Matrix4& mat, const Plane& other)
{
  const float* m = mat.M;
  float fx = other.a;
  float fy = other.b;
  float fz = other.c;
  float fw = other.d;
  a = m[0] * fx + m[4] * fy + m[8] * fz + m[12] * fw;
  b = m[1] * fx + m[5] * fy + m[9] * fz + m[13] * fw;
  c = m[2] * fx + m[6] * fy + m[10] * fz + m[14] * fw;
  d = m[3] * fx + m[7] * fy + m[11] * fz + m[15] * fw;
}
 

// ------------------------------------------------------------
// Machinery project - math library
// ------------------------------------------------------------
// (c) 2000 by Sascha 'EvilOne' Salevsky
// E-mail: Sascha.Salevsky@Ibykus.de
// ------------------------------------------------------------

#ifndef _mathematics_h_
#error "Dont include Quaternion directly!!! Use Mathematics.h instead."
#endif

// Constructors

inline Quaternion::Quaternion()
{}

inline Quaternion::Quaternion(const Quaternion& other)
{
	x = other.x; y = other.y; z = other.z; w = other.w;
}

inline Quaternion::Quaternion(const float* other)
{
	x = other[0]; y = other[1]; z = other[2]; w = other[3];
}

inline Quaternion::Quaternion(float _x, float _y, float _z, float _w)
{
	x = _x; y = _y; z = _z; w = _w;
}

// Assignment

inline Quaternion& Quaternion::operator=(const Quaternion& other)
{
	x = other.x; y = other.y; z = other.z; w = other.w;
	return *this;
}

inline void Quaternion::Assign(const Quaternion& other)
{
	x = other.x; y = other.y; z = other.z; w = other.w;
}

inline void Quaternion::Assign(const float* other)
{
	x = other[0]; y = other[1]; z = other[2]; w = other[3];
}

inline void Quaternion::Assign(float _x, float _y, float _z, float _w)
{
	x = _x; y = _y; z = _z; w = _w;
}

// Simple math

inline void Quaternion::Invert()
{
	x = -x; y = -y; z = -z;
}

inline void Quaternion::Invert(const Quaternion& other)
{
	x = -other.x; y = -other.y; z = -other.z;
	w = other.w;
}

inline Quaternion& Quaternion::operator-()
{
	x = -x; y = -y; z = -z;
	return *this;
}

inline float Quaternion::Length() const
{
	return (float)sqrt(w*w + x*x + y*y + z*z);
}

inline float Quaternion::LengthSqr() const
{
	return w*w + x*x + y*y + z*z;
}

inline void Quaternion::Normalize()
{
	float f = 1.0f / (w*w + x*x + y*y + z*z);
	x *= f; y *= f; z *= f; w *= f;
}

inline void Quaternion::Normalize(const Quaternion& other)
{
	float f = 1.0f / other.Length();
	x = other.x * f; y = other.y * f; z = other.z * f; w = other.w * f;
}

inline Quaternion& Quaternion::Mul(const Quaternion& other)
{
	Mul(*this, other);
	return *this;
}

inline Quaternion& Quaternion::Mul(const Quaternion& q1, const Quaternion& q2)
{
	const Vector3 v1 = &q1.x;
	const Vector3 v2 = &q2.x;

    float scalar = q1.w * q2.w - v1.Dot(v2);

    Vector3 va; va.Cross(v1, v2);
	Vector3 vb; vb.Mul(v1, q2.w);
	Vector3 vc; vc.Mul(v2, q1.w);
	va.Add(vb); va.Add(vc);

	x = va.x; y = va.y; z = va.z; w = scalar;

    Normalize();
	return *this;
}

inline Quaternion Quaternion::operator*(const Quaternion& other) const
{
	Quaternion tmp;
	tmp.Mul(*this, other);
	return tmp;
}

inline Quaternion& Quaternion::operator*=(const Quaternion& other)
{
	Mul(*this, other);
	return *this;
}

// Rotation

inline Quaternion& Quaternion::SetRotate(float angle, const Vector3& v)
{
	return SetRotate(angle, v.x, v.y, v.z);
}

inline Quaternion& Quaternion::Rotate(float angle, const Vector3& v)
{
	return Rotate(angle, v.x, v.y, v.z);
}

inline Quaternion& Quaternion::SetRotate(const Vector3& v)
{
	return SetRotate(v.x, v.y, v.z);
}

inline Quaternion& Quaternion::Rotate(const Vector3& v)
{
	return Rotate(v.x, v.y, v.z);
}

inline Quaternion& Quaternion::SetRotate(float angle, float _x, float _y, float _z)
{
    float s = SinD(angle / 2.0f);
    float c = CosD(angle / 2.0f);

    x = _x * s;
    y = _y * s;
    z = _z * s;
    w = c;
	Normalize();
	return *this;
}

inline Quaternion& Quaternion::Rotate(float angle, float _x, float _y, float _z)
{
	Quaternion tmp;
	tmp.SetRotate(angle, _x, _y, _z);
	Mul(*this, tmp);
	return *this;
}

inline Quaternion& Quaternion::SetRotate(float rx, float ry, float rz)
{
	Quaternion qy, qz;
	SetRotate(rx, 1.0f, 0.0f, 0.0f);
	qy.SetRotate(ry, 0.0f, 1.0f, 0.0f);
	qz.SetRotate(rz, 0.0f, 0.0f, 1.0f);
	Mul(*this, qy);
	Mul(*this, qz);
	return *this;
}

inline Quaternion& Quaternion::Rotate(float rx, float ry, float rz)
{
	Quaternion tmp;
	tmp.SetRotate(rx, ry, rz);
	Mul(*this, tmp);
	return *this;
}
 

// ------------------------------------------------------------
// Machinery project - math library
// ------------------------------------------------------------
// (c) 2000 by Sascha 'EvilOne' Salevsky
// E-mail: Sascha.Salevsky@Ibykus.de
// ------------------------------------------------------------

#ifndef _mathematics_h_
#error "Dont include Vector2 directly!!! Use Mathematics.h instead."
#endif

// Constructors

inline Vector2::Vector2()
{}

inline Vector2::Vector2(const float* other)
{
	x = other[0]; y = other[1];
}

inline Vector2::Vector2(float _x, float _y)
{
	x = _x; y = _y;
}

inline Vector2::Vector2(const Vector2& other)
{
	x = other.x; y = other.y;
}

// Assign

inline Vector2& Vector2::operator=(const Vector2& other)
{
	x = other.x; y = other.y;
	return *this;
}

inline void Vector2::Assign(float _x, float _y)
{
	x = _x; y = _y;
}


inline void Vector2::Assign(const Vector2& other)
{
	x = other.x; y = other.y;
}

inline void Vector2::Assign(float* other)
{
	x = other[0]; y = other[1];
}

// Swap

inline void Vector2::Swap(Vector2& other)
{
	Vector2 tmp = *this;
	Assign(other);
	other.Assign(tmp);
}

// Casting

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

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

// Math

inline void Vector2::Add(const Vector2& other)
{
	x += other.x; y += other.y;
}

inline void Vector2::Add(const Vector2& v1, const Vector2& v2)
{
	x = v1.x + v2.x; y = v1.y + v2.y;
}

inline void Vector2::Sub(const Vector2& other)
{
	x -= other.x; y -= other.y;
}

inline void Vector2::Sub(const Vector2& v1, const Vector2& v2)
{
	x = v1.x - v2.x; y = v1.y - v2.y;
}

inline void Vector2::Mul(float f)
{
	x *= f; y *= f;
}

inline void Vector2::Mul(const Vector2& other, float f)
{
	x = other.x * f; y = other.y * f;
}

inline void Vector2::Div(float f)
{
	f = 1.0f / f;
	x *= f; y *= f;
}

inline void Vector2::Div(const Vector2& other, float f)
{
	f = 1.0f / f;
	x = other.x * f; y = other.y * f;
}

inline void Vector2::Invert()
{
	x = -x; x = -y;
}

inline void Vector2::Invert(const Vector2& other)
{
	x = -other.x; x = -other.y;
}

// Linear combination

inline void Vector2::Combine(const Vector2& other, float s)
{
	x += other.x * s; y += other.y * s;
}

inline void Vector2::Combine(float s, const Vector2& other, float t)
{
	x = x * s + other.x * t; y = y * s + other.y * t;
}

inline void Vector2::Combine(const Vector2& v1, float s, const Vector2& v2, float t)
{
	x = v1.x * s + v2.x * t; y = v1.y * s + v2.y * t;
}

// operators

inline Vector2& Vector2::operator+=(const Vector2& other)
{
	x += other.x; y += other.y;
	return *this;
}

inline Vector2& Vector2::operator-=(const Vector2& other)
{
	x -= other.x; y -= other.y;
	return *this;
}

inline Vector2& Vector2::operator*=(float f)
{
	x *= f; y *= f;
	return *this;
}

inline Vector2& Vector2::operator-=(float f)
{
	x -= f; y -= f;
	return *this;
}

inline Vector2& Vector2::operator+=(float f)
{
	x += f; y += f;
	return *this;
}

inline Vector2& Vector2::operator/=(float f)
{
	f = 1.0f / f;
	x *= f; y *= f;
	return *this;
}

inline Vector2& Vector2::operator-()
{
	x = -x; y = -y;
	return *this;
}

inline Vector2 Vector2::operator+(const Vector2& other) const
{
	return Vector2(x + other.x, y + other.y);
}

inline Vector2 Vector2::operator-(const Vector2& other) const
{
	return Vector2(x - other.x, y - other.y);
}

inline Vector2 Vector2::operator+(float f) const
{
	return Vector2(x + f, y + f);
}

inline Vector2 Vector2::operator-(float f) const
{
	return Vector2(x - f, y - f);
}

inline Vector2 Vector2::operator*(float f) const
{
	return Vector2(x * f, y * f);
}

inline Vector2 Vector2::operator/(float f) const
{
	f = 1.0f / f;
	return Vector2(x * f, y * f);
}

// Linear algebra

inline float Vector2::Length() const
{
	return (float)sqrt(x * x + y * y);
}

inline float Vector2::LengthSqr() const
{
	return x * x + y * y;
}

inline float Vector2::Distance(const Vector2& other) const
{
	Vector2 tmp = other - *this;
	return tmp.Length();
}

inline float Vector2::DistanceSqr(const Vector2& other) const
{
	Vector2 tmp = other - *this;
	return tmp.LengthSqr();
}

inline void Vector2::Normalize()
{
	float f = 1.0f / Length();
	x = x * f; y = y * f;
}

inline void Vector2::Normalize(const Vector2& other)
{
	float f = 1.0f / other.Length();
	x = other.x * f; y = other.y * f;
}

inline void Vector2::Rotate(float angle)
{
	float _x = x;
	x =  _x * CosD(angle) + y * SinD(angle);
	y = -_x * SinD(angle) + y * CosD(angle);
}

inline void Vector2::Rotate(Vector2 other, float angle)
{
	float _x = other.x;
	float _y = other.y;

	x =  _x * CosD(angle) + _y * SinD(angle);
	y = -_x * SinD(angle) + _y * CosD(angle);
}

// Others

inline void Vector2::Clamp(float min, float max)
{
	x = ::Clamp(x, min, max); y = ::Clamp(y, min, max);
}

inline void Vector2::Lerp(const Vector2& v1, const Vector2& v2, float s)
{
	x = ::Lerp(s, v1.x, v2.x); y = ::Lerp(s, v1.y, v2.y);
}

inline void Vector2::Transform(const Matrix3& mat, const Vector2& other)
{
	const float* m = mat.M;
	float fx = other.x; float fy = other.y;
	x = m[0] * fx + m[3] * fy + m[6];  // w assumed to be 1.0F
	y = m[1] * fx + m[4] * fy + m[7];
}

inline void Vector2::Transform(const Matrix3& mat)
{
	const float* m = mat.M;
	float fx = x; float fy = y;
	x = m[0] * fx + m[3] * fy + m[6];  // w assumed to be 1.0F
	y = m[1] * fx + m[4] * fy + m[7];
}
 

// ------------------------------------------------------------
// Machinery project - math library
// ------------------------------------------------------------
// (c) 2000 by Sascha 'EvilOne' Salevsky
// E-mail: Sascha.Salevsky@Ibykus.de
// ------------------------------------------------------------

#ifndef _mathematics_h_
#error "Dont include Vector3 directly!!! Use Mathematics.h instead."
#endif

// Constructors

inline Vector3::Vector3()
{}

inline Vector3::Vector3(float _x, float _y, float _z)
{
	x = _x; y = _y; z = _z;
}

inline Vector3::Vector3(Vector3& other)
{
	x = other.x; y = other.y; z = other.z;
}

inline Vector3::Vector3(const float* other)
{
	x = other[0]; y = other[1]; z = other[2];
}

// assignemt

inline Vector3& Vector3::operator=(const Vector3& other)
{
	x = other.x; y = other.y; z = other.z;
	return *this;
}

inline void Vector3::Assign(float _x, float _y, float _z)
{
	x = _x; y = _y; z = _z;
}

inline void Vector3::Assign(const Vector3& other)
{
	x = other.x; y = other.y; z = other.z;
}

inline void Vector3::Assign(float* other)
{
	x = other[0]; y = other[1]; z = other[2];
}

// Swap

inline void Vector3::Swap(Vector3& other)
{
	float _x = x, _y = y, _z = z;
	x = other.x; y = other.y; z = other.z;
	other.x = _x; other.y = _y; other.z = _z;
}

// Casting

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

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

// base functions

inline void Vector3::Add(const Vector3& other)
{
	x += other.x; y += other.y; z += other.z;
}

inline void Vector3::Add(const Vector3& v1, const Vector3& v2)
{
	x = v1.x + v2.x; y = v1.y + v2.y; z = v1.z + v2.z;
}

inline void Vector3::Sub(const Vector3& other)
{
	x -= other.x; y -= other.y; z -= other.z;
}

inline void Vector3::Sub(const Vector3& v1, const Vector3& v2)
{
	x = v1.x - v2.x; y = v1.y - v2.y; z = v1.z - v2.z;
}

inline void Vector3::Mul(float f)
{
	x *= f; y *= f; z *= f;
}

inline void Vector3::Mul(const Vector3& other, float f)
{
	x = other.x * f; y = other.y * f; z = other.z * f;
}

inline void Vector3::Div(float f)
{
	f = 1.0f / f;
	x *= f; y *= f; z *= f;
}

inline void Vector3::Div(const Vector3& other, float f)
{
	f = 1.0f / f;
	x = other.x * f; y = other.y * f; z = other.z * f;
}

inline void Vector3::Invert()
{
	x = -x; x = -y; z = -z;
}

inline void Vector3::Invert(const Vector3& other)
{
	x = -other.x; x = -other.y; z = -other.z;
}

// Linear combination

inline void Vector3::Combine(const Vector3& other, float s)
{
	x += other.x * s; y += other.y * s; z += other.z * s;
}

inline void Vector3::Combine(float s, const Vector3& other, float t)
{
	x = x * s + other.x * t;
	y = y * s + other.y * t;
	z = z * s + other.z * t;
}

inline void Vector3::Combine(const Vector3& v1, float s, const Vector3& v2, float t)
{
	x = v1.x * s + v2.x * t;
	y = v1.y * s + v2.y * t;
	z = v1.z * s + v2.z * t;
}

// EvilOne's beloved overloads

inline Vector3& Vector3::operator+=(const Vector3& other)
{
	x += other.x; y += other.y; z += other.z;
	return *this;
}

inline Vector3& Vector3::operator-=(const Vector3& other)
{
	x -= other.x; y -= other.y; z -= other.z;
	return *this;
}

inline Vector3& Vector3::operator*=(float f)
{
	x *= f; y *= f; z *= f;
	return *this;
}

inline Vector3& Vector3::operator-=(float f)
{
	x -= f; y -= f; z -= f;
	return *this;
}

inline Vector3& Vector3::operator+=(float f)
{
	x += f; y += f; z += f;
	return *this;
}

inline Vector3& Vector3::operator/=(float f)
{
	f = 1.0f / f;
	x *= f; y *= f; z *= f;
	return *this;
}

inline Vector3& Vector3::operator-()
{
	x = -x; y = -y; z = -z;
	return *this;
}

inline Vector3 Vector3::operator+(const Vector3& other) const
{
	return Vector3(x + other.x, y + other.y, z + other.z);
}

inline Vector3 Vector3::operator-(const Vector3& other) const
{
	return Vector3(x - other.x, y - other.y, z - other.z);
}

inline Vector3 Vector3::operator+(float f) const
{
	return Vector3(x + f, y + f, z + f);
}

inline Vector3 Vector3::operator-(float f) const
{
	return Vector3(x - f, y - f, z - f);
}

inline Vector3 Vector3::operator*(float f) const
{
	return Vector3(x * f, y * f, z * f);
}

inline Vector3 Vector3::operator/(float f) const
{
	f = 1.0f / f;
	return Vector3(x * f, y * f, z * f);
}

// Linear Algebra

inline float Vector3::Dot(const Vector3& v) const
{
	return x * v.x + y * v.y + z * v.z;
}

inline void Vector3::Cross(const Vector3& v)
{
	float _x = y * v.z - z * v.y;
	float _y = z * v.x - x * v.z;
	float _z = x * v.y - y * v.x;
	x = _x; y = _y; z = _z;
}

inline void Vector3::Cross(const Vector3& v1, const Vector3& v2)
{
	float _x = v1.y * v2.z - v1.z * v2.y;
	float _y = v1.z * v2.x - v1.x * v2.z;
	float _z = v1.x * v2.y - v1.y * v2.x;
	x = _x; y = _y; z = _z;
}

inline float Vector3::Length() const
{
	return (float)sqrt(x * x + y * y + z * z);
}

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

inline float Vector3::Distance(const Vector3& other) const
{
	Vector3 tmp = other - *this;
	return tmp.Length();
}

inline float Vector3::DistanceSqr(const Vector3& other) const
{
	Vector3 tmp = other - *this;
	return tmp.LengthSqr();
}

inline void Vector3::Normalize()
{
	float f = 1.0f / Length();
	x = x * f; y = y * f; z = z * f;
}

inline void Vector3::Normalize(const Vector3& other)
{
	float f = 1.0f / other.Length();
	x = other.x * f; y = other.y * f; z = other.z * f;
}


// Other usefull funtions

inline void Vector3::Clamp(float min, float max)
{
	x = ::Clamp(x, min, max);
	y = ::Clamp(y, min, max);
	z = ::Clamp(z, min, max);
}

inline void Vector3::Lerp(const Vector3& v1, const Vector3& v2, float s)
{
	x = ::Lerp(s, v1.x, v2.x);
	y = ::Lerp(s, v1.y, v2.y);
	z = ::Lerp(s, v1.z, v2.z);
}

// Operations with a plane

inline void Vector3::Intersect(const Vector3& start, float s, const Vector3& end, float e)
{
	float ws = s / (e - s);
	float we = e / (e - s);
	x = start.x * ws + end.x * we;
	y = start.y * ws + end.y * we;
	z = start.z * ws + end.z * we;
}

inline void Vector3::Intersect(const Plane& p, const Vector3& start, const Vector3& end)
{
	float s = p.Distance(start);
	float e = p.Distance(end);
	Intersect(start, s, end, e);
}

inline void Vector3::Reflect(const Plane& p, const Vector3& v)
{
	Vector3 tmp = p.n * 2 * v.Dot(p.n) - v;
	x = tmp.x; y = tmp.y; z = tmp.z;
}

// Operations with a matrix

inline void Vector3::Transform(const Matrix4& mat)
{
	const float* m = mat.M;
	float fx = x; float fy = y; float fz = z;
	x = m[0] * fx + m[4] * fy + m[8] * fz + m[12];  // w assumed to be 1.0F
	y = m[1] * fx + m[5] * fy + m[9] * fz + m[13];
	z = m[2] * fx + m[6] * fy + m[10] * fz + m[14];
}

inline void Vector3::Transform(const Matrix4& mat, const Vector3& other)
{
	const float* m = mat.M;
	float fx = other.x; float fy = other.y; float fz = other.z;
	x = m[0] * fx + m[4] * fy + m[8] * fz + m[12];  // w assumed to be 1.0F
	y = m[1] * fx + m[5] * fy + m[9] * fz + m[13];
	z = m[2] * fx + m[6] * fy + m[10] * fz + m[14];
}

inline void Vector3::TransformTranspose(const Matrix4& mat)
{
	const float* m = mat.M;
	float fx = x; float fy = y; float fz = z;
	x = m[0] * fx + m[1] * fy + m[2] * fz + m[3];  // w assumed to be 1.0F
	y = m[4] * fx + m[5] * fy + m[6] * fz + m[7];
	z = m[8] * fx + m[9] * fy + m[10] * fz + m[11];
}

inline void Vector3::TransformTranspose(const Matrix4& mat, const Vector3& other)
{
	const float* m = mat.M;
	float fx = other.x; float fy = other.y; float fz = other.z;
	x = m[0] * fx + m[1] * fy + m[2] * fz + m[3];  // w assumed to be 1.0F
	y = m[4] * fx + m[5] * fy + m[6] * fz + m[7];
	z = m[8] * fx + m[9] * fy + m[10] * fz + m[11];
}