//------------------------------------------------------------------------
// Math Library for right-handed 3D graphics APIs.
// Copyright (C) 2001-2002 David Poon
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library 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 Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General 
// Public License along with this library; if not, write to the
// Free Software Foundation, Inc., 59 Temple Place, Suite 330, 
// Boston, MA 02111-1307 USA
//------------------------------------------------------------------------
// Revision History:
//
// 31/10/2001 David Poon
// - Changed Vector::angle() to return the angle in degrees NOT radians.
//
// 06/11/2001 David Poon
// - Added Quaternion::set() method.
// - Rewrote Quaternion::toRotationMatrix() method.
// - Changed value of PI to match the M_PI definition in GCC math.h
//
// 10/11/2001 David Poon
// - Removed Camera class from the math library.
//------------------------------------------------------------------------

#include "mathlib.hpp"
#include <cassert>

// PI - matches definition in GCC's math.h
const float PI = 3.14159265358979323846f;

//------------------------------------------------------------------------
// class Matrix
//------------------------------------------------------------------------
// References:
// 1. Tomas Mollar and Eric Haines, "Real-Time Rendering," A K Peters
//    Ltd., Massachusetts, 1999.
//
// 2. Paul Nettle's Matrix template class. http://www.fluidstudios.com
//
// 3. Tomas Moller and John F. Hughes, "Efficiently building a matrix
//    to rotate one vector to another," Journal of Graphics Tools,
//    4(4):1-4, 1999.
//------------------------------------------------------------------------

Matrix::Matrix() {}

Matrix::~Matrix() {}

Matrix::Matrix(const Matrix &rhs)
{
	for (int i = 0; i < 16; i++)
		m_mtx[i] = rhs.m_mtx[i];
}

Matrix &Matrix::operator=(const Matrix &rhs)
{
	if (this != &rhs)
	{
		for (int i = 0; i < 16; i++)
			m_mtx[i] = rhs.m_mtx[i];
	}

	return *this;
}

bool Matrix::operator==(const Matrix &rhs) const
{
	for (int i = 0; i < 16; i++)
	{
		if (!CloseEnough(m_mtx[i], rhs.m_mtx[i]))
			return false;
	}

	return true;
}
		
bool Matrix::operator!=(const Matrix &rhs) const
{
	return !(*this == rhs);
}

void Matrix::add(const Matrix &m)
{
	for (int i = 0; i < 16; i++)
		m_mtx[i] += m.m_mtx[i];
}

void Matrix::add(const Matrix &m1, const Matrix &m2)
{
	Matrix tmp(m1);
	tmp.add(m2);
	*this = tmp;
}

void Matrix::subtract(const Matrix &m)
{
	for (int i = 0; i < 16; i++)
		m_mtx[i] -= m.m_mtx[i];
}

void Matrix::subtract(const Matrix &m1, const Matrix &m2)
{
	Matrix tmp(m1);
	tmp.subtract(m2);
	*this = tmp;
}

void Matrix::multiply(float scalar)
{
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
			elementAt(i,j) *= scalar;
	}
}

void Matrix::multiply(const Matrix &m)
{
	Matrix temp;

	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			temp.elementAt(i, j) = 0.f;
			
			for (int k = 0; k < 4; k++)
				temp.elementAt(i, j) += elementAt(i, k) * m.elementAt(k, j);
		}
	}

	*this = temp;
}
	
void Matrix::multiply(const Matrix &m1, const Matrix &m2)
{
	Matrix tmp(m1);
	tmp.multiply(m2);
	*this = tmp;
}

void Matrix::loadIdentity()
{
	// Loads 'this' Matrix with the Identity Matrix.
	//
	//     | 1	0  0  0 |
	// I = | 0	1  0  0 |
	//     | 0  0  1  0 |
	//     | 0  0  0  1 |
	//
	
	m_mtx[0] = 1.f;
	m_mtx[1] = 0.f;
	m_mtx[2] = 0.f;
	m_mtx[3] = 0.f;

	m_mtx[4] = 0.f;
	m_mtx[5] = 1.f;
	m_mtx[6] = 0.f;
	m_mtx[7] = 0.f;

	m_mtx[8] = 0.f;
	m_mtx[9] = 0.f;
	m_mtx[10] = 1.f;
	m_mtx[11] = 0.f;

	m_mtx[12] = 0.f;
	m_mtx[13] = 0.f;
	m_mtx[14] = 0.f;
	m_mtx[15] = 1.f;
}

