/*
 * $Id$
 *
 * $Date$
 * $Revision$
 *
 * (C) 1999 by Hyperion
 * All rights reserved
 *
 * This file is part of the MiniGL library project
 * See the file Licence.txt for more details
 *
 */

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

#ifndef PI
	#ifdef M_PI
	#define PI M_PI
	#else
	#define PI 3.1415927
	#endif
#endif
static char rcsid[] = "$Id$";

INLINE void m_MatCopy(Matrix *pA, Matrix *pB);
INLINE void m_MatMultGeneral(Matrix *pA, float *pB, Matrix *pC);
INLINE void m_MatMultB0001(Matrix *pA, float *pB, Matrix *pC);
INLINE void m_MatMultA0001(Matrix *pA, float *pB, Matrix *pC);
INLINE void m_MatMultATrans(Matrix *pA, float *pB, Matrix *pC);
INLINE void m_MatMultBTrans(Matrix *pA, float *pB, Matrix *pC);
INLINE void m_LoadMatrixf(Matrix *pA, const float *v);
INLINE void m_LoadMatrixd(Matrix *pA, const double *v);
INLINE void m_LoadIdentity(Matrix *pA);
INLINE void m_Mult(Matrix *pA, float *v, int vtype, Matrix *pC);
void m_CombineMatrices(GLcontext context);
void GLMatrixInit(GLcontext context);


/* I am too lazy to convert our old code */
#define CM_0 OF_11
#define CM_1 OF_12
#define CM_2 OF_13
#define CM_3 OF_21
#define CM_4 OF_22
#define CM_5 OF_23
#define CM_6 OF_31
#define CM_7 OF_32
#define CM_8 OF_33

/*
** Computes the determinant of the upper 3x3 submatrix block
** of the supplied matrix
*/
INLINE GLfloat m_Determinant(Matrix *pA)
{
	#define a(x) (pA->v[CM_##x])
	 return ( a(0)*a(4)*a(8)  +  a(1)*a(5)*a(6)  + a(2)*a(3)*a(7)
	   -  a(0)*a(5)*a(7)  -  a(1)*a(3)*a(8)  - a(2)*a(4)*a(6));
	#undef a
}

/*
** Builds the inverse of the upper 3x3 submatrix block of the
** supplied matrix for the backtransformation of the normals
*/
void m_DoInvert(Matrix *pA, GLfloat *out)
{
	GLfloat det = 1.f / m_Determinant(pA);

	#define a(x) (pA->v[CM_##x])

	*out++ = (GLfloat)(a(4)*a(8) - a(5)*a(7))*det;
	*out++ = (GLfloat)(a(2)*a(7) - a(1)*a(8))*det;
	*out++ = (GLfloat)(a(1)*a(5) - a(2)*a(4))*det;

	*out++ = (GLfloat)(a(5)*a(6) - a(3)*a(8))*det;
	*out++ = (GLfloat)(a(0)*a(8) - a(2)*a(6))*det;
	*out++ = (GLfloat)(a(2)*a(3) - a(0)*a(5))*det;
	
	*out++ = (GLfloat)(a(3)*a(7) - a(4)*a(6))*det;
	*out++ = (GLfloat)(a(1)*a(6) - a(0)*a(7))*det;
	*out++ = (GLfloat)(a(0)*a(4) - a(1)*a(3))*det;

	#undef a
}

void m_BuildInverted(GLcontext context)
{
	context->InvRotValid = GL_TRUE;
	m_DoInvert(CurrentMV, context->InvRot);
}

/*
** Copy matrix B to matrix A
** A = B
*/
INLINE void m_MatCopy(Matrix *pA, Matrix *pB)
{
	int i;

	for (i=0; i<16; i++) pA->v[i] = pB->v[i];
	pA->flags = pB->flags;
	pA->Inverse = pB->Inverse;
}

