/************************************************************************/
/* CaveUTGL                                                             */
/* Willem de Jonge (willem@argcargv.com)                                */
/* 2003                                                                 */
/************************************************************************/

#include "matrixstack.h"
#include <math.h>

const GLfloat deg2rad = 3.1415926f/180.0f;
static GLfloat id_arr[] = { 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1 };
Matrix4 Matrix4::Identity(id_arr);

Matrix4 Matrix4::operator*(const Matrix4& B) const
{
    Matrix4 C;
    C.a00 = a00 * B.a00 + a01 * B.a10 + a02 * B.a20 + a03 * B.a30;
    C.a10 = a10 * B.a00 + a11 * B.a10 + a12 * B.a20 + a13 * B.a30;
    C.a20 = a20 * B.a00 + a21 * B.a10 + a22 * B.a20 + a23 * B.a30;
    C.a30 = a30 * B.a00 + a31 * B.a10 + a32 * B.a20 + a33 * B.a30;
    C.a01 = a00 * B.a01 + a01 * B.a11 + a02 * B.a21 + a03 * B.a31;
    C.a11 = a10 * B.a01 + a11 * B.a11 + a12 * B.a21 + a13 * B.a31;
    C.a21 = a20 * B.a01 + a21 * B.a11 + a22 * B.a21 + a23 * B.a31;
    C.a31 = a30 * B.a01 + a31 * B.a11 + a32 * B.a21 + a33 * B.a31;
    C.a02 = a00 * B.a02 + a01 * B.a12 + a02 * B.a22 + a03 * B.a32;
    C.a12 = a10 * B.a02 + a11 * B.a12 + a12 * B.a22 + a13 * B.a32;
    C.a22 = a20 * B.a02 + a21 * B.a12 + a22 * B.a22 + a23 * B.a32;
    C.a32 = a30 * B.a02 + a31 * B.a12 + a32 * B.a22 + a33 * B.a32;
    C.a03 = a00 * B.a03 + a01 * B.a13 + a02 * B.a23 + a03 * B.a33;
    C.a13 = a10 * B.a03 + a11 * B.a13 + a12 * B.a23 + a13 * B.a33;
    C.a23 = a20 * B.a03 + a21 * B.a13 + a22 * B.a23 + a23 * B.a33;
    C.a33 = a30 * B.a03 + a31 * B.a13 + a32 * B.a23 + a33 * B.a33;
    return C;
}

void Matrix4::GetNearFar(GLfloat& n, GLfloat& f) const
{
#define UT2K3_PATCH
#ifdef UT2K3_PATCH
	/*  this has been "fixed" to extract far/near matching
		those UT2K3 intends to use (actually UT2K3 sends a
		perspective matrix intended for normalized device
		coordinates ranging from 0..1 in z like D3D, and not
		for the -1..1 range opengl is using)
	*/

	GLfloat a,b,c,d,mag;
	a = a20;
	b = a21;
	c = a22;
	d = a23;
	mag = sqrt(a*a + b*b + c*c);
	d /= mag;

	n = -d;

	a = a30 - a20;
	b = a31 - a21;
	c = a32 - a22;
	d = a33 - a23;
	mag = sqrt(a*a + b*b + c*c);
	d /= mag;

	f = d-n;

#else

	GLfloat a,b,c,d,mag;
	a = a30 + a20;
	b = a31 + a21;
	c = a32 + a22;
	d = a33 + a23;
	mag = sqrt(a*a + b*b + c*c);
	d /= mag;
	n = -d;

	a = a30 - a20;
	b = a31 - a21;
	c = a32 - a22;
	d = a33 - a23;
	mag = sqrt(a*a + b*b + c*c);
	d /= mag;
	f = d;

#endif
}