bool Matrix::inverse(const Matrix &m)
{
	// Calculates the inverse of Matrix 'm' and stores the 
	// result in 'this' Matrix. The inverse is calculated using 
	// Cramer's rule. If no inverse exists then the Identity Matrix is 
	// loaded into 'this' Matrix and 'false' is returned.

	float d = (m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1)) * (m(2, 2) * m(3, 3) - m(3, 2) * m(2, 3))	- (m(0, 0) * m(2, 1) - m(2, 0) * m(0, 1)) * (m(1, 2) * m(3, 3) - m(3, 2) * m(1, 3))
			+ (m(0, 0) * m(3, 1) - m(3, 0) * m(0, 1)) * (m(1, 2) * m(2, 3) - m(2, 2) * m(1, 3))	+ (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1)) * (m(0, 2) * m(3, 3) - m(3, 2) * m(0, 3))
			- (m(1, 0) * m(3, 1) - m(3, 0) * m(1, 1)) * (m(0, 2) * m(2, 3) - m(2, 2) * m(0, 3))	+ (m(2, 0) * m(3, 1) - m(3, 0) * m(2, 1)) * (m(0, 2) * m(1, 3) - m(1, 2) * m(0, 3));
	
	if (CloseEnough(d, 0.f))
	{
		loadIdentity();
		return false;
	}
	else
	{
		d = 1.f / d;

		Matrix temp;

		temp(0, 0) = d * (m(1, 1) * (m(2, 2) * m(3, 3) - m(3, 2) * m(2, 3)) + m(2, 1) * (m(3, 2) * m(1, 3) - m(1, 2) * m(3, 3)) + m(3, 1) * (m(1, 2) * m(2, 3) - m(2, 2) * m(1, 3)));
		temp(1, 0) = d * (m(1, 2) * (m(2, 0) * m(3, 3) - m(3, 0) * m(2, 3)) + m(2, 2) * (m(3, 0) * m(1, 3) - m(1, 0) * m(3, 3)) + m(3, 2) * (m(1, 0) * m(2, 3) - m(2, 0) * m(1, 3)));
		temp(2, 0) = d * (m(1, 3) * (m(2, 0) * m(3, 1) - m(3, 0) * m(2, 1)) + m(2, 3) * (m(3, 0) * m(1, 1) - m(1, 0) * m(3, 1)) + m(3, 3) * (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1)));
		temp(3, 0) = d * (m(1, 0) * (m(3, 1) * m(2, 2) - m(2, 1) * m(3, 2)) + m(2, 0) * (m(1, 1) * m(3, 2) - m(3, 1) * m(1, 2)) + m(3, 0) * (m(2, 1) * m(1, 2) - m(1, 1) * m(2, 2)));
		temp(0, 1) = d * (m(2, 1) * (m(0, 2) * m(3, 3) - m(3, 2) * m(0, 3)) + m(3, 1) * (m(2, 2) * m(0, 3) - m(0, 2) * m(2, 3)) + m(0, 1) * (m(3, 2) * m(2, 3) - m(2, 2) * m(3, 3)));
		temp(1, 1) = d * (m(2, 2) * (m(0, 0) * m(3, 3) - m(3, 0) * m(0, 3)) + m(3, 2) * (m(2, 0) * m(0, 3) - m(0, 0) * m(2, 3)) + m(0, 2) * (m(3, 0) * m(2, 3) - m(2, 0) * m(3, 3)));
		temp(2, 1) = d * (m(2, 3) * (m(0, 0) * m(3, 1) - m(3, 0) * m(0, 1)) + m(3, 3) * (m(2, 0) * m(0, 1) - m(0, 0) * m(2, 1)) + m(0, 3) * (m(3, 0) * m(2, 1) - m(2, 0) * m(3, 1)));
		temp(3, 1) = d * (m(2, 0) * (m(3, 1) * m(0, 2) - m(0, 1) * m(3, 2)) + m(3, 0) * (m(0, 1) * m(2, 2) - m(2, 1) * m(0, 2)) + m(0, 0) * (m(2, 1) * m(3, 2) - m(3, 1) * m(2, 2)));
		temp(0, 2) = d * (m(3, 1) * (m(0, 2) * m(1, 3) - m(1, 2) * m(0, 3)) + m(0, 1) * (m(1, 2) * m(3, 3) - m(3, 2) * m(1, 3)) + m(1, 1) * (m(3, 2) * m(0, 3) - m(0, 2) * m(3, 3)));
		temp(1, 2) = d * (m(3, 2) * (m(0, 0) * m(1, 3) - m(1, 0) * m(0, 3)) + m(0, 2) * (m(1, 0) * m(3, 3) - m(3, 0) * m(1, 3)) + m(1, 2) * (m(3, 0) * m(0, 3) - m(0, 0) * m(3, 3)));
		temp(2, 2) = d * (m(3, 3) * (m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1)) + m(0, 3) * (m(1, 0) * m(3, 1) - m(3, 0) * m(1, 1)) + m(1, 3) * (m(3, 0) * m(0, 1) - m(0, 0) * m(3, 1)));
		temp(3, 2) = d * (m(3, 0) * (m(1, 1) * m(0, 2) - m(0, 1) * m(1, 2)) + m(0, 0) * (m(3, 1) * m(1, 2) - m(1, 1) * m(3, 2)) + m(1, 0) * (m(0, 1) * m(3, 2) - m(3, 1) * m(0, 2)));
		temp(0, 3) = d * (m(0, 1) * (m(2, 2) * m(1, 3) - m(1, 2) * m(2, 3)) + m(1, 1) * (m(0, 2) * m(2, 3) - m(2, 2) * m(0, 3)) + m(2, 1) * (m(1, 2) * m(0, 3) - m(0, 2) * m(1, 3)));
		temp(1, 3) = d * (m(0, 2) * (m(2, 0) * m(1, 3) - m(1, 0) * m(2, 3)) + m(1, 2) * (m(0, 0) * m(2, 3) - m(2, 0) * m(0, 3)) + m(2, 2) * (m(1, 0) * m(0, 3) - m(0, 0) * m(1, 3)));
		temp(2, 3) = d * (m(0, 3) * (m(2, 0) * m(1, 1) - m(1, 0) * m(2, 1)) + m(1, 3) * (m(0, 0) * m(2, 1) - m(2, 0) * m(0, 1)) + m(2, 3) * (m(1, 0) * m(0, 1) - m(0, 0) * m(1, 1)));
		temp(3, 3) = d * (m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) + m(1, 0) * (m(2, 1) * m(0, 2) - m(0, 1) * m(2, 2)) + m(2, 0) * (m(0, 1) * m(1, 2) - m(1, 1) * m(0, 2)));
	
		*this = temp;
		return true;
	}
}

void Matrix::transpose(const Matrix &m)
{
	// Calculates the transpose of Matrix 'm' and stores the transposed
	// Matrix in 'this' Matrix.
	
	Matrix temp;

	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
			temp(i, j) = m(j, i);
	}

	*this = temp;
}

void Matrix::translate(float tx, float ty, float tz)
{
	// Creates a translation matrix and stores it in 'this' matrix.
	//
	//                 | 1  0  0  tx |
	// t(tx, ty, tz) = | 0  1  0  ty |
	//                 | 0  0  1  tz |
	//                 | 0  0  0  1  |

	elementAt(0, 0) = 1.f;
	elementAt(0, 1) = 0.f;
	elementAt(0, 2) = 0.f;
	elementAt(0, 3) = tx;

	elementAt(1, 0) = 0.f;
	elementAt(1, 1) = 1.f;
	elementAt(1, 2) = 0.f;
	elementAt(1, 3) = ty;

	elementAt(2, 0) = 0.f;
	elementAt(2, 1) = 0.f;
	elementAt(2, 2) = 1.f;
	elementAt(2, 3) = tz;

	elementAt(3, 0) = 0.f;
	elementAt(3, 1) = 0.f;
	elementAt(3, 2) = 0.f;
	elementAt(3, 3) = 1.f;
}

void Matrix::scale(float sx, float sy, float sz)
{
	// Creates a scaling matrix and stores it in 'this' matrix.
	//
	//                 | sx   0    0    0 |
	// S(sx, sy, sz) = | 0    sy   0    0 |
	//                 | 0    0    sz   0 |
	//                 | 0    0    0    1 |
	//

	elementAt(0, 0) = sx;
	elementAt(0, 1) = 0.f;
	elementAt(0, 2) = 0.f;
	elementAt(0, 3) = 0.f;

	elementAt(1, 0) = 0.f;
	elementAt(1, 1) = sy;
	elementAt(1, 2) = 0.f;
	elementAt(1, 3) = 0.f;

	elementAt(2, 0) = 0.f;
	elementAt(2, 1) = 0.f;
	elementAt(2, 2) = sz;
	elementAt(2, 3) = 0.f;

	elementAt(3, 0) = 0.f;
	elementAt(3, 1) = 0.f;
	elementAt(3, 2) = 0.f;
	elementAt(3, 3) = 1.f;
}

void Matrix::rotate(float angle, Vector &axis)
{
	// Creates a rotation matrix about the axis (x, y, z). The axis must 
	// be a unit vector. If it isn't rotate() will normalize the axis. 
	// The angle is assumed to be in degrees.
	//
	// Let u = axis of rotation = (x, y, z)
	//
	//             | x^2(1-c) + c   xy(1-c) - zs   xz(1-c) + ys   0 |
	// Ru(angle) = | yx(1-c) + zs   y^2(1-c) + c   yz(1-c) - xs   0 |
	//             | xz(1-c) - ys   yz(1-c) + xs   z^2(1-c) + c   0 |
	//             |      0              0               0        1 |
	//
	// where,
	//	c = cos(angle)
	//  s = sin(angle)
	//

	if (CloseEnough(axis.length(), 0.f))
	{
		loadIdentity();
	}
	else
	{
		axis.normalize();
		float x = axis.x;
		float y = axis.y;
		float z = axis.z;

		angle = Degrees2Radians(angle);
		float c = cosf(angle);
		float s = sinf(angle);

		elementAt(0, 0) = x * x * (1.f - c) + c;
		elementAt(0, 1) = x * y * (1.f - c) - (z * s);
		elementAt(0, 2) = x * z * (1.f - c) + (y * s);
		elementAt(0, 3) = 0.f;

		elementAt(1, 0) = y * x * (1.f - c) + (z * s);
		elementAt(1, 1) = y * y * (1.f - c) + c;
		elementAt(1, 2) = y * z * (1.f - c) - (x * s);
		elementAt(1, 3) = 0.f;

		elementAt(2, 0) = x * z * (1.f - c) - (y * s);
		elementAt(2, 1) = y * z * (1.f - c) + (x * s);
		elementAt(2, 2) = z * z * (1.f - c) + c;
		elementAt(2, 3) = 0.f;

		elementAt(3, 0) = 0.f;
		elementAt(3, 1) = 0.f;
		elementAt(3, 2) = 0.f;
		elementAt(3, 3) = 1.f;
	}
}