/*
** General case: Matrix multiply
** Multiply matrix A by float field, storing the result in matrix C
*/
INLINE void m_MatMultGeneral(Matrix *pA, float *pB, Matrix *pC)
{
	#define a(x) (pA->v[OF_##x])
	#define b(x) (pB[OF_##x])
	#define c(x) (pC->v[OF_##x])

	c(11) = a(11)*b(11) + a(12)*b(21) +
			a(13)*b(31) + a(14)*b(41);

	c(12) = a(11)*b(12) + a(12)*b(22) +
			a(13)*b(32) + a(14)*b(42);

	c(13) = a(11)*b(13) + a(12)*b(23) +
			a(13)*b(33) + a(14)*b(43);

	c(14) = a(11)*b(14) + a(12)*b(24) +
			a(13)*b(34) + a(14)*b(44);

	c(21) = a(21)*b(11) + a(22)*b(21) +
			a(23)*b(31) + a(24)*b(41);

	c(22) = a(21)*b(12) + a(22)*b(22) +
			a(23)*b(32) + a(24)*b(42);

	c(23) = a(21)*b(13) + a(22)*b(23) +
			a(23)*b(33) + a(24)*b(43);

	c(24) = a(21)*b(14) + a(22)*b(24) +
			a(23)*b(34) + a(24)*b(44);

	c(31) = a(31)*b(11) + a(32)*b(21) +
			a(33)*b(31) + a(34)*b(41);

	c(32) = a(31)*b(12) + a(32)*b(22) +
			a(33)*b(32) + a(34)*b(42);

	c(33) = a(31)*b(13) + a(32)*b(23) +
			a(33)*b(33) + a(34)*b(43);

	c(34) = a(31)*b(14) + a(32)*b(24) +
			a(33)*b(34) + a(34)*b(44);

	c(41) = a(41)*b(11) + a(42)*b(21) +
			a(43)*b(31) + a(44)*b(41);

	c(42) = a(41)*b(12) + a(42)*b(22) +
			a(43)*b(32) + a(44)*b(42);

	c(43) = a(41)*b(13) + a(42)*b(23) +
			a(43)*b(33) + a(44)*b(43);

	c(44) = a(41)*b(14) + a(42)*b(24) +
			a(43)*b(34) + a(44)*b(44);

	#undef a
	#undef b
}

/*
** Matrix B has three rows
** Multiply matrix A by float field, storing the result in matrix C
*/
INLINE void m_MatMultB0001(Matrix *pA, float *pB, Matrix *pC)
{
	#define a(x) (pA->v[OF_##x])
	#define b(x) (pB[OF_##x])
	#define c(x) (pC->v[OF_##x])

	c(11) = a(11)*b(11) + a(12)*b(21) +
			a(13)*b(31);

	c(12) = a(11)*b(12) + a(12)*b(22) +
			a(13)*b(32);

	c(13) = a(11)*b(13) + a(12)*b(23) +
			a(13)*b(33);

	c(14) = a(11)*b(14) + a(12)*b(24) +
			a(13)*b(34) + a(14);

	c(21) = a(21)*b(11) + a(22)*b(21) +
			a(23)*b(31);

	c(22) = a(21)*b(12) + a(22)*b(22) +
			a(23)*b(32);

	c(23) = a(21)*b(13) + a(22)*b(23) +
			a(23)*b(33);

	c(24) = a(21)*b(14) + a(22)*b(24) +
			a(23)*b(34) + a(24);

	c(31) = a(31)*b(11) + a(32)*b(21) +
			a(33)*b(31);

	c(32) = a(31)*b(12) + a(32)*b(22) +
			a(33)*b(32);

	c(33) = a(31)*b(13) + a(32)*b(23) +
			a(33)*b(33);

	c(34) = a(31)*b(14) + a(32)*b(24) +
			a(33)*b(34) + a(34);

	c(41) = a(41)*b(11) + a(42)*b(21) +
			a(43)*b(31);

	c(42) = a(41)*b(12) + a(42)*b(22) +
			a(43)*b(32);

	c(43) = a(41)*b(13) + a(42)*b(23) +
			a(43)*b(33);

	c(44) = a(41)*b(14) + a(42)*b(24) +
			a(43)*b(34) + a(44);

	#undef a
	#undef b
}