void Matrix4::Frustum(GLfloat fovl, GLfloat fovr, GLfloat fovb, GLfloat fovt, GLfloat n, GLfloat f, bool swapz)
{
	GLfloat l,r,b,t;
	l = n*tan( fovl * deg2rad );
	r = n*tan( fovr * deg2rad );
	b = n*tan( fovb * deg2rad );
	t = n*tan( fovt * deg2rad );

    a00 = (2.0f*n) / (r-l);
    a10 = 0.0;
    a20 = 0.0;
    a30 = 0.0;

    a01 = 0.0;
    a11 = (2.0f*n) / (t-b);
    a21 = 0.0;
    a31 = 0.0;

    a02 =  (r+l) / (r-l);
    a12 =  (t+b) / (t-b);
    a22 = -(f+n) / (f-n);
    a32 = -1.0f;

	if (swapz)
	{
		a02 *= -1;
		a12 *= -1;
		a22 *= -1;
		a32 *= -1;
	}

    a03 = 0.0;
    a13 = 0.0;
    a23 = -(2.0f*f*n) / (f-n);
    a33 = 0.0;
}

MatrixStack::MatrixStack()
{
	m_stack.push_back(Matrix4::Identity);
}

bool MatrixStack::IsPerspective()
{
	Matrix4& C = m_stack.back();
	return abs(C.a32) > 1.0e-3f;
}


const Matrix4& MatrixStack::CurrentMatrix()
{
	return m_stack.back();
}

void MatrixStack::PushMatrix()
{
	Matrix4 m = m_stack.back();
	m_stack.push_back(m);
}

void MatrixStack::PopMatrix()
{
	m_stack.pop_back();
}

void MatrixStack::LoadIdentity()
{
	m_stack.back() = Matrix4::Identity;
}

void MatrixStack::LoadMatrix(const GLfloat* m)
{
	m_stack.back() = *(Matrix4*)m;
}

void MatrixStack::LoadTransposeMatrix(const GLfloat* p)
{
	Matrix4& C = m_stack.back();
	Matrix4& M = *(Matrix4*)p;

	C.a00 = M.a00;
	C.a10 = M.a01;
	C.a20 = M.a02;
	C.a30 = M.a03;

	C.a01 = M.a10;
	C.a11 = M.a11;
	C.a21 = M.a12;
	C.a31 = M.a13;

	C.a02 = M.a20;
	C.a12 = M.a21;
	C.a22 = M.a22;
	C.a32 = M.a23;

	C.a03 = M.a30;
	C.a13 = M.a31;
	C.a23 = M.a32;
	C.a33 = M.a33;
}

void MatrixStack::MultMatrix(const GLfloat* p)
{
	Matrix4& C = m_stack.back();
	Matrix4& M = *(Matrix4*)p;
	C = C * M;
}

void MatrixStack::MultTransposeMatrix(const GLfloat* p)
{
	Matrix4& C = m_stack.back();
	Matrix4& T = *(Matrix4*)p;
	Matrix4  M;

	M.a00 = T.a00;
	M.a10 = T.a01;
	M.a20 = T.a02;
	M.a30 = T.a03;

	M.a01 = T.a10;
	M.a11 = T.a11;
	M.a21 = T.a12;
	M.a31 = T.a13;

	M.a02 = T.a20;
	M.a12 = T.a21;
	M.a22 = T.a22;
	M.a32 = T.a23;

	M.a03 = T.a30;
	M.a13 = T.a31;
	M.a23 = T.a32;
	M.a33 = T.a33;

	C = C * M;
}