void Matrix::rotate(const Vector &from, const Vector &to)
{
	// Creates a rotation matrix that will rotate the Vector 'from' into
	// the Vector 'to'. The resulting matrix is stored in 'this' Matrix.
	// For this method to work correctly, Vector 'from' and Vector 'to' 
	// must both be unit length Vectors.

	assert(from.isUnitVector() && to.isUnitVector());
	float e = from.dot(to);
	
	if (CloseEnough(e, 1.f))
	{
		// Special case where 'from' is equal to 'to'. In other words,
		// the angle between Vector 'from' and Vector 'to' is zero 
		// degrees. In this case the identity matrix is returned in
		// Matrix 'result'.

		loadIdentity();
	}
	else if (CloseEnough(e, -1.f))
	{
		// Special case where 'from' is directly opposite to 'to'. In
		// other words, the angle between Vector 'from' and Vector 'to'
		// is 180 degrees. In this case, the following matrix is used:
		//
		// Let:
		//   F = from
		//   S = vector perpendicular to F
		//   U = S X F
		//
		// We want to rotate from (F, U, S) to (-F, U, -S)
		//
		// | -FxFx+UxUx-SxSx  -FxFy+UxUy-SxSy  -FxFz+UxUz-SxSz  0 |
		// | -FxFy+UxUy-SxSy  -FyFy+UyUy-SySy  -FyFz+UyUz-SySz  0 |
		// | -FxFz+UxUz-SxSz  -FyFz+UyUz-SySz  -FzFz+UzUz-SzSz  0 |
		// |       0                 0                0         1 |

		Vector side(0.f, from.z, -from.y);
		if (CloseEnough(side.dot(side), 0.f))
		{
			side.x = -from.z;
			side.y = 0.f;
			side.z = from.x;
		}
		
		side.normalize();

		Vector up;
		up.cross(side, from);
		up.normalize();

		elementAt(0, 0) = -(from.x * from.x) + (up.x * up.x) - (side.x * side.x);
		elementAt(0, 1) = -(from.x * from.y) + (up.x * up.y) - (side.x * side.y);
		elementAt(0, 2) = -(from.x * from.z) + (up.x * up.z) - (side.x * side.z);
		elementAt(0, 3) = 0.f;
		elementAt(1, 0) = -(from.x * from.y) + (up.x * up.y) - (side.x * side.y);
		elementAt(1, 1) = -(from.y * from.y) + (up.y * up.y) - (side.y * side.y);
		elementAt(1, 2) = -(from.y * from.z) + (up.y * up.z) - (side.y * side.z);
		elementAt(1, 3) = 0.f;
		elementAt(2, 0) = -(from.x * from.z) + (up.x * up.z) - (side.x * side.z);
		elementAt(2, 1) = -(from.y * from.z) + (up.y * up.z) - (side.y * side.z);
		elementAt(2, 2) = -(from.z * from.z) + (up.z * up.z) - (side.z * side.z);
		elementAt(2, 3) = 0.f;
		elementAt(3, 0) = 0.f;
		elementAt(3, 1) = 0.f;
		elementAt(3, 2) = 0.f;
		elementAt(3, 3) = 1.f;
	}
	else
	{
		// This is the most common case. Creates the rotation matrix:
		//
		//               | E + HVx^2    HVxVy - Vz   HVxVz + Vy   0 |
		// R(from, to) = | HVxVy + Vz   E + HVy^2    HVyVz - Vx   0 |
		//               | HVxVz - Vy   HVyVz + Vx   E + HVz^2    0 |
		//               |     0            0            0        1 |
		//
		// where,
		//   V = from X to
		//   E = from.dot(to)
		//   H = (1 - E) / V.dot(V)

		Vector v;
		v.cross(from, to);
		v.normalize();

		float h = (1.f - e) / v.dot(v);

		elementAt(0, 0) = e + h * v.x * v.x;
		elementAt(0, 1) = h * v.x * v.y - v.z;
		elementAt(0, 2) = h * v.x * v.z + v.y;
		elementAt(0, 3) = 0.f;

		elementAt(1, 0) = h * v.x * v.y + v.z;
		elementAt(1, 1) = e + h * v.y * v.y;
		elementAt(1, 2) = h * v.y * v.z - v.x;
		elementAt(1, 3) = 0.f;

		elementAt(2, 0) = h * v.x * v.z - v.y;
		elementAt(2, 1) = h * v.y * v.z + v.x;
		elementAt(2, 2) = e + h * v.z * v.z;
		elementAt(2, 3) = 0.f;

		elementAt(3, 0) = 0.f;
		elementAt(3, 1) = 0.f;
		elementAt(3, 2) = 0.f;
		elementAt(3, 3) = 1.f;
	}
}

//------------------------------------------------------------------------
// struct Point
//------------------------------------------------------------------------

Point::Point()
{
	x = y = z = 0.f;
	w = 1.f;
}

Point::~Point() {}

Point::Point(float x, float y, float z)
{
	set(x, y, z);
}

Point::Point(const Point &rhs)
{
	set(rhs.x, rhs.y, rhs.z);
}

void Point::set(float x, float y, float z)
{
	this->x = x;
	this->y = y;
	this->z = z;
	w = 1.f;
}