/*
** Matrix A has three rows
** Multiply matrix A by float field, storing the result in matrix C
*/
INLINE void m_MatMultA0001(Matrix *pA, float *pB, Matrix *pC)
{
	#define a(x) (pA->v[OF_##x])
	#define b(x) (pB[OF_##x])
	#define c(x) (pC->v[OF_##x])

	c(11) = a(11)*b(11) + a(12)*b(21) +
			a(13)*b(31) + a(14)*b(41);

	c(12) = a(11)*b(12) + a(12)*b(22) +
			a(13)*b(32) + a(14)*b(42);

	c(13) = a(11)*b(13) + a(12)*b(23) +
			a(13)*b(33) + a(14)*b(43);

	c(14) = a(11)*b(14) + a(12)*b(24) +
			a(13)*b(34) + a(14)*b(44);

	c(21) = a(21)*b(11) + a(22)*b(21) +
			a(23)*b(31) + a(24)*b(41);

	c(22) = a(21)*b(12) + a(22)*b(22) +
			a(23)*b(32) + a(24)*b(42);

	c(23) = a(21)*b(13) + a(22)*b(23) +
			a(23)*b(33) + a(24)*b(43);

	c(24) = a(21)*b(14) + a(22)*b(24) +
			a(23)*b(34) + a(24)*b(44);

	c(31) = a(31)*b(11) + a(32)*b(21) +
			a(33)*b(31) + a(34)*b(41);

	c(32) = a(31)*b(12) + a(32)*b(22) +
			a(33)*b(32) + a(34)*b(42);

	c(33) = a(31)*b(13) + a(32)*b(23) +
			a(33)*b(33) + a(34)*b(43);

	c(34) = a(31)*b(14) + a(32)*b(24) +
			a(33)*b(34) + a(34)*b(44);

	c(41) = b(41);

	c(42) = b(42);

	c(43) = b(43);

	c(44) = b(44);

	#undef a
	#undef b
}

/*
** Matrix A is a translation matrix
** Multiply matrix A by float field, storing the result in matrix C
*/
INLINE void m_MatMultATrans(Matrix *pA, float *pB, Matrix *pC)
{
	#define a(x) (pA->v[OF_##x])
	#define b(x) (pB[OF_##x])
	#define c(x) (pC->v[OF_##x])

	c(11) = b(11) + a(14)*b(41);

	c(12) = b(12) + a(14)*b(42);

	c(13) = b(13) + a(14)*b(43);

	c(14) = b(14) + a(14)*b(44);

	c(21) = b(21) + a(24)*b(41);

	c(22) = b(22) + a(24)*b(42);

	c(23) = b(23) + a(24)*b(43);

	c(24) = b(24) + a(24)*b(44);

	c(31) = b(31) + a(34)*b(41);

	c(32) = b(32) + a(34)*b(42);

	c(33) = b(33) + a(34)*b(43);

	c(34) = b(34) + a(34)*b(44);

	c(41) = b(41);

	c(42) = b(42);

	c(43) = b(43);

	c(44) = b(44);

	#undef a
	#undef b
}

/*
** Matrix B is a translation matrix
** Multiply matrix A by float field, storing the result in matrix C
*/
INLINE void m_MatMultBTrans(Matrix *pA, float *pB, Matrix *pC)
{
	#define a(x) (pA->v[OF_##x])
	#define b(x) (pB[OF_##x])
	#define c(x) (pC->v[OF_##x])

	c(11) = a(11);

	c(12) = a(12);

	c(13) = a(13);

	c(14) = a(14)*b(14) + a(12)*b(24) +
			a(13)*b(34) + a(14);

	c(21) = a(21)*b(11) + a(22)*b(21) +
			a(23)*b(31) + a(24)*b(41);

	c(22) = a(22);

	c(23) = a(22);

	c(24) = a(21)*b(14) + a(22)*b(24) +
			a(23)*b(34) + a(24);

	c(31) = a(31);

	c(32) = a(32);

	c(33) = a(33);

	c(34) = a(31)*b(14) + a(32)*b(24) +
			a(33)*b(34) + a(34);

	c(41) = a(41);

	c(42) = a(42);

	c(43) = a(43);

	c(44) = a(41)*b(14) + a(42)*b(24) +
			a(43)*b(34) + a(44);

	#undef a
	#undef b
}


INLINE void m_LoadMatrixf(Matrix *pA, const float *v)
{
	#define a(x) pA->v[OF_##x] = *v++

	a(11); a(21); a(31); a(41);
	a(12); a(22); a(32); a(42);
	a(13); a(23); a(33); a(43);
	a(14); a(24); a(34); a(44);

	if (*(v-4) == 0.f && *(v-3) == 0.f && *(v-2) == 0.f && *(v-1) == 1.f)
		pA->flags = MGLMAT_0001;

	#undef a
}

