/*
 * $Id: matrix.c,v 1.1.1.1 2000/04/07 19:44:51 hfrieden Exp $
 *
 * $Date: 2000/04/07 19:44:51 $
 * $Revision: 1.1.1.1 $
 *
 * (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: matrix.c,v 1.1.1.1 2000/04/07 19:44:51 hfrieden Exp $";

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);

}