void MatrixStack::Frustum(GLdouble l, GLdouble r, GLdouble b, GLdouble t, GLdouble n, GLdouble f)
{
	Matrix4& C = m_stack.back();
	Matrix4 M;
    M.a00 = static_cast<GLfloat>( (2.0f*n) / (r-l) );
    M.a10 = 0.0;
    M.a20 = 0.0;
    M.a30 = 0.0;

    M.a01 = 0.0;
    M.a11 = static_cast<GLfloat>( (2.0f*n) / (t-b) );
    M.a21 = 0.0;
    M.a31 = 0.0;

    M.a02 = static_cast<GLfloat>( (r+l) / (r-l) );
    M.a12 = static_cast<GLfloat>( (t+b) / (t-b) );
    M.a22 = static_cast<GLfloat>( -(f+n) / (f-n) );
    M.a32 = static_cast<GLfloat>( -1.0f );

    M.a03 = 0.0;
    M.a13 = 0.0;
    M.a23 = static_cast<GLfloat>( -(2.0f*f*n) / (f-n) );
    M.a33 = 0.0;

	C = C * M;
}

void MatrixStack::Ortho(GLdouble l, GLdouble r, GLdouble b, GLdouble t, GLdouble n, GLdouble f)
{
	Matrix4& C = m_stack.back();
	Matrix4 M;
    M.a00 = static_cast<GLfloat>( 2.0f / (r-l) );
    M.a10 = 0.0;
    M.a20 = 0.0;
    M.a30 = 0.0;

    M.a01 = 0.0;
    M.a11 = static_cast<GLfloat>( 2.0f / (t-b) );
    M.a21 = 0.0;
    M.a31 = 0.0;

    M.a02 = 0.0;
    M.a12 = 0.0;
    M.a22 = static_cast<GLfloat>( -2.0f / (f-n) );
    M.a32 = 0.0;

    M.a03 = static_cast<GLfloat>( -(r+l) / (r-l) );
    M.a13 = static_cast<GLfloat>( -(t+b) / (t-b) );
    M.a23 = static_cast<GLfloat>( -(f+n) / (f-n) );
    M.a33 = 1.0f;

	C = C * M;
}

void MatrixStack::Translate(GLfloat x, GLfloat y, GLfloat z)
{
	Matrix4& C = m_stack.back();
	Matrix4  M(Matrix4::Identity);

	M.a03 = x;
	M.a13 = y;
	M.a23 = z;

	C = C * M;
}

void MatrixStack::Rotate(GLfloat angle, GLfloat x, GLfloat y, GLfloat z)
{
	Matrix4& C = m_stack.back();
	Matrix4 M;
    const GLfloat mag = (GLfloat) sqrt(x * x + y * y + z * z);

	if (mag <= 1.0e-4) {
		/* no rotation, leave mat as-is */
		return;
	}

	x /= mag;
	y /= mag;
	z /= mag;

	GLfloat s = (GLfloat) sin( angle * deg2rad );
	GLfloat c = (GLfloat) cos( angle * deg2rad );

	GLfloat xx = x * x;
	GLfloat yy = y * y;
	GLfloat zz = z * z;
	GLfloat xy = x * y;
	GLfloat yz = y * z;
	GLfloat zx = z * x;
	GLfloat xs = x * s;
	GLfloat ys = y * s;
	GLfloat zs = z * s;
	GLfloat one_c = 1.0f - c;

	M.a00 = (one_c * xx) + c;
	M.a01 = (one_c * xy) - zs;
	M.a02 = (one_c * zx) + ys;
	M.a03 = 0.0F; 

	M.a10 = (one_c * xy) + zs;
	M.a11 = (one_c * yy) + c;
	M.a12 = (one_c * yz) - xs;
	M.a13 = 0.0F; 

	M.a20 = (one_c * zx) - ys;
	M.a21 = (one_c * yz) + xs;
	M.a22 = (one_c * zz) + c;
	M.a23 = 0.0F; 

	M.a30 = 0.0F;
	M.a31 = 0.0F;
	M.a32 = 0.0F;
	M.a33 = 1.0F;

	C = C * M;
}

void MatrixStack::Scale(GLfloat x, GLfloat y, GLfloat z)
{
	Matrix4& C = m_stack.back();
	Matrix4  M(Matrix4::Identity);

	M.a00 = x;
	M.a11 = y;
	M.a33 = z;

	C = C * M;
}