INLINE void m_LoadMatrixd(Matrix *pA, const double *v)
{
	#define a(x) pA->v[OF_##x] = (float)*v++

	a(11); a(21); a(31); a(41);
	a(12); a(22); a(32); a(42);
	a(13); a(23); a(33); a(43);
	a(14); a(24); a(34); a(44);

	if (*(v-4) == 0.0 && *(v-3) == 0.0 && *(v-2) == 0.0 && *(v-1) == 1.0)
		pA->flags = MGLMAT_0001;

	#undef a
}


INLINE void m_LoadIdentity(Matrix *pA)
{
	#define a(x) pA->v[OF_##x] = 0.f;
	#define b(x) pA->v[OF_##x] = 1.f;

	b(11); a(21); a(31); a(41);
	a(12); b(22); a(32); a(42);
	a(13); a(23); b(33); a(43);
	a(14); a(24); a(34); b(44);

	pA->flags = MGLMAT_IDENTITY;

	#undef a
	#undef b
}

/*
** Multiply matrix A by the matrix passed by v.
** The vtype specifies what kind of matrix v points to, so that
** this routine may select an optimized matrix multiplication routine.
*/
INLINE void m_Mult(Matrix *pA, float *v, int vtype, Matrix *pC)
{
#if 0
	if ((pA->flags & vtype) == 0)
	{
		m_MatMultGeneral(pA, v, pC);
	}
	else if (vtype == MGLMAT_TRANSLATION)
	{
		m_MatMultBTrans(pA, v, pC);
	}
	else if (vtype == MGLMAT_ROTATION || vtype == MGLMAT_0001)
	{
		m_MatMultB0001(pA, v, pC);
	}
	else if (pA->flags == MGLMAT_TRANSLATION)
	{
		m_MatMultATrans(pA, v, pC);
	}
	else if (pA->flags == MGLMAT_0001)
	{
		m_MatMultA0001(pA, v, pC);
	}
	else
	{
		m_MatMultGeneral(pA, v, pC);
	}

	pC->flags = 0;

	if (vtype & MGLMASK_0001)
	{
		if (pA->flags & MGLMASK_0001)
		{
			pC->flags = MGLMAT_0001;
		}
	}
#else
	//FIXME: Put those special case mults back.
	m_MatMultGeneral(pA, v, pC);
#endif
}

void m_CombineMatrices(GLcontext context)
{
	m_Mult(CurrentP,
	CurrentMV->v,
	CurrentMV->flags, &(context->CombinedMatrix));
	context->CombinedValid = GL_TRUE;
}

void m_PrintMatrix(Matrix *pA)
{
	#define a(x) (pA->v[OF_##x])
	mykprintf("Matrix at 0x%lX\n", (ULONG)pA);
	mykprintf("    | %6.3f %6.3f %6.3f %6.3f |\n",
		a(11), a(12), a(13), a(14));
	mykprintf("    | %6.3f %6.3f %6.3f %6.3f |\n",
		a(21), a(22), a(23), a(24));
	mykprintf("A = | %6.3f %6.3f %6.3f %6.3f |\n",
		a(31), a(32), a(33), a(34));
	mykprintf("    | %6.3f %6.3f %6.3f %6.3f |\n",
		a(41), a(42), a(43), a(44));
	#undef a
}

// Interface functions
void GLMatrixMode(GLcontext context, GLenum mode)
{
	//LOG(3, glMatrixMode, "%d", mode);
	GLASSERT(mode == GL_PROJECTION || mode == GL_MODELVIEW);
	GLASSERT(context != NULL);

	context->CurrentMatrixMode = mode;
}

void GLLoadIdentity(GLcontext context)
{
	//LOG(3, glLoadIdentity, "");
	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	if (context->CurrentMatrixMode == GL_MODELVIEW)
	{
		m_LoadIdentity(CurrentMV);
	}
	else
	{
		m_LoadIdentity(CurrentP);
	}
	context->CombinedValid = GL_FALSE;
	context->InvRotValid = GL_FALSE;
}

void MGLPrintMatrix(GLcontext context, int mode)
{
	if (mode == GL_MODELVIEW)
	{
		m_PrintMatrix(CurrentMV);
	}
	else
	{
		m_PrintMatrix(CurrentP);
	}
	mykprintf("\n");
}