void Point::multiply(const Point &p, const Matrix &m)
{
	// Applies the transformation in Matrix 'm' to Point 'p' and
	// stores the transformed Point in 'this' Point.

	float x = (m(0, 0) * p.x) + (m(0, 1) * p.y) + (m(0, 2) * p.z) + (m(0, 3) * p.w);
	float y = (m(1, 0) * p.x) + (m(1, 1) * p.y) + (m(1, 2) * p.z) + (m(1, 3) * p.w);
	float z = (m(2, 0) * p.x) + (m(2, 1) * p.y) + (m(2, 2) * p.z) + (m(2, 3) * p.w);
	float w = (m(3, 0) * p.x) + (m(3, 1) * p.y) + (m(3, 2) * p.z) + (m(3, 3) * p.w);

	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

Point &Point::operator=(const Point &rhs)
{
	if (this != &rhs)
	{
		x = rhs.x;
		y = rhs.y;
		z = rhs.z;
		w = rhs.w;
	}

	return *this;
}

bool Point::operator==(const Point &rhs) const
{
	return CloseEnough(x, rhs.x) && CloseEnough(y, rhs.y) 
		&& CloseEnough(z, rhs.z) && CloseEnough(w, rhs.w);
}

//------------------------------------------------------------------------
// struct Vector
//------------------------------------------------------------------------

Vector::Vector()
{
	x = y = z = w = 0.f;
}

Vector::~Vector() {}

Vector::Vector(const Point &from, const Point &to)
{
	set(from, to);
}

Vector::Vector(float x, float y, float z)
{
	set(x, y, z);
}

Vector::Vector(const Vector &rhs)
{
	set(rhs.x, rhs.y, rhs.z);
}

void Vector::add(const Vector &v)
{
	// Adds vector 'v' to 'this' vector.

	x += v.x;
	y += v.y;
	z += v.z;
	w = 0.f;
}

void Vector::add(const Vector &v1, const Vector &v2)
{
	// Adds vector 'v1' to vector 'v2' and stores the resulting vector
	// in vector 'this'. The vectors are added in the order: 
	// 'v1' + 'v2' = 'this'.
	
	Vector tmp(v1);
	tmp.add(v2);
	*this = tmp;
}

float Vector::angle(const Vector &other) const
{
	// Finds the angle between vector 'this' and vector 'other'.
	// The angle returned is in degrees.

	float a = (x * other.x) + (y * other.y) + (z * other.z);
	float b = length() * other.length();
	
	return Radians2Degrees(acosf(a / b));
}

void Vector::set(float x, float y, float z)
{
	// Creates a vector from three cartesian coordinates.

	this->x = x;
	this->y = y;
	this->z = z;
	this->w = 0.f;
}

void Vector::set(const Point &from, const Point &to)
{
	// Creates a vector from point 'from' to point 'to'.

	x = to.x - from.x;
	y = to.y - from.y;
	z = to.z - from.z;
	w = 0.f;
}

void Vector::cross(const Vector &v)
{
	// Calculates the cross product: 'this' x 'v'.
	
	Vector temp;

	temp.x = (y * v.z) - (z * v.y);
	temp.y = (z * v.x) - (x * v.z);
	temp.z = (x * v.y) - (y * v.x);
	
	*this = temp;
}

void Vector::cross(const Vector &v1, const Vector &v2)
{
	// Calculates the cross product: 'v1' x 'v2'.
	
	Vector tmp(v1);
	tmp.cross(v2);
	*this = tmp;
}

void Vector::divide(const float scalar)
{
	// Divides each component of this vector by the scalar value.

	assert(scalar != 0.f);
	x /= scalar;
	y /= scalar;
	z /= scalar;
	w /= scalar;
}

float Vector::dot(const Vector &other) const
{
	// Calculates the dot product: 'this' dot 'other'.
	// The result can be used to determine if vector 'this' is 
	// orthogonal (or perpendicular) to vector 'other':
	//   0 if angle between 'this' and 'other' equals 90 degrees.
	// < 0 if angle between 'this' and 'other' less than 90 degrees.
	// > 0 if angle between 'this' and 'other' greater than 90 degrees.

	return (x * other.x) + (y * other.y) + (z * other.z);
}

float Vector::dot(const Point &other) const
{
	// The point 'other' is interpreted as a vector from the origin to
	// the point 'other'.

	return (x * other.x) + (y * other.y) + (z * other.z);
}

bool Vector::isUnitVector() const
{
	// Returns true if 'this' vector is of unit length.

	return CloseEnough(length(), 1.f);
}

bool Vector::isZeroVector() const
{
	// Returns true if 'this' vector's (x,y,z,w) components are all zero.

	return CloseEnough(x, 0.f) && CloseEnough(y, 0.f)
		&& CloseEnough(z, 0.f) && CloseEnough(w, 0.f);
}

float Vector::length() const
{
	// Returns the length (magnitude) of the vector.

	return sqrtf((x * x) + (y * y) + (z * z));
}

void Vector::multiply(const float scalar)
{
	// Multiplies each component of this vector by the scalar value.

	x *= scalar;
	y *= scalar;
	z *= scalar;
	w *= scalar;
}

void Vector::multiply(const Vector &v, const Matrix &m)
{
	// Applies the transformation in Matrix 'm' to Vector 'v' and
	// stores the transformed Vector in 'this' Vector.

	float x = (m(0, 0) * v.x) + (m(0, 1) * v.y) + (m(0, 2) * v.z) + (m(0, 3) * v.w);
	float y = (m(1, 0) * v.x) + (m(1, 1) * v.y) + (m(1, 2) * v.z) + (m(1, 3) * v.w);
	float z = (m(2, 0) * v.x) + (m(2, 1) * v.y) + (m(2, 2) * v.z) + (m(2, 3) * v.w);
	float w = (m(3, 0) * v.x) + (m(3, 1) * v.y) + (m(3, 2) * v.z) + (m(3, 3) * v.w);

	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

void Vector::normalize()
{
	// Converts the vector to unit length.

	float length = sqrtf((x * x) + (y * y) + (z * z));
	
	assert(length != 0.f);
	length = 1.f / length;

	x *= length;
	y *= length;
	z *= length;
}

void Vector::subtract(const Vector &v)
{
	// Subtracts vector 'this' from vector 'v'.
	
	Vector temp;

	temp.x = x - v.x;
	temp.y = y - v.y;
	temp.z = z - v.z;

	*this = temp;
}

void Vector::subtract(const Vector &v1, const Vector &v2)
{
	// Subtracts vector 'v1' from vector 'v2' and stores the 
	// resulting vector in vector 'this'. The vectors are subtracted 
	// in the order: 'v1' - 'v2' = 'this'.

	Vector tmp(v1);
	tmp.subtract(v2);
	*this = tmp;
}

Vector &Vector::operator=(const Vector &rhs)
{
	if (this != &rhs)
	{
		x = rhs.x;
		y = rhs.y;
		z = rhs.z;
		w = rhs.w;
	}

	return *this;
}

bool Vector::operator==(const Vector &rhs) const
{
	return CloseEnough(x, rhs.x) && CloseEnough(y, rhs.y)
		&& CloseEnough(z, rhs.z) && CloseEnough(w, rhs.w);
}

//------------------------------------------------------------------------
// struct Quaternion
//------------------------------------------------------------------------
// References:
// 1. Bobick, Nick. "Rotating Objects Using Quaternions," Game Developer, 
//    February 1998.
//
// 2. Shoemake, Ken. "Animating Rotation with Quaternion Curves," 
//    Computer Graphics, 19 (3), July 1985. (SIGGRAPH '85 Proceedings),
//    245-254.
//
// 3. Shoemake, Ken. "Quaternion Calculus For Animation," Notes for 
//    Course #23, Math for SIGGRAPH, SIGGRAPH '89.
//------------------------------------------------------------------------

Quaternion::Quaternion()
{
	x = y = z = w = 0.f;
}

Quaternion::~Quaternion() {}

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

Quaternion::Quaternion(float x, float y, float z, float w)
{
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

Quaternion::Quaternion(const Vector &axis, float angle)
{
	fromAxisAngle(axis, angle);
}

Quaternion::Quaternion(float rx, float ry, float rz)
{
	fromEuler(rx, ry, rz);
}

Quaternion::Quaternion(const Matrix &m)
{
	fromRotationMatrix(m);
}

void Quaternion::add(const Quaternion &q)
{
	// Adds quaternion 'q' to 'this' quaternion.
	
	x += q.x;
	y += q.y;
	z += q.z;
	w += q.w;

	normalize();
}

void Quaternion::add(const Quaternion &q1, const Quaternion &q2)
{
	// Adds quaternion 'q1' to quaternion 'q2' and stores the resulting
	// quaternion in 'this'. The two quaternions to add must both already
	// be unit quaternions. The quaternions are added in the order: 
	// this + other = result

	Quaternion tmp(q1);
	tmp.add(q2);
	*this = tmp;
}

void Quaternion::conjugate(const Quaternion &q)
{
	// Finds the conjugate of quaternion 'q'.
	
	x = -q.x;
	y = -q.y;
	z = -q.z;
	w = q.w;
}

void Quaternion::fromAxisAngle(const Vector &axis, float angle)
{
	// Creates a quaternion from an axis (x, y, z) and an angle.
	// The angle must be in degrees.

	// Normalize the axis of rotation.
	Vector unitAxis = axis;
	unitAxis.normalize();

	// Assemble the quaternion.
	angle = Degrees2Radians(angle);
	float sine = sinf(angle);
	float cosine = cosf(angle);

	x = unitAxis.x * sine;
	y = unitAxis.y * sine;
	z = unitAxis.z * sine;
	w = unitAxis.z * cosine;

	normalize();
}

void Quaternion::fromEuler(float rx, float ry, float rz)
{
	// Creates a quaternion from an Euler transform. 'rx' is the
	// rotation about the x-axis (also called pitch). 'ry' is the
	// rotation about the y-axis (also called heading or yaw).
	// 'rz' is the rotation about the z-axis (also called roll).
	// The rotation about each axis must be in degrees.

	rx = Degrees2Radians(rx) * 0.5f;
	ry = Degrees2Radians(ry) * 0.5f;
	rz = Degrees2Radians(rz) * 0.5f;

	float cx = cosf(rx);
	float cy = cosf(ry);
	float cz = cosf(rz);
	float sx = sinf(rx);
	float sy = sinf(ry);
	float sz = sinf(rz);

	float cc = cx * cz;
	float cs = cx * sz;
	float sc = sx * cz;
	float ss = sx * sz;

	x = cy * sc - sy * cs;
	y = cy * ss + sy * cc;
	z = cy * cs - sy * sc;
	w = cy * cc + sy * ss;

	normalize();
}

void Quaternion::fromRotationMatrix(const Matrix &m)
{
	// Creates a quaternion from a rotation matrix.

	float trace = m(0,0) + m(1,1) + m(2,2);

	if (trace > 0.f)
	{
		float s = sqrtf(trace + 1.f);
		
		w = s * 0.5f;
		
		assert(s != 0.f);
		s = 0.5f / s;

		x = (m(1,2) - m(2,1)) * s;
		y = (m(2,0) - m(0,2)) * s;
		z = (m(0,1) - m(1,0)) * s;
	}
	else
	{
		int nxt[] = {1, 2, 0};
		int i = 0;

		if (m(1,1) > m(0,0))
			i = 1;

		if (m(2,2) > m(i,i))
			i = 2;

		int j = nxt[i];
		int k = nxt[j];
		float s = sqrtf((m(i,i) - (m(j,j) + m(k,k)) + 1.f));
		float q[4];

		q[i] = s * 0.5f;

		assert(s != 0.f);
		s = 0.5f / s;

		q[3] = (m(j,k) - m(k,j)) * s;
		q[j] = (m(i,j) + m(j,i)) * s;
		q[k] = (m(i,k) + m(k,i)) * s;

		x = q[0];
		y = q[1];
		z = q[2];
		w = q[3];
	}
}

void Quaternion::inverse(const Quaternion &q)
{
	// Find the inverse of quaternion 'q'.
	
	float lgth = q.length();

	assert(lgth != 0.f);
	lgth = 1.f / lgth;

	x = -q.x * lgth;
	y = -q.y * lgth;
	z = -q.z * lgth;
	w = q.w * lgth;
}

bool Quaternion::isUnitQuaternion() const
{
	// Checks if the quaternion is of unit length.

	return CloseEnough(length(), 1.f);
}

float Quaternion::length() const
{
	// Returns the length (magnitude) of the quaternion.
	
	return sqrtf((x * x) + (y * y) + (z * z) + (w * w));
}

void Quaternion::multiply(Quaternion &q)
{
	// Multiplies 'this' quaternion with the quaternion 'q' and stores
	// the result in quaternion 'this'. It is assumed that the two 
	// quaternions to be multiplied are unit quaternions. The
	// quaternions are multiplied in the order: this = this * q

	Quaternion temp;
	
	temp.x = (w * q.x) + (x * q.w) + (y * q.z) - (z * q.y);
	temp.y = (w * q.y) + (y * q.w) + (z * q.x) - (x * q.z);
	temp.z = (w * q.z) + (z * q.w) + (x * q.y) - (y * q.x);
	temp.w = (w * q.w) - (x * q.x) - (y * q.y) - (z * q.z);

	temp.normalize();
	*this = temp;
}

void Quaternion::multiply(Quaternion &q1, Quaternion &q2)
{
	// Multiplies quaternion 'q1' with the quaternion 'q2' and stores
	// the result in quaternion 'this'. It is assumed that the two 
	// quaternions to be multiplied are unit quaternions. The
	// quaternions are multiplied in the order: this = q1 * q2

	Quaternion tmp(q1);
	tmp.multiply(q2);
	*this = tmp;
}

void Quaternion::normalize()
{
	// Converts the quaternion to unit length.

	float lgth = sqrtf((x * x) + (y * y) + (z * z) + (w * w));
	
	assert(lgth != 0.f);
	lgth = 1.f / lgth;

	x *= lgth;
	y *= lgth;
	z *= lgth;
	w *= lgth;
}

void Quaternion::set(float x, float y, float z, float w)
{
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

void Quaternion::slerp(const Quaternion &from, const Quaternion &to, float t)
{
	// Smoothly interpolates from quaternion 'from' to quaternion 'to'
	// using spherical linear interpolation (slerp). The resulting
	// slerp quaternion is stored in 'this' quaternion.
	// As 't' goes from 0 to 1, the slerp quaternion goes from 'from'
	// to 'to'.

	float cosom = (from.x * to.x) + (from.y * to.y) 
		+ (from.z * to.z) + (from.w * to.w);

	Quaternion temp = to;

	// Adjust sign (if necessary).
	if (cosom < 0.f)
	{
		cosom = -cosom;
		temp.x = -temp.x;
		temp.y = -temp.y;
		temp.z = -temp.z;
		temp.w = -temp.w;
	}

	float scale0;
	float scale1;

	// Calculate coefficients.
	if (CloseEnough(1.f - cosom, 0.f))
	{
		// 'from' and 'to' very close...linearly interpolate.
		scale0 = 1.f - t;
		scale1 = t;
	}
	else
	{
		// Standard case...slerp.
		float omega = acosf(cosom);
		float sinom = sinf(omega);
		
		scale0 = sinf((1.f - t) * omega) / sinom;
		scale1 = sinf(t * omega) / sinom;
	}

	// Calculate final slerp quaternion.
	x = (scale0 * from.x) + (scale1 * temp.x);
	y = (scale0 * from.y) + (scale1 * temp.y);
	z = (scale0 * from.z) + (scale1 * temp.z);
	w = (scale0 * from.w) + (scale1 * temp.w);
}

void Quaternion::toAxisAngle(Vector &axis, float &angle) const
{
	// Returns the axis-angle representation for 'this' quaternion. 
	// The axis of rotation is returned in vector 'axis'. The angle
	// of rotation is returned in 'angle' and will be in degrees.

	float lgth = length();

	if (CloseEnough(lgth, 0.f))
	{
		axis.x = 0.f;
		axis.y = 0.f;
		axis.z = 1.f;
		axis.w = 0.f;
		angle = 0.f;
	}
	else
	{
		lgth = 1.f / lgth;
		axis.x = x * lgth;
		axis.y = y * lgth;
		axis.z = z * lgth;
		axis.w = 0.f;
		angle = Radians2Degrees(2.f * acosf(w));
	}
}

void Quaternion::toRotationMatrix(Matrix &m) const
{
	// Returns the rotation matrix for this quaternion.
	// The rotation matrix is returned in matrix 'm'.
	//
	//  | 1 - 2(y^2 + z^2)  2(xy - wz)         2(xy + wy)        0 |
	//  | 2(xy + wz)        1 - 2(x^2 + z^2)   2(yz - wx)        0 |
	//  | 2(xz - wy)        2(yz + wx)         1 - 2(x^2 + y^2)  0 |
	//  |      0                  0                  0           1 |
	//

	float x2 = x + x; 
	float y2 = y + y; 
	float z2 = z + z;
	float xx = x * x2;
	float xy = x * y2;
	float xz = x * z2;
	float yy = y * y2;
	float yz = y * z2;
	float zz = z * z2;
	float wx = w * x2;
	float wy = w * y2;
	float wz = w * z2;

	m(0,0) = 1.f - (yy + zz);
	m(0,1) = xy - wz;
	m(0,2) = xz + wy;
	m(0,3) = 0.f;

	m(1,0) = xy + wz;
	m(1,1) = 1.f - (xx + zz);
	m(1,2) = yz - wx;
	m(1,3) = 0.f;

	m(2,0) = xz - wy;
	m(2,1) = yz + wx;
	m(2,2) = 1.f - (xx + yy);
	m(2,3) = 0.f;

	m(3,0) = 0.f;
	m(3,1) = 0.f;
	m(3,2) = 0.f;
	m(3,3) = 1.f;
}

Quaternion &Quaternion::operator=(const Quaternion &rhs)
{
	if (this != &rhs)
	{
		x = rhs.x;
		y = rhs.y;
		z = rhs.z;
		w = rhs.w;
	}

	return *this;
}

bool Quaternion::operator==(const Quaternion &rhs) const
{
	return CloseEnough(x, rhs.x) && CloseEnough(y, rhs.y)
		&& CloseEnough(z, rhs.z) && CloseEnough(w, rhs.w);
}

//------------------------------------------------------------------------
// struct Plane
//------------------------------------------------------------------------

Plane::Plane()
{
	distOrigin = 0.f;
}

Plane::~Plane() {}

Plane::Plane(const Plane &rhs)
{
	normal = rhs.normal;
	distOrigin = rhs.distOrigin;
}

Plane::Plane(const Point &p1, const Point &p2, const Point &p3)
{
	set(p1, p2, p3);
}

void Plane::set(const Point &p1, const Point &p2, const Point &p3)
{
	// Find the normal to the plane.
	Vector u, v;
	u.set(p1, p2);
	v.set(p1, p3);
	normal.cross(u, v);
	normal.normalize();

	// Find the distance of the plane to the origin.
	distOrigin = -normal.dot(p1);
}

float Plane::distanceTo(const Point &p)
{
	// Calculates the smallest distance from the point 'p' to this
	// plane. The distance returned can be interpreted as:
	//   0 if the point 'p' is located in this plane
	// > 0 if the point 'p' is in front of this plane
	// < 0 if the point 'p' is behind this plane

	return normal.dot(p) + distOrigin;
}

Plane &Plane::operator=(const Plane &rhs)
{
	if (this != &rhs)
	{
		normal = rhs.normal;
		distOrigin = rhs.distOrigin;
	}

	return *this;
}

bool Plane::operator==(const Plane &rhs) const
{
	return (normal == rhs.normal) 
		&& CloseEnough(distOrigin, rhs.distOrigin);
}
 

//------------------------------------------------------------------------
// Math Library for right-handed 3D graphics APIs.
// Copyright (C) 2001-2002 David Poon
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library 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 Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General 
// Public License along with this library; if not, write to the
// Free Software Foundation, Inc., 59 Temple Place, Suite 330, 
// Boston, MA 02111-1307 USA
//------------------------------------------------------------------------

#ifndef MATH_LIBRARY_RIGHT_HANDED_HPP
#define MATH_LIBRARY_RIGHT_HANDED_HPP

#include <cmath>
#include <limits>

extern const float PI;

//------------------------------------------------------------------------
// Math library inlined functions
//------------------------------------------------------------------------

inline bool CloseEnough(const float &f1, const float &f2)
{
	// Determines whether the two floating point values f1 and f2 are
	// close enough together that they can be considered equal.
	// This function is taken from Pete Becker's "The Journeyman's Shop"
	// column published in C/C++ Users Journal, December 2000. It is 
	// presented here slightly modified to deal exclusively with floats.
	return fabs((f1 - f2) / ((f2 == 0.f) ? 1.f : f2)) 
		< std::numeric_limits<float>::epsilon();
}

inline float Degrees2Radians(float angle)
{
	// Converts the angle (in degrees) to radians.
	return (angle * PI) / 180.f;
}

inline float Radians2Degrees(float angle)
{
	// Converts the angle (in radians) to degrees.
	return (angle * 180.f) / PI;
}

//------------------------------------------------------------------------
// Definitions of common 3D data structures for use in 3D graphics.
// These data structures are suitable for graphics APIs that use a
// right-handed cartesian coordinate system (i.e., OpenGL).
//------------------------------------------------------------------------

class Matrix;
struct Point;
struct Vector;
struct Quaternion;
struct Plane;

class Matrix
{
public:
	Matrix();
	Matrix(const Matrix &rhs);
	~Matrix();

	float &operator()(int row, int col) {return elementAt(row, col);}
	const float &operator()(int row, int col) const {return elementAt(row, col);}

	void add(const Matrix &m);
	void add(const Matrix &m1, const Matrix &m2);
	void subtract(const Matrix &m);
	void subtract(const Matrix &m1, const Matrix &m2);
	void multiply(float scalar);
	void multiply(const Matrix &m);
	void multiply(const Matrix &m1, const Matrix &m2);
		
	Matrix &operator=(const Matrix &rhs);
	bool operator==(const Matrix &rhs) const;
	bool operator!=(const Matrix &rhs) const;
	
	operator float*() {return m_mtx;}
	operator const float*() const {return m_mtx;}

	bool inverse(const Matrix &m);
	void loadIdentity();
	void transpose(const Matrix &m);
	
	void rotate(float angle, Vector &axis);
	void rotate(const Vector &from, const Vector &to);
	void scale(float sx, float sy, float sz);
	void translate(float tx, float ty, float tz);

private:
	float &elementAt(int row, int col) {return m_mtx[col*4+row];}
	const float &elementAt(int row, int col) const {return m_mtx[col*4+row];}

	float m_mtx[16];
};

struct Point
{
	float x, y, z, w;

	Point();
	Point(const Point &rhs);
	Point(float x, float y, float z);
	~Point();

	void set(float x, float y, float z);

	void multiply(const Point &p, const Matrix &m);

	Point &operator=(const Point &rhs);
	bool operator==(const Point &rhs) const;
};

struct Vector
{
	float x, y, z, w;

	Vector();
	Vector(const Vector &rhs);
	Vector(float x, float y, float z);
	Vector(const Point &from, const Point &to);
	~Vector();

	void set(float x, float y, float z);
	void set(const Point &from, const Point &to);

	void add(const Vector &v);
	void add(const Vector &v1, const Vector &v2);
	void cross(const Vector &v);
	void cross(const Vector &v1, const Vector &v2);
	void divide(const float scalar);
	float dot(const Vector &other) const;
	float dot(const Point &other) const;
	void multiply(const float scalar);
	void multiply(const Vector &v, const Matrix &m);
	void subtract(const Vector &v);
	void subtract(const Vector &v1, const Vector &v2);

	float angle(const Vector &other) const;
	bool isUnitVector() const;
	bool isZeroVector() const;
	float length() const;
	void normalize();
		
	Vector &operator=(const Vector &rhs);
	bool operator==(const Vector &rhs) const;
};

struct Quaternion
{
	float x, y, z, w;

	Quaternion();
	Quaternion(const Quaternion &rhs);
	Quaternion(float x, float y, float z, float w);
	Quaternion(const Vector &axis, float angle);
	Quaternion(float rx, float ry, float rz);
	Quaternion(const Matrix &m);
	~Quaternion();

	void add(const Quaternion &q);
	void add(const Quaternion &q1, const Quaternion &q2);
	void multiply(Quaternion &q);
	void multiply(Quaternion &q1, Quaternion &q2);
	void inverse(const Quaternion &q);
	void conjugate(const Quaternion &q);

	void fromAxisAngle(const Vector &axis, float angle);
	void fromEuler(float rx, float ry, float rz);
	void fromRotationMatrix(const Matrix &m);
	bool isUnitQuaternion() const;
	float length() const;
	void normalize();
	void set(float x, float y, float z, float w);
	void slerp(const Quaternion &from, const Quaternion &to, float t);
	void toAxisAngle(Vector &axis, float &angle) const;
	void toRotationMatrix(Matrix &m) const;

	Quaternion &operator=(const Quaternion &rhs);
	bool operator==(const Quaternion &rhs) const;
};

struct Plane
{
	Vector normal;
	float distOrigin;	// smallest distance from plane to origin

	Plane();
	Plane(const Plane &rhs);
	Plane(const Point &p1, const Point &p2, const Point &p3);
	~Plane();

	void set(const Point &p1, const Point &p2, const Point &p3);
	float distanceTo(const Point &p);

	Plane &operator=(const Plane &rhs);
	bool operator==(const Plane &rhs) const;
};

#endif 

//------------------------------------------------------------------------
// Tester for the 3D Math Library.
// Copyright (C) 2001-2002 David Poon
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library 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 Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General 
// Public License along with this library; if not, write to the
// Free Software Foundation, Inc., 59 Temple Place, Suite 330, 
// Boston, MA 02111-1307 USA
//------------------------------------------------------------------------

#include <cstdio>
#include "mathlib.hpp"

bool TestCastToFloatPtr(float *array = 0)
{
	if (!array)
		return false;

	return true;
}

void TestMatrix()
{
	/*
	Expected Output for TestMatrix():
	----------------------------------------------
	Testing Matrix portion of Maths Library...
	Test #1: Matrix float* cast operator PASSED
	Test #2: Matrix (row, col) operator PASSED
	Test #3: Matrix multiplication operator PASSED
	Test #4: Matrix transpose PASSED
	Test #5: Matrix inverse PASSED
	Test #6: Matrix addition PASSED
	Test #7: Matrix subtraction PASSED
	Test #8: Matrix multiplication by scalar PASSED
	Test #9: (1.f / 10.f) * 10.f == 1.f  PASSED
	Test #10: Build matrix to rotate from one vector to another
			Rotate(from, to) Case 1 PASSED
			Rotate(from, to) Case 2 PASSED
			Rotate(from, to) Case 3 PASSED
	Test #11: Build translation matrix PASSED
	Test #12: Build scaling matrix PASSED
	Test #13: Build rotation matrix PASSED
	*/

	puts("\nTesting Matrix portion of Maths Library...");

	// Test #1: Matrix float* cast operator.
	{
		Matrix m;
		if (!TestCastToFloatPtr(m))
			puts("Test #1: Matrix float* cast operator FAILED");
		else
			puts("Test #1: Matrix float* cast operator PASSED");
	}

	// Test #2: Matrix (row, col) operator.
	{
		Matrix m;
		m(1, 2) = 12.f;
		if (!CloseEnough(m(1, 2), 12.f))
			puts("Test #2: Matrix (row, col) operator FAILED");
		else
			puts("Test #2: Matrix (row, col) operator PASSED");
	}

	// Test #3: Matrix multiplication operator.
	{
		Matrix m;
		m(0,0) = 1.f;	m(0,1) = 1.f;	m(0,2) = 1.f;	m(0,3) = 1.f;
		m(1,0) = 2.f;	m(1,1) = 2.f;	m(1,2) = 2.f;	m(1,3) = 2.f;
		m(2,0) = 3.f;	m(2,1) = 3.f;	m(2,2) = 3.f;	m(2,3) = 3.f;
		m(3,0) = 4.f;	m(3,1) = 4.f;	m(3,2) = 4.f;	m(3,3) = 4.f;

		Matrix identity, result;
		
		identity.loadIdentity();
		result.multiply(m, identity);

		if (result != m)
			puts("Test #3: Matrix multiplication operator FAILED");
		else
			puts("Test #3: Matrix multiplication operator PASSED");
	}

	// Test #4: Matrix transpose.
	{
		Matrix m; // original matrix
		m(0,0) = 1.f;	m(0,1) = 1.f;	m(0,2) = 1.f;	m(0,3) = 1.f;
		m(1,0) = 2.f;	m(1,1) = 2.f;	m(1,2) = 2.f;	m(1,3) = 2.f;
		m(2,0) = 3.f;	m(2,1) = 3.f;	m(2,2) = 3.f;	m(2,3) = 3.f;
		m(3,0) = 4.f;	m(3,1) = 4.f;	m(3,2) = 4.f;	m(3,3) = 4.f;
		
		Matrix t; // the expected transpose of m
		t(0,0) = 1.f;	t(0,1) = 2.f;	t(0,2) = 3.f;	t(0,3) = 4.f;
		t(1,0) = 1.f;	t(1,1) = 2.f;	t(1,2) = 3.f;	t(1,3) = 4.f;
		t(2,0) = 1.f;	t(2,1) = 2.f;	t(2,2) = 3.f;	t(2,3) = 4.f;
		t(3,0) = 1.f;	t(3,1) = 2.f;	t(3,2) = 3.f;	t(3,3) = 4.f;

		Matrix result;
		result.transpose(m);

		if (result != t)
			puts("Test #4: Matrix transpose FAILED");
		else
			puts("Test #4: Matrix transpose PASSED");
	}

	// Test #5: Matrix inverse.
	{
		Matrix identity; // identity matrix
		identity.loadIdentity();

		Matrix m; // original matrix.
		m(0,0) = 1.f;	m(0,1) = -3.f;	m(0,2) = 0.f;	m(0,3) = -2.f;
		m(1,0) = 3.f;	m(1,1) = -12.f;	m(1,2) = -2.f;	m(1,3) = -6.f;
		m(2,0) = -2.f;	m(2,1) = 10.f;	m(2,2) = 2.f;	m(2,3) = 5.f;
		m(3,0) = -1.f;	m(3,1) = 6.f;	m(3,2) = 1.f;	m(3,3) = 3.f;

		Matrix i; // the expected inverse of m
		i(0,0) = 0.f;	i(0,1) = 1.f;	i(0,2) = 0.f;	i(0,3) = 2.f;
		i(1,0) = 1.f;	i(1,1) = -1.f;	i(1,2) = -2.f;	i(1,3) = 2.f;
		i(2,0) = 0.f;	i(2,1) = 1.f;	i(2,2) = 3.f;	i(2,3) = -3.f;
		i(3,0) = -2.f;	i(3,1) = 2.f;	i(3,2) = 3.f;	i(3,3) = -2.f;

		Matrix inverse;
		inverse.inverse(m);
		
		if (inverse == i)
		{
			inverse.multiply(m);

			if (inverse == identity)
				puts("Test #5: Matrix inverse PASSED");
			else
				puts("Test #5: Matrix inverse FAILED");
		}
		else
		{
			puts("Test #5: Matrix inverse FAILED");
		}
	}

	// Test #6: Matrix addition.
	{
		Matrix m1, m2;
		m1.loadIdentity();
		m2.loadIdentity();
		
		Matrix m3; // expected result
		m3(0,0) = 2.f;	m3(0,1) = 0.f;	m3(0,2) = 0.f;	m3(0,3) = 0.f;
		m3(1,0) = 0.f;	m3(1,1) = 2.f;	m3(1,2) = 0.f;	m3(1,3) = 0.f;
		m3(2,0) = 0.f;	m3(2,1) = 0.f;	m3(2,2) = 2.f;	m3(2,3) = 0.f;
		m3(3,0) = 0.f;	m3(3,1) = 0.f;	m3(3,2) = 0.f;	m3(3,3) = 2.f;

		m1.add(m2);

		if (m1 != m3)
			puts("Test #6: Matrix addition FAILED");
		else
			puts("Test #6: Matrix addition PASSED");
	}

	// Test #7: Matrix subtraction.
	{
		Matrix m1, m2; 
		m1.loadIdentity();
		m2.loadIdentity();

		Matrix m3; // expected result
		m3(0,0) = 0.f;	m3(0,1) = 0.f;	m3(0,2) = 0.f;	m3(0,3) = 0.f;
		m3(1,0) = 0.f;	m3(1,1) = 0.f;	m3(1,2) = 0.f;	m3(1,3) = 0.f;
		m3(2,0) = 0.f;	m3(2,1) = 0.f;	m3(2,2) = 0.f;	m3(2,3) = 0.f;
		m3(3,0) = 0.f;	m3(3,1) = 0.f;	m3(3,2) = 0.f;	m3(3,3) = 0.f;

		m1.subtract(m2);

		if (m1 != m3)
			puts("Test #7: Matrix subtraction FAILED");
		else
			puts("Test #7: Matrix subtraction PASSED");
	}

	// Test #8: Matrix multiplication by scalar.
	{
		Matrix m1; // original matrix
		m1(0,0) = 1.f;	m1(0,1) = 1.f;	m1(0,2) = 1.f;	m1(0,3) = 1.f;
		m1(1,0) = 1.f;	m1(1,1) = 1.f;	m1(1,2) = 1.f;	m1(1,3) = 1.f;
		m1(2,0) = 1.f;	m1(2,1) = 1.f;	m1(2,2) = 1.f;	m1(2,3) = 1.f;
		m1(3,0) = 1.f;	m1(3,1) = 1.f;	m1(3,2) = 1.f;	m1(3,3) = 1.f;

		Matrix m2; // expected result
		m2(0,0) = 3.f;	m2(0,1) = 3.f;	m2(0,2) = 3.f;	m2(0,3) = 3.f;
		m2(1,0) = 3.f;	m2(1,1) = 3.f;	m2(1,2) = 3.f;	m2(1,3) = 3.f;
		m2(2,0) = 3.f;	m2(2,1) = 3.f;	m2(2,2) = 3.f;	m2(2,3) = 3.f;
		m2(3,0) = 3.f;	m2(3,1) = 3.f;	m2(3,2) = 3.f;	m2(3,3) = 3.f;

		m1.multiply(3.f);

		if (m1 == m2)
			puts("Test #8: Matrix multiplication by scalar PASSED");
		else
			puts("Test #8: Matrix multiplication by scalar FAILED");
	}

	// Test #9: Comparing floating points for equality.
	{
		float tenth = 1.f / 10.f;

		if (tenth * 10.f != 1.f)
		{
			if (CloseEnough(tenth * 10.f, 1.f))
				puts("Test #9: (1.f / 10.f) * 10.f == 1.f  PASSED");
		}
		else
		{
			puts("Test #9: (1.f / 10.f) * 10.f == 1.f  FAILED");
		}
	}

	// Test #10: Rotation of vector FROM into vector TO.
	{
		puts("Test #10: Build matrix to rotate from one vector to another");

		Vector temp;
		Vector from;
		Vector to;
		Matrix m;

		// Case 1: from = (1,0,0), to = (1,0,0)
		from.set(1.f, 0.f, 0.f);
		to = from;
		
		m.rotate(from, to);
		temp.multiply(from, m);

		if (temp == to)
			puts("\tRotate(from, to) Case 1 PASSED");
		else
			puts("\tRotate(from, to) Case 1 FAILED");

		// Case 2: from = (1,0,0), to = (-1,0,0)
		from.set(1.f, 0.f, 0.f);
		to.set(-1.f, 0.f, 0.f);
		
		m.rotate(from, to);
		temp.multiply(from, m);
		
		if (temp == to)
			puts("\tRotate(from, to) Case 2 PASSED");
		else
			puts("\tRotate(from, to) Case 2 FAILED");

		// Case 3: from = (1,0,0), to = (0,1,0)
		from.set(1.f, 0.f, 0.f);
		to.set(0.f, 1.f, 0.f);
		
		m.rotate(from, to);
		temp.multiply(from, m);
		
		if (temp == to)
			puts("\tRotate(from, to) Case 3 PASSED");
		else
			puts("\tRotate(from, to) Case 3 FAILED");
	}

	// Test #11: Build translation matrix.
	{
		Point p(0.f, 0.f, 0.f);
		Point expected(7.f, 7.f, 7.f);

		Matrix m;
		m.translate(7.f, 7.f, 7.f);

		Point result;
		result.multiply(p, m);

		if (result == expected)
			puts("Test #11: Build translation matrix PASSED");
		else
			puts("Test #11: Build translation matrix FAILED");
	}

	// Test #12: Build scaling matrix.
	{
		Point p(1.f, 1.f, 1.f);
		Point expected(7.f, 7.f, 7.f);

		Matrix m;
		m.scale(7.f, 7.f, 7.f);

		Point result;
		result.multiply(p, m);

		if (result == expected)
			puts("Test #12: Build scaling matrix PASSED");
		else
			puts("Test #12: Build scaling matrix FAILED");
	}

	// Test #13: Build rotation matrix.
	{
		Point p(1.f, 0.f, 0.f);
		Point expected(0.f, 1.f, 0.f);

		Matrix m;
		Vector zAxis(0.f, 0.f, 1.f); // rotate about z-axis
		m.rotate(90.f, zAxis);

		Point result;
		result.multiply(p, m);

		if (result == expected)
			puts("Test #13: Build rotation matrix PASSED");
		else
			puts("Test #13: Build rotation matrix FAILED");
	}
}

void TestPoint()
{
	/*
	Expected Output for TestPoint():
	----------------------------------------------
	Testing Point portion of Maths Library...
	Test #1: Translate Point PASSED
	Test #2: Scale Point PASSED
	Test #3: Rotate Point PASSED
	*/

	puts("\nTesting Point portion of Maths Library...");

	// Test #1: Translate Point.
	{
		Point p(0.f, 0.f, 0.f);
		Point expected(7.f, 7.f, 7.f);

		Matrix m;
		m.translate(7.f, 7.f, 7.f);

		Point result;
		result.multiply(p, m);

		if (result == expected)
			puts("Test #1: Translate Point PASSED");
		else
			puts("Test #1: Translate Point FAILED");
	}

	// Test #2: Scale Point.
	{
		Point p(1.f, 1.f, 1.f);
		Point expected(7.f, 7.f, 7.f);

		Matrix m;
		m.scale(7.f, 7.f, 7.f);

		Point result;
		result.multiply(p, m);

		if (result == expected)
			puts("Test #2: Scale Point PASSED");
		else
			puts("Test #2: Scale Point FAILED");
	}

	// Test #3: Rotate Point.
	{
		Point p(1.f, 0.f, 0.f);
		Point expected(0.f, 1.f, 0.f);

		Matrix m;
		Vector zAxis(0.f, 0.f, 1.f); // rotate about z-axis
		m.rotate(90.f, zAxis);

		Point result;
		result.multiply(p, m);

		if (result == expected)
			puts("Test #3: Rotate Point PASSED");
		else
			puts("Test #3: Rotate Point FAILED");
	}
}

void TestVector()
{
	/*
	Expected Output for TestVector():
	----------------------------------------------
	*/

	puts("\nTesting Vector portion of Maths Library...");
}

void TestQuaternion()
{
	/*
	Expected Output for TestQuaternion():
	----------------------------------------------
	*/

	puts("\nTesting Quaternion portion of Maths Library...");
}

void TestPlane()
{
	/*
	Expected Output for TestPlane():
	----------------------------------------------
	Planes PASSED
	*/

	puts("\nTesting Plane portion of Maths Library...");

	Point points[] =
	{
		Point(-1.f, 1.f, 1.f),
		Point(1.f,  1.f, 1.f),
		Point(1.f, -1.f, 1.f)
	};

	Plane plane(points[0], points[1], points[2]);
	Point infront(0.f, 0.f, 0.5f);
	Point coplanar(1.f, 1.f, 1.f);
	Point behind(0.f, 0.f, 5.f);

	if (plane.distanceTo(infront) > 0.f 
		&& plane.distanceTo(coplanar) == 0.f
		&& plane.distanceTo(behind) < 0.f)
	{
		puts("Planes PASSED");
	}
	else
	{
		puts("Planes FAILED");
	}
}

int main()
{
	TestMatrix();
	TestPoint();
	TestVector();
	TestQuaternion();
	TestPlane();
	return 0;
}