void MGLPrintMatrixStack(GLcontext context, int mode)
{
	int i;

	mykprintf("Stack Top:\n");
	MGLPrintMatrix(context, mode);
	mykprintf("Rest of stack:\n");

	if (mode == GL_MODELVIEW) {
		if (context->ModelViewStackPointer == 0) {
			mykprintf("Empty\n\n\n");
			return;
		}
		for (i=context->ModelViewStackPointer-1; i>=0; i--)
		{
			mykprintf("%d:\n", i);
			m_PrintMatrix(&(context->ModelViewStack[i]));
		}
	}
	else
	{
		if (context->ProjectionStackPointer == 0) {
			mykprintf("Empty\n\n\n");
			return;
		}
		for (i=context->ProjectionStackPointer-1; i>=0; i--)
		{
			mykprintf("%d:\n", i);
			m_PrintMatrix(&(context->ProjectionStack[i]));
		}
	}
	mykprintf("\n\n");
}


void GLLoadMatrixf(GLcontext context, const GLfloat *m)
{
	//LOG(3, glLoadMatrixf, "%f %f %f ...", *m, *(m+1), *(m+2));
	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	context->CombinedValid = GL_FALSE;
	context->InvRotValid = GL_FALSE;
	m_LoadMatrixf(CMATRIX(context), m);
}

void GLLoadMatrixd(GLcontext context, const GLdouble *m)
{
	//LOG(3, glLoadMatrixf, "%lf %lf %lf ...", *m, *(m+1), *(m+2));
	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	context->CombinedValid = GL_FALSE;
	context->InvRotValid = GL_FALSE;
	m_LoadMatrixd(CMATRIX(context), m);
}

void GLPushMatrix(GLcontext context)
{
	//LOG(3, glPushMatrix, "");
	GLASSERT(context != NULL);
	GLASSERT(context->ProjectionStackPointer <= PROJECTION_STACK_SIZE);
	GLASSERT(context->ModelViewStackPointer  <= MODELVIEW_STACK_SIZE);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);

	if (context->CurrentMatrixMode == GL_PROJECTION)
	{
		m_MatCopy(&(context->ProjectionStack[context->ProjectionStackPointer]),
			CurrentP);
		context->ProjectionStackPointer ++;
	}
	else
	{
		m_MatCopy(&(context->ModelViewStack[context->ModelViewStackPointer]),
			CurrentMV);
		context->ModelViewStackPointer ++;
	}
}

void GLPopMatrix(GLcontext context)
{
	//LOG(3, glPopMatrix, "");
	GLASSERT(context != NULL);
	GLASSERT(context->ProjectionStackPointer >= 0);
	GLASSERT(context->ModelViewStackPointer  >= 0);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	context->InvRotValid = GL_FALSE;
	context->CombinedValid = GL_FALSE;

	if (context->CurrentMatrixMode == GL_PROJECTION)
	{
		if (context->ProjectionStackPointer <= 0)
		{
			context->CurrentError = GL_INVALID_OPERATION;
			return;
		}
		context->ProjectionStackPointer --;
		m_MatCopy(CurrentP,
			&(context->ProjectionStack[context->ProjectionStackPointer]));
	}
	else
	{
		if (context->ModelViewStackPointer <= 0)
		{
			context->CurrentError = GL_INVALID_OPERATION;
			return;
		}
		context->ModelViewStackPointer --;
		m_MatCopy(CurrentMV,
			&(context->ModelViewStack[context->ModelViewStackPointer]));
	}

}

void GLFrustum(GLcontext context, GLdouble left, GLdouble right, GLdouble bottom, GLdouble top, GLdouble zNear, GLdouble zFar)
{
	float v[16];
	float n2 = 2.0*zNear;
	float rli = 1.f / (float)(right-left);
	float tbi = 1.f / (float)(top-bottom);
	float fni = 1.f / (float)(zFar-zNear);

	//LOG(3, glFrustum, "%lf &lf %lf %lf %lf %lf", left, right, bottom, top, zNear, zFar);

	GLASSERT(context != NULL);
	GLFlagError(context, zFar<=0.0 || zNear <=0.0, GL_INVALID_VALUE);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	context->InvRotValid = GL_FALSE;
	context->CombinedValid = GL_FALSE;

	v[OF_11] = n2*rli; v[OF_21] = 0.0; v[OF_31] = 0.0; v[OF_41] = 0.0;

	v[OF_22] = n2*tbi; v[OF_12] = 0.0; v[OF_32] = 0.0; v[OF_42] = 0.0;

	v[OF_13] = (right+left)*rli; v[OF_23] = (top+bottom)*tbi;
	v[OF_33] = -(zFar+zNear)*fni; v[OF_43] = -1.0;

	v[OF_14] = 0.0; v[OF_24] = 0.0;
	v[OF_34] = -(2.0*zFar*zNear)*fni; v[OF_44] = 0.0;
	m_Mult(CMATRIX(context),
		v,
		MGLMAT_PERSPECTIVE,
		OMATRIX(context)
		);
	SMATRIX(context);

}

void GLOrtho(GLcontext context, GLdouble left, GLdouble right, GLdouble bottom, GLdouble top, GLdouble zNear, GLdouble zFar)
{
	float rli = 1.0  / (right-left);
	float tbi = 1.0  / (top-bottom);
	float fni = 1.0 / (zFar-zNear);
	float v[16];

	//LOG(3, glOrtho, "%lf %lf %lf %lf %lf %lf", left, right, bottom, top, zNear, zFar);

	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	context->InvRotValid = GL_FALSE;
	context->CombinedValid = GL_FALSE;

	v[OF_11] = 2.0*rli; v[OF_12] = 0.0; v[OF_13] = 0.0;
	v[OF_14] = -(right+left)*rli;

	v[OF_21] = 0.0; v[OF_22] = 2.0*tbi;
	v[OF_23] = 0.0; v[OF_24] = -(top+bottom)*tbi;

	v[OF_31] = 0.0; v[OF_32] = 0.0;
	v[OF_33] = -2.0*fni; v[OF_34] = -(zFar+zNear)*fni;

	v[OF_41] = v[OF_42] = v[OF_43] = 0.0;
	v[OF_44] = 1.0;

	m_Mult(CMATRIX(context), v, MGLMAT_ORTHO, OMATRIX(context));
	SMATRIX(context);
}

void GLMultMatrixf(GLcontext context, const GLfloat *mat)
{
	//LOG(3, glMultMatrixf, "%f %f %f ...", *mat, *(mat+1), *(mat+2));
	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	context->InvRotValid = GL_FALSE;
	context->CombinedValid = GL_FALSE;

	m_Mult(CMATRIX(context), (float *)mat, 0, OMATRIX(context));
	SMATRIX(context);
}

void GLMultMatrixd(GLcontext context, const GLdouble *mat)
{
	GLfloat v[16];
	int i;
	//LOG(3, glMultMatrixd, "%lf %lf %lf ...", *mat, *(mat+1), *(mat+2));

	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	context->InvRotValid = GL_FALSE;
	context->CombinedValid = GL_FALSE;

	for (i=0; i<16; i++)
	{
		v[i] = (GLfloat) *mat++;
	}

	m_Mult(CMATRIX(context), v, 0, OMATRIX(context));
	SMATRIX(context);
}

/*
** The following routine was ripped from the Mesa source code.
** I did use this because it is much better than my original design. The code
** is just modified a bit to adapt to my code layout, but otherwise is unmodified.
** This code is included with the kind permission of Brian Paul, the author of Mesa.
*/
void GLRotatef(GLcontext context, GLfloat angle, GLfloat x, GLfloat y, GLfloat z)
{
	float v[16];
	#define v(x) v[OF_##x]
	float mag, s, c;
	float xx,yy,zz,xy,yz,zx,xs,ys,zs,one_c;

	//LOG(3, glRotatef, "%f %f %f %f", angle, x,y,z);

	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	context->InvRotValid = GL_FALSE;
	context->CombinedValid = GL_FALSE;


	s = (float)sin(angle * PI/180.0);
	c = (float)cos(angle * PI/180.0);
	mag = sqrt(x*x+y*y+z*z);

	if (mag == 0.0) return;

	if (mag != 1.0)
	{
		x /= mag;
		y /= mag;
		z /= mag;
	}

	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;
	one_c = 1.f - c;

	v(11) = one_c*xx + c;
	v(12) = one_c*xy - zs;
	v(13) = one_c*zx + ys;
	v(14) = 0.f;

	v(21) = one_c*xy + zs;
	v(22) = one_c*yy + c;
	v(23) = one_c*yz - xs;
	v(24) = 0.f;

	v(31) = one_c*zx - ys;
	v(32) = one_c*yz + xs;
	v(33) = one_c*zz + c;
	v(34) = 0.f;

	v(41) = 0.f;
	v(42) = 0.f;
	v(43) = 0.f;
	v(44) = 1.f;

	m_Mult(CMATRIX(context), v, MGLMAT_ROTATION, OMATRIX(context));
	SMATRIX(context);
	#undef v
}

void GLRotated(GLcontext context, GLdouble angle, GLdouble x, GLdouble y, GLdouble z)
{
	GLRotatef(context, (GLfloat)angle, (GLfloat)x, (GLfloat)y, (GLfloat)z);
}

void GLScaled(GLcontext context, GLdouble x, GLdouble y, GLdouble z)
{
	float v[16];
	//LOG(3, glScaled, "%lf %lf %lf", x,y,z);
	GLASSERT(context!=NULL);
	context->InvRotValid = GL_FALSE;
	context->CombinedValid = GL_FALSE;
	#define v(x) v[OF_##x]
	v(11) = (float)x;   v(12) = 0.0; v(13) = 0.0; v(14) = 0.0;
	v(21) = 0.0; v(22) = (float)y;   v(23) = 0.0; v(24) = 0.0;
	v(31) = 0.0; v(32) = 0.0; v(33) = (float)z;   v(34) = 0.0;
	v(41) = 0.0; v(42) = 0.0; v(43) = 0.0; v(44) = 1.0;

	m_Mult(CMATRIX(context), v, MGLMAT_GENERAL_SCALE, OMATRIX(context));
	SMATRIX(context);
	#undef v
}

void GLScalef(GLcontext context, GLfloat x, GLfloat y, GLfloat z)
{
	float v[16];
	//LOG(3, glScalef, "%f %f %f", x,y,z);
	GLASSERT(context!=NULL);
	context->InvRotValid = GL_FALSE;
	context->CombinedValid = GL_FALSE;
	#define v(x) v[OF_##x]
	v(11) = x;   v(12) = 0.0; v(13) = 0.0; v(14) = 0.0;
	v(21) = 0.0; v(22) = y;   v(23) = 0.0; v(24) = 0.0;
	v(31) = 0.0; v(32) = 0.0; v(33) = z;   v(34) = 0.0;
	v(41) = 0.0; v(42) = 0.0; v(43) = 0.0; v(44) = 1.0;
	m_Mult(CMATRIX(context), v, MGLMAT_GENERAL_SCALE, OMATRIX(context));
	SMATRIX(context);
	#undef v
}

void GLTranslated(GLcontext context, GLdouble x, GLdouble y, GLdouble z)
{
	float v[16];

	//LOG(3, glTranslated, "%lf %lf %lf", x,y,z);

	GLASSERT(context != NULL);
	context->InvRotValid = GL_FALSE;
	context->CombinedValid = GL_FALSE;

	#define v(x) v[OF_##x]
	v(11) = v(22) = v(33) = v(44) = 1.f;
	v(21) = v(31) = v(41) = v(12) = v(32) = v(42) = v(13) = v(23) = v(43) = 0.f;
	v(14) = (float)x; v(24) = (float)y; v(34) = (float)z;

	m_Mult(CMATRIX(context), v, MGLMAT_TRANSLATION, OMATRIX(context));
	SMATRIX(context);
	#undef v
}

void GLTranslatef(GLcontext context, GLfloat x, GLfloat y, GLfloat z)
{
	float vv[16];
	//LOG(3, glTranslatef, "%f %f %f", x,y,z);

	GLASSERT(context != NULL);
	context->InvRotValid = GL_FALSE;
	context->CombinedValid = GL_FALSE;

	#define v(x) vv[OF_##x]
	v(11) = v(22) = v(33) = v(44) = 1.f;
	v(21) = v(31) = v(41) = v(12) = v(32) = v(42) = v(13) = v(23) = v(43) = 0.f;
	v(14) = (float)x; v(24) = (float)y; v(34) = (float)z;
	#undef v
	m_Mult(CMATRIX(context), vv, MGLMAT_TRANSLATION, OMATRIX(context));
	SMATRIX(context);
}

void GLMatrixInit(GLcontext context)
{
	context->ModelViewStackPointer = 0;
	context->ProjectionStackPointer = 0;
	context->ModelViewNr = 0;
	context->ProjectionNr = 0;
	context->CombinedValid = GL_FALSE;
	context->InvRotValid = GL_FALSE;
	GLMatrixMode(context, GL_PROJECTION);
	GLLoadIdentity(context);
	GLMatrixMode(context, GL_MODELVIEW);
	GLLoadIdentity(context);

}