/*
 * Matrix handling functions.
 *
 * Copyright (c) 1998 Criterion Software Ltd.
 *
 */

/**
 * \ingroup rwmatrix
 * \page rwmatrixoverview RwMatrix Overview
 *
 * This object defines a RenderWare Graphics Matrix object.
 *
 * The Matrix is heavily used throughout the API and a full range of
 * functions are provided, including: rotation, multiplication,
 * concatenation, scaling, translation, creation, destruction, stream
 * read/write functions and a number of access functions to access
 * particular vector data within the matrix.
 *
 * RenderWare uses 4x3, row-major affine matrices.
 */

/****************************************************************************
 Includes
 */

#include <stdlib.h>

#include "batypes.h"
#include "batype.h"
#include "balibtyp.h"
#include "badebug.h"
#include "bamemory.h"
#include "bacamera.h"
#include "batextur.h"
#include "bapipe.h"
#include "baresour.h"
#include "bavector.h"

#include "bamatrix.h"

#if (defined(__ICL) && defined(RWSIMD))
#if (400<=__ICL)
#pragma message ( __FILE__ "(" RW_STRINGIFY_EXPANDED(__LINE__) "):" RW_STRINGIFY_EXPANDED(__ICL) "==__ICL - including <rtintel.h>")
#define RWCORE_H
#include <rtintel.h>
#pragma comment (lib , "rtintel.LIB")
#endif /* (400<=__ICL) */
#endif /* (defined(__ICL) && defined(RWSIMD)) */

#if (!defined(DOXYGEN))
static const char   rcsid[] __RWUNUSED__ =
    "@@(#)$Id: bamatrix.c,v 1.178 2001/08/17 11:54:29 johns Exp $";
#endif /* (!defined(DOXYGEN)) */

/****************************************************************************
 Local Types
 */

/****************************************************************************
 Local (Static) Prototypes
 */

/****************************************************************************
 Local Defines
 */

#ifndef M_PI

/* #define M_PI 3.1415926535897932 */
#   define M_PI 3.14159265358979323846
#endif /* M_PI */

#ifndef _ORTHONORMAL_TOL
#define _ORTHONORMAL_TOL ((RwReal)0.01)
#endif /* _ORTHONORMAL_TOL */

#ifndef _IDENTITY_TOL
#define _IDENTITY_TOL ((RwReal)0.01)
#endif /* _IDENTITY_TOL */

#ifndef  rwTOLSINMULTIPLEPI
#define  rwTOLSINMULTIPLEPI ((RwReal)0.01)
#endif /* rwTOLSINMULTIPLEPI */

#define RWMATRIXGLOBAL(var)                     \
    (RWPLUGINOFFSET(rwMatrixGlobals,            \
     RwEngineInstance,                          \
     matrixModule.globalsOffset)->var)

#define RwV3dDotDOWN(_a,_b,_col)                \
  ( ((_a).x) * ((_b).right._col) +              \
    ((_a).y) * ((_b).up._col)    +              \
    ((_a).z) * ((_b).at._col)    )

#define GENERIC_FLAGS(_matrix)                  \
   (~rwMATRIXINTERNALIDENTITY & (rwMatrixGetFlags(_matrix)))

/*
 * RwMatrixMultiplyMacro() can be overloaded per platform in
 * rwsdk/driver/$(RWTARGET)/drvmodel.h
 * The default/generic implementation is below.
 */
#if (!defined(RwMatrixMultiplyMacro))
#define RwMatrixMultiplyMacro(_matrix, _matrixIn1, _matrixIn2)              \
MACRO_START                                                                 \
{                                                                           \
    if (rwMatrixTestFlags((_matrixIn1),                                     \
                          (RWMATRIXGLOBAL(matrixOptimizations) &            \
                           rwMATRIXINTERNALIDENTITY)))                      \
    {                                                                       \
        RWASSERT(rwMatrixIsIdentity((_matrixIn1), _IDENTITY_TOL));          \
        RwMatrixCopy((_matrix), (_matrixIn2));                              \
    }                                                                       \
    else if (rwMatrixTestFlags((_matrixIn2),                                \
                               (RWMATRIXGLOBAL(matrixOptimizations) &       \
                                rwMATRIXINTERNALIDENTITY)))                 \
    {                                                                       \
        RWASSERT(rwMatrixIsIdentity((_matrixIn2), _IDENTITY_TOL));          \
        RwMatrixCopy((_matrix), (_matrixIn1));                              \
    }                                                                       \
    else                                                                    \
    {                                                                       \
        RWMATRIXGLOBAL(multMatrix) ((_matrix), (_matrixIn1), (_matrixIn2)); \
        rwMatrixSetFlags((_matrix),                                         \
                         rwMatrixGetFlags(matrix) &                         \
                         ~rwMATRIXINTERNALIDENTITY);                        \
        /* If either of the source matrices are not ortho-normal,           \
         * then the result is not orthonormal either                        \
         */                                                                 \
        if (!rwMatrixTestFlags((_matrixIn1), rwMATRIXTYPEORTHONORMAL) ||    \
            !rwMatrixTestFlags((_matrixIn2), rwMATRIXTYPEORTHONORMAL))      \
        {                                                                   \
            rwMatrixSetFlags((_matrix),                                     \
                             rwMatrixGetFlags(_matrix) &                    \
                             ~rwMATRIXTYPEORTHONORMAL);                     \
                                                                            \
        }                                                                   \
    }                                                                       \
}                                                                           \
MACRO_STOP
#endif /* (!defined(RwMatrixMultiplyMacro)) */

/*
 * RwMatrixInvertMacro() can be overloaded per platform in
 * rwsdk/driver/$(RWTARGET)/drvmodel.h
 * The default/generic implementation is below.
 */
#if (!defined(RwMatrixInvertMacro))
#define RwMatrixInvertMacro(_matrix, _matrixIn)                           \
MACRO_START                                                               \
{                                                                         \
    /* We might be able to do a really fast inversion if its Identity */  \
    if (rwMatrixTestFlags(_matrixIn,                                      \
                          (RWMATRIXGLOBAL(matrixOptimizations) &          \
                           rwMATRIXINTERNALIDENTITY)))                    \
    {                                                                     \
        RWASSERT(rwMatrixIsIdentity(_matrixIn, _IDENTITY_TOL));           \
        RwMatrixCopy(_matrix, _matrixIn);                                 \
    }                                                                     \
    else if (rwMatrixTestFlags(_matrixIn, rwMATRIXTYPEMASK) ==            \
             rwMATRIXTYPEORTHONORMAL)                                     \
    {                                                                     \
        /* We can do a fast inversion if it's ortho normal */             \
        MatrixInvertOrthoNormalized(_matrix, _matrixIn);                  \
    }                                                                     \
    else                                                                  \
    {                                                                     \
        MatrixInvertGeneric(_matrix, _matrixIn);                          \
    }                                                                     \
}                                                                         \
MACRO_STOP
#endif /* (!defined(RwMatrixInvertMacro)) */

/****************************************************************************
 Globals (across program)
 */

/****************************************************************************
 Local (static) Globals
 */

static RwModuleInfo matrixModule;

/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

   Matrix handling

   !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */

/****************************************************************************
 MatrixMultiply

 On entry   : Dest matrix pointer, two source matrix pointers
 On exit    : Matrix pointer contains result
 */
static
RWASMAPI(void)
MatrixMultiply(RwMatrix * dstMat,
               const RwMatrix * matA, const RwMatrix * matB)
{
    RWFUNCTION(RWSTRING("MatrixMultiply"));
    RWASSERT(dstMat);
    RWASSERT(matA);
    RWASSERT(RWMATRIXALIGNMENT(matA));
    RWASSERT(matB);
    RWASSERT(RWMATRIXALIGNMENT(matB));

    /* Multiply out right */
    dstMat->right.x =
        (matA->right.x * matB->right.x) +
        (matA->right.y * matB->up.x) + (matA->right.z * matB->at.x);
    dstMat->right.y =
        (matA->right.x * matB->right.y) +
        (matA->right.y * matB->up.y) + (matA->right.z * matB->at.y);
    dstMat->right.z =
        (matA->right.x * matB->right.z) +
        (matA->right.y * matB->up.z) + (matA->right.z * matB->at.z);

    /* Then up */
    dstMat->up.x =
        (matA->up.x * matB->right.x) +
        (matA->up.y * matB->up.x) + (matA->up.z * matB->at.x);
    dstMat->up.y =
        (matA->up.x * matB->right.y) +
        (matA->up.y * matB->up.y) + (matA->up.z * matB->at.y);
    dstMat->up.z =
        (matA->up.x * matB->right.z) +
        (matA->up.y * matB->up.z) + (matA->up.z * matB->at.z);

    /* Then at */
    dstMat->at.x =
        (matA->at.x * matB->right.x) +
        (matA->at.y * matB->up.x) + (matA->at.z * matB->at.x);
    dstMat->at.y =
        (matA->at.x * matB->right.y) +
        (matA->at.y * matB->up.y) + (matA->at.z * matB->at.y);
    dstMat->at.z =
        (matA->at.x * matB->right.z) +
        (matA->at.y * matB->up.z) + (matA->at.z * matB->at.z);

    /* Then pos - this is different because there is an extra add
     * (implicit 1 (one) in bottom right of matrix)
     */
    dstMat->pos.x =
        (matA->pos.x * matB->right.x) +
        (matA->pos.y * matB->up.x) +
        (matA->pos.z * matB->at.x) + ( /* (1*) */ matB->pos.x);
    dstMat->pos.y =
        (matA->pos.x * matB->right.y) +
        (matA->pos.y * matB->up.y) +
        (matA->pos.z * matB->at.y) + ( /* (1*) */ matB->pos.y);
    dstMat->pos.z =
        (matA->pos.x * matB->right.z) +
        (matA->pos.y * matB->up.z) +
        (matA->pos.z * matB->at.z) + ( /* (1*) */ matB->pos.z);

    /* And that's all folks */
    RWRETURNVOID();
}

/*
 * John's modified Gram-Schmidt algorithm
 * See also
 * http://www.gamasutra.com/features/programming/19980703/quaternions_03.htm
 */

static RwMatrix    *
MatrixOrthoNormalize(RwMatrix * matrix, const RwMatrix * matrixIn)
{
    RwV3d              *vpU, *vpV, *vpW;
    RwReal              vInner_x;
    RwReal              vInner_y;
    RwReal              vInner_z;
    RwV3d               vOuter;
    RwReal              recip;

    RWFUNCTION(RWSTRING("MatrixOrthoNormalize"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(matrixIn);
    RWASSERT(RWMATRIXALIGNMENT(matrixIn));

    /* Ortho-normalize a matrix in a right handed sense */
    RwV3dAssign(&matrix->right, &matrixIn->right);
    RwV3dAssign(&matrix->up, &matrixIn->up);
    RwV3dAssign(&matrix->at, &matrixIn->at);
    RwV3dAssign(&matrix->pos, &matrixIn->pos);

    _rwV3dNormalizeMacro(vInner_x, &matrix->right, &matrix->right);
    _rwV3dNormalizeMacro(vInner_y, &matrix->up, &matrix->up);
    _rwV3dNormalizeMacro(vInner_z, &matrix->at, &matrix->at);

    /*
     * pivot about non-zero pair of axes
     */
    if (vInner_x > 0.0f)
    {
        if (vInner_y > 0.0f)
        {
            if (vInner_z > 0.0f)
            {
                /*
                 * pivot about most 'right-angular' pair of axes
                 */
                vOuter.x =
                    RwV3dDotProductMacro(&matrix->up, &matrix->at);
                vOuter.x = RwRealAbs(vOuter.x);
                vOuter.y =
                    RwV3dDotProductMacro(&matrix->at, &matrix->right);
                vOuter.y = RwRealAbs(vOuter.y);
                vOuter.z =
                    RwV3dDotProductMacro(&matrix->right, &matrix->up);
                vOuter.z = RwRealAbs(vOuter.z);
                if (vOuter.x < vOuter.y)
                {
                    if (vOuter.x < vOuter.z)
                    {
                        vpU = &matrix->up;
                        vpV = &matrix->at;
                        vpW = &matrix->right;
                    }
                    else
                    {
                        vpU = &matrix->right;
                        vpV = &matrix->up;
                        vpW = &matrix->at;
                    }
                }
                else
                {
                    if (vOuter.y < vOuter.z)
                    {
                        vpU = &matrix->at;
                        vpV = &matrix->right;
                        vpW = &matrix->up;
                    }
                    else
                    {
                        vpU = &matrix->right;
                        vpV = &matrix->up;
                        vpW = &matrix->at;
                    }
                }
            }                  /* (Inner_z > 0.0f */
            else
            {
                vpU = &matrix->right;
                vpV = &matrix->up;
                vpW = &matrix->at;
            }
        }                      /* (Inner_y > 0.0f */
        else
        {
            vpU = &matrix->at;
            vpV = &matrix->right;
            vpW = &matrix->up;
        }
    }                          /* (Inner_x > 0.0f */
    else
    {
        vpU = &matrix->up;
        vpV = &matrix->at;
        vpW = &matrix->right;
    }

    RwV3dCrossProductMacro(vpW, vpU, vpV);
    _rwV3dNormalizeMacro(recip, vpW, vpW);
    RwV3dCrossProductMacro(vpV, vpW, vpU);
    _rwV3dNormalizeMacro(recip, vpV, vpV);

    /* It's now dirty, needs it's objects resyncing and is not identity */
    /* It is also orthonormal */
    rwMatrixSetFlags(matrix,
                     (rwMatrixGetFlags(matrix) |
                      rwMATRIXTYPEORTHONORMAL) &
                     ~rwMATRIXINTERNALIDENTITY);

    RWASSERT(rwMatrixIsOrthonormalPositive(matrix, _ORTHONORMAL_TOL));
    RWRETURN(matrix);
}

/****************************************************************************
 *
 * A faster matrix inversion function for orthonormal matrices
 * Tranposition
 *
 */
static RwMatrix    *
MatrixInvertOrthoNormalized(RwMatrix * matrix,
                            const RwMatrix * matrixIn)
{
    RWFUNCTION(RWSTRING("MatrixInvertOrthoNormalized"));
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(matrixIn);
    RWASSERT(RWMATRIXALIGNMENT(matrixIn));
    RWASSERT(rwMatrixIsOrthonormal(matrixIn, _ORTHONORMAL_TOL));
    /*
     * Inverse of upper left 3x3 sub matrix
     * is a simple transpose
     */
    matrix->right.x = matrixIn->right.x;
    matrix->right.y = matrixIn->up.x;
    matrix->right.z = matrixIn->at.x;
    matrix->up.x = matrixIn->right.y;
    matrix->up.y = matrixIn->up.y;
    matrix->up.z = matrixIn->at.y;
    matrix->at.x = matrixIn->right.z;
    matrix->at.y = matrixIn->up.z;
    matrix->at.z = matrixIn->at.z;
    /*
     * calculate translation componennt of inverse
     */
    matrix->pos.x =
        -RwV3dDotProductMacro(&matrixIn->pos, &matrixIn->right);
    matrix->pos.y =
        -RwV3dDotProductMacro(&matrixIn->pos, &matrixIn->up);
    matrix->pos.z =
        -RwV3dDotProductMacro(&matrixIn->pos, &matrixIn->at);
    rwMatrixSetFlags(matrix,
                     (rwMatrixGetFlags(matrix) |
                      (RwInt32) (rwMATRIXTYPEORTHONORMAL)) &
                     ~rwMATRIXINTERNALIDENTITY);

    RWASSERT(rwMatrixIsOrthonormal(matrix, _ORTHONORMAL_TOL));
    /* All done */
    RWRETURN(matrix);
}

static RwMatrix    *
MatrixInvertGeneric(RwMatrix * matrix, const RwMatrix * matrixIn)
{
    RWFUNCTION(RWSTRING("MatrixInvertGeneric"));

    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(matrixIn);
    RWASSERT(RWMATRIXALIGNMENT(matrixIn));

#undef  m_X_X
#define m_X_X matrixIn->right.x
#undef  m_X_Y
#define m_X_Y matrixIn->right.y
#undef  m_X_Z
#define m_X_Z matrixIn->right.z

#undef  m_Y_X
#define m_Y_X matrixIn->up.x
#undef  m_Y_Y
#define m_Y_Y matrixIn->up.y
#undef  m_Y_Z
#define m_Y_Z matrixIn->up.z

#undef  m_Z_X
#define m_Z_X matrixIn->at.x
#undef  m_Z_Y
#define m_Z_Y matrixIn->at.y
#undef  m_Z_Z
#define m_Z_Z matrixIn->at.z

#define T_X matrixIn->pos.x
#define T_Y matrixIn->pos.y
#define T_Z matrixIn->pos.z

#undef  i_X_X
#define i_X_X matrix->right.x
#undef  i_X_Y
#define i_X_Y matrix->right.y
#undef  i_X_Z
#define i_X_Z matrix->right.z

#undef  i_Y_X
#define i_Y_X matrix->up.x
#undef  i_Y_Y
#define i_Y_Y matrix->up.y
#undef  i_Y_Z
#define i_Y_Z matrix->up.z

#undef  i_Z_X
#define i_Z_X matrix->at.x
#undef  i_Z_Y
#define i_Z_Y matrix->at.y
#undef  i_Z_Z
#define i_Z_Z matrix->at.z

#undef  i_W_X
#define i_W_X matrix->pos.x
#undef  i_W_Y
#define i_W_Y matrix->pos.y
#undef  i_W_Z
#define i_W_Z matrix->pos.z

    /* Assumes input of form
     * [ [ m_X_X, m_X_Y, m_X_Z, 0 ]
     * [ m_Y_X, m_Y_Y, m_Y_Z, 0 ]
     * [ m_Z_X, m_Z_Y, m_Z_Z, 0 ]
     * [ T_X,   T_Y,   T_Z,   1 ] ]
     */

    /* Find inverse of 3 sub matrix
     * via transposed matrix of cofactors (cross-products) */

    rwMat03Inv(i_X_X, i_X_Y, i_X_Z,
               i_Y_X, i_Y_Y, i_Y_Z,
               i_Z_X, i_Z_Y, i_Z_Z,
               m_X_X, m_X_Y, m_X_Z,
               m_Y_X, m_Y_Y, m_Y_Z, m_Z_X, m_Z_Y, m_Z_Z);
    /*
     * Find translation component of inverse
     */

    i_W_X = -((T_X) * (i_X_X) + (T_Y) * (i_Y_X) + (T_Z) * (i_Z_X));
    i_W_Y = -((T_X) * (i_X_Y) + (T_Y) * (i_Y_Y) + (T_Z) * (i_Z_Y));
    i_W_Z = -((T_X) * (i_X_Z) + (T_Y) * (i_Y_Z) + (T_Z) * (i_Z_Z));

    /* Mark it as dirty */
    rwMatrixSetFlags(matrix,
                     rwMatrixGetFlags(matrix) &
                     ~rwMATRIXINTERNALIDENTITY);

    RWRETURN(matrix);
}

static const RwMatrix *
MatrixQueryRotateDegenerateUnitAxis(const RwMatrix * matrix,
                                    RwV3d * unitAxis)
{
    RwV3d               vTwoSinThetaAxis;
    RwReal              recip;

    RWFUNCTION(RWSTRING("MatrixQueryRotateDegenerateUnitAxis"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(unitAxis);

    /*
     * Matrix is:
     * [ [ 2 a_x^2 - 1,  2 a_x a_y,   2 a_x a_z,  0 ]
     *   [  2 a_x a_y,  2 a_y^2 - 1,  2 a_y a_z,  0 ]
     *   [  2 a_x a_z,   2 a_y a_z,  2 a_z^2 - 1, 0 ]
     *   [      0,           0,           0,      1 ] ]
     * Build axis scaled by 4 * component of maximum absolute value
     */
    if (matrix->right.x > matrix->up.y)
    {
        if (matrix->right.x > matrix->at.z)
        {
            vTwoSinThetaAxis.x = 1.0f + matrix->right.x;
            vTwoSinThetaAxis.x =
                vTwoSinThetaAxis.x + vTwoSinThetaAxis.x;
            vTwoSinThetaAxis.y = matrix->right.y + matrix->up.x;
            vTwoSinThetaAxis.z = matrix->right.z + matrix->at.x;
        }
        else
        {
            vTwoSinThetaAxis.z = 1.0f + matrix->at.z;
            vTwoSinThetaAxis.z =
                vTwoSinThetaAxis.z + vTwoSinThetaAxis.z;
            vTwoSinThetaAxis.x = matrix->at.x + matrix->right.z;
            vTwoSinThetaAxis.y = matrix->at.y + matrix->up.z;
        }
    }
    else
    {
        if (matrix->up.y > matrix->at.z)
        {
            vTwoSinThetaAxis.y = 1.0f + matrix->up.y;
            vTwoSinThetaAxis.y =
                vTwoSinThetaAxis.y + vTwoSinThetaAxis.y;
            vTwoSinThetaAxis.z = matrix->up.z + matrix->at.y;
            vTwoSinThetaAxis.x = matrix->up.x + matrix->right.y;
        }
        else
        {
            vTwoSinThetaAxis.z = 1.0f + matrix->at.z;
            vTwoSinThetaAxis.z =
                (vTwoSinThetaAxis.z + vTwoSinThetaAxis.z);
            vTwoSinThetaAxis.x = matrix->at.x + matrix->right.z;
            vTwoSinThetaAxis.y = matrix->at.y + matrix->up.z;
        }
    }

    /*
     * and normalize the axis
     */

    _rwV3dNormalizeMacro(recip, unitAxis, &vTwoSinThetaAxis);
    RWRETURN(matrix);
}

static const RwMatrix *
MatrixQueryRotate(const RwMatrix * matrix, RwV3d * unitAxis,
                  RwReal * angle, RwV3d * center)
{
    RwReal              nTwoSinTheta, nTwoCosTheta;
    RwV3d               vTwoSinThetaAxis;
    RwReal              nRadians;
    RwMatrix            mISubMat;
    RwMatrix            mISubMatInverse;

    RWFUNCTION(RWSTRING("MatrixQueryRotate"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(unitAxis);
    RWASSERT(angle);
    RWASSERT(center);

    rwMatrixInitialize(&mISubMat, rwMATRIXTYPENORMAL);
    rwMatrixInitialize(&mISubMatInverse, rwMATRIXTYPENORMAL);
    nTwoCosTheta = matrix->right.x + matrix->up.y + matrix->at.z - 1.0f;
    vTwoSinThetaAxis.x = matrix->up.z - matrix->at.y;
    vTwoSinThetaAxis.y = matrix->at.x - matrix->right.z;
    vTwoSinThetaAxis.z = matrix->right.y - matrix->up.x;
    nTwoSinTheta = RwV3dLength(&vTwoSinThetaAxis);
    if (nTwoSinTheta > 0.0f)
    {
        RwReal              recipLength = (1.0f / (nTwoSinTheta));

        RwV3dScaleMacro(unitAxis, &vTwoSinThetaAxis, recipLength);
    }
    else
    {
        unitAxis->x = 0.0f;
        unitAxis->y = 0.0f;
        unitAxis->z = 0.0f;
    }

    nRadians = (RwReal) RwATan2(nTwoSinTheta, nTwoCosTheta);
    (*angle) = nRadians * (((RwReal) 180) / ((RwReal) M_PI));
    if ((nTwoSinTheta <= rwTOLSINMULTIPLEPI) && (nTwoCosTheta <= 0.0f))
    {
        /*
         * sin theta is 0; cos theta is -1; theta is 180 degrees
         * vTwoSinThetaAxis was degenerate
         * axis will have to be found another way.
         */

        MatrixQueryRotateDegenerateUnitAxis(matrix, unitAxis);
    }

    /*
     * Find center of line of rotation:
     * [ v - c ] * R + c   = v * R + s
     * -> v * R + c - c *  R = v * R + s
     * -> c * [ I - R ]      = s
     * -> c                  = s * [ I - R ] ^ -1
     */

    RwMatrixSetIdentity(&mISubMat);
    /*
     * Find [ I - R ] ^ -1
     */
    RwV3dSubMacro(&mISubMat.right, &mISubMat.right, &matrix->right);
    RwV3dSubMacro(&mISubMat.up, &mISubMat.up, &matrix->up);
    RwV3dSubMacro(&mISubMat.at, &mISubMat.at, &matrix->at);
    rwMatrixSetFlags(&mISubMat, GENERIC_FLAGS(&mISubMatInverse));
    rwMatrixSetFlags(&mISubMatInverse, GENERIC_FLAGS(&mISubMatInverse));
    /* MatrixInvertOrthoNormalized(&mISubMatInverse, &mISubMat); */
    MatrixInvertGeneric(&mISubMatInverse, &mISubMat);
    /*
     * Find s * [ I - R ] ^ -1
     */
    *center = mISubMatInverse.pos;
    RwV3dIncrementScaledMacro(center, &mISubMatInverse.right,
                              matrix->pos.x);
    RwV3dIncrementScaledMacro(center, &mISubMatInverse.up,
                              matrix->pos.y);
    RwV3dIncrementScaledMacro(center, &mISubMatInverse.at,
                              matrix->pos.z);

    RWRETURN(matrix);
}

/****************************************************************************
 _rwMatrixSetMultFn

 On entry   : Matrix multiply function (NULL for default)
 On exit    : TRUE on success
 */

RwBool
_rwMatrixSetMultFn(rwMatrixMultFn multMat)
{
    RWFUNCTION(RWSTRING("_rwMatrixSetMultFn"));

    if (NULL == multMat)
    {
        multMat = MatrixMultiply;
#if (defined(__ICL) && defined(RWSIMD))
#if (400<=__ICL)
        {
            RtIntelOverload    *overload = _rtIntelOverloadGetHandle();
            rwMatrixMultFn      func =
                overload ? overload->MatrixMultiplyFunction : NULL;

            if (NULL != func)
                multMat = func;
        }
#endif /* (400<=__ICL) */
#endif /* (defined(__ICL) && defined(RWSIMD)) */
    }
    RWMATRIXGLOBAL(multMatrix) = multMat;

    RWRETURN(TRUE);
}

/****************************************************************************
 _rwMatrixSetOptimizations

 On entry   : Optimize flags (currently only rwMATRIXOPTIMIZE_IDENTITY is supported).
 On exit    : TRUE on success
 */

RwBool
_rwMatrixSetOptimizations(RwInt32 optimizeFlags)
{
    RWFUNCTION(RWSTRING("_rwMatrixSetOptimizations"));

    RWMATRIXGLOBAL(matrixOptimizations) = optimizeFlags;

    RWRETURN(TRUE);
}

/****************************************************************************
 _rwMatrixDeterminant
 */

RwReal
_rwMatrixDeterminant(const RwMatrix * matrix)
{
    RwReal              result;
    const RwV3d        *mx;
    const RwV3d        *my;
    const RwV3d        *mz;
    RwV3d               cross;

    /*
     * Expanded down implict final column
     */

    RWFUNCTION(RWSTRING("_rwMatrixDeterminant"));
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    mx = &matrix->right;
    my = &matrix->up;
    mz = &matrix->at;

    RwV3dCrossProductMacro(&cross, my, mz);
    result = RwV3dDotProductMacro(&cross, mx);

    RWRETURN(result);
}

/****************************************************************************
 _rwMatrixOrthogonalError
 */

RwReal
_rwMatrixOrthogonalError(const RwMatrix * matrix)
{
    RwReal              result;
    const RwV3d        *mx;
    const RwV3d        *my;
    const RwV3d        *mz;
    RwV3d               dot;

    RWFUNCTION(RWSTRING("_rwMatrixOrthogonalError"));
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    mx = &matrix->right;
    my = &matrix->up;
    mz = &matrix->at;

    dot.x = RwV3dDotProductMacro(my, mz);
    dot.y = RwV3dDotProductMacro(mz, mx);
    dot.z = RwV3dDotProductMacro(mx, my);

    result = RwV3dDotProductMacro(&dot, &dot);

    RWRETURN(result);
}

/****************************************************************************
 _rwMatrixNormalError
 */

RwReal
_rwMatrixNormalError(const RwMatrix * matrix)
{
    RwReal              result;
    const RwV3d        *x;
    const RwV3d        *y;
    const RwV3d        *z;
    RwV3d               dot;

    RWFUNCTION(RWSTRING("_rwMatrixNormalError"));
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    x = &matrix->right;
    y = &matrix->up;
    z = &matrix->at;

    dot.x = RwV3dDotProductMacro(x, x) - 1.0f;
    dot.y = RwV3dDotProductMacro(y, y) - 1.0f;
    dot.z = RwV3dDotProductMacro(z, z) - 1.0f;

    result = RwV3dDotProductMacro(&dot, &dot);

    RWRETURN(result);
}

/****************************************************************************
 _rwMatrixIdentityError
 */

RwReal
_rwMatrixIdentityError(const RwMatrix * matrix)
{
    /*
     * Row-norm implied metric
     */
    RwReal              result;
    const RwV3d        *mx;
    const RwV3d        *my;
    const RwV3d        *mz;
    const RwV3d        *mw;
    RwReal              error_x;
    RwReal              error_y;
    RwReal              error_z;
    RwReal              error_w;

    RWFUNCTION(RWSTRING("_rwMatrixIdentityError"));
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    mx = &matrix->right;
    my = &matrix->up;
    mz = &matrix->at;
    mw = &matrix->pos;

    error_x = (mx->x - 1.0f);
    error_x = error_x * error_x + mx->y * mx->y + mx->z * mx->z;

    error_y = (my->y - 1.0f);
    error_y = my->x * my->x + error_y * error_y + my->z * my->z;

    error_z = (mz->z - 1.0f);
    error_z = mz->x * mz->x + mz->y * mz->y + error_z * error_z;

    error_w = mw->x * mw->x + mw->y * mw->y + mw->z * mw->z;

    result = error_x + error_y + error_z + error_w;

    RWRETURN(result);
}

/****************************************************************************
 _rwMatrixClose

 On entry   :
 On exit    : TRUE on success
 */

void               *
_rwMatrixClose(void *instance,
               RwInt32 __RWUNUSED__ offset, RwInt32 __RWUNUSED__ size)
{
    RWFUNCTION(RWSTRING("_rwMatrixClose"));
    if (RWMATRIXGLOBAL(matrixFreeList))
    {
        RwFreeListDestroy(RWMATRIXGLOBAL(matrixFreeList));
        RWMATRIXGLOBAL(matrixFreeList) = (RwFreeList *) NULL;
    }

    /* One less module instance */
    matrixModule.numInstances--;
    /* Success */
    RWRETURN(instance);
}

/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

   Opening the matrix library

   !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */

/****************************************************************************
 _rwMatrixOpen

 On entry   :
 On exit    : FALSE on error
 */

void               *
_rwMatrixOpen(void *instance, RwInt32 offset, RwInt32 __RWUNUSED__ size)
{
    RWFUNCTION(RWSTRING("_rwMatrixOpen"));

    /* Save data block offset (same for all instances) */
    matrixModule.globalsOffset = offset;

    /* We just happen to know that the size is a mutiple of 16 bytes, so
     * this doesn't hurt on other platforms. :-)
     */
    RWMATRIXGLOBAL(matrixFreeList) =
        RwFreeListCreate(sizeof(RwMatrix), 50, rwMATRIXALIGNMENT);
    if (!RWMATRIXGLOBAL(matrixFreeList))
    {
        /* Failure */
        instance = NULL;
    }
    else
    {
        /* Set default optimizations */
        _rwMatrixSetOptimizations(rwMATRIXOPTIMIZE_IDENTITY);
        /* Set default multiply function */
        _rwMatrixSetMultFn((rwMatrixMultFn) NULL);
        /* One more module instance */
        matrixModule.numInstances++;
    }

    /* Success */
    RWRETURN(instance);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixOptimize informs the library that the application has
 * changed the specified matrix and that the library should take note of the
 * new matrix values. 
 *
 * This is only necessary when the application directly
 * modifies the matrix with
 * \ref RwMatrixGetRight,
 * \ref RwMatrixGetUp,
 * \ref RwMatrixGetAt or
 * \ref RwMatrixGetPos.
 * 
 * This differs from \ref RwMatrixUpdate in that extra work is undertaken
 * to identify special properties of the matrix suitable for subsequent
 * optimizations.
 *
 * \param matrix  Pointer to the matrix to update.
 * \param tolerance Tolerance for optimizations
 *
 * \return Returns pointer to the updated matrix.
 *
 * \see RwMatrixGetRight
 * \see RwMatrixGetUp
 * \see RwMatrixGetAt
 * \see RwMatrixGetPos
 * \see RwMatrixUpdate
 */
RwMatrix           *
RwMatrixOptimize(RwMatrix * matrix, RwReal tolerance)
{
    RwUInt32            flags;
    RwBool              MatrixIsNormal;
    RwBool              MatrixIsOrthogonal;
    RwBool              MatrixIsIdentity;

    RWAPIFUNCTION(RWSTRING("RwMatrixOptimize"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    /* Identify special properties of the matrix 
     * suitable for subsequent optimizations
     */

    MatrixIsNormal = (tolerance >= _rwMatrixNormalError(matrix));
    MatrixIsOrthogonal =
        (tolerance >= _rwMatrixOrthogonalError(matrix));
    MatrixIsIdentity = (MatrixIsNormal && MatrixIsOrthogonal
                        && rwMatrixIsIdentity(matrix, tolerance));

    /* Optimize the matrix flags */

    flags = rwMatrixGetFlags(matrix);

    if (MatrixIsNormal)
        flags |= rwMATRIXTYPENORMAL;
    else
        flags &= ~rwMATRIXTYPENORMAL;

    if (MatrixIsOrthogonal)
        flags |= rwMATRIXTYPEORTHOGANAL;
    else
        flags &= ~rwMATRIXTYPEORTHOGANAL;

    if (MatrixIsIdentity)
        flags |= rwMATRIXINTERNALIDENTITY;
    else
        flags &= ~rwMATRIXINTERNALIDENTITY;

    rwMatrixSetFlags(matrix, flags);

    RWRETURN(matrix);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixUpdate informs the library that the application has
 * changed the specified matrix and that the library should take note of the
 * new matrix values. This is only necessary when the application directly
 * modifies the matrix with
 * \ref RwMatrixGetRight,
 * \ref RwMatrixGetUp,
 * \ref RwMatrixGetAt or
 * \ref RwMatrixGetPos.
 *
 * \param matrix  Pointer to the matrix to update.
 *
 * \return Returns pointer to the updated matrix.
 *
 * \see RwMatrixGetRight
 * \see RwMatrixGetUp
 * \see RwMatrixGetAt
 * \see RwMatrixGetPos
 * \see RwMatrixOptimize
 */
RwMatrix           *
RwMatrixUpdate(RwMatrix * matrix)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixUpdate"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    /* Update the matrix flags */
    rwMatrixSetFlags(matrix,
                     rwMatrixGetFlags(matrix)
                     & ~(rwMATRIXTYPEORTHONORMAL |
                         rwMATRIXINTERNALIDENTITY));

    RWRETURN(matrix);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixMultiply multiplies two matrices in the order given.
 * Note that the matrix used for the result must be different from
 * the input matrices.
 *
 * \param matrix  Pointer to matrix which will receive the matrix product.
 * \param matrixIn1  Pointer to first matrix.
 * \param matrixIn2  Pointer to second matrix.
 *
 * \return Returns pointer to the product matrix.
 *
 * \see RwMatrixRotate
 * \see RwMatrixTranslate
 * \see RwMatrixScale
 * \see RwMatrixTransform
 * \see RwV3dTransformPoints
 * \see RwV3dTransformVectors
 *
 */
RwMatrix           *
RwMatrixMultiply(RwMatrix * matrix,
                 const RwMatrix * matrixIn1, const RwMatrix * matrixIn2)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixMultiply"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(matrixIn1);
    RWASSERT(RWMATRIXALIGNMENT(matrixIn1));
    RWASSERT(matrixIn2);
    RWASSERT(RWMATRIXALIGNMENT(matrixIn2));

    RwMatrixMultiplyMacro(matrix, matrixIn1, matrixIn2);

    RWRETURN(matrix);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixOrthoNormalize extracts the 'rigid-body'
 * transformation component from a matrix.
 *
 * Whilst RenderWare supports arbitrary 4x3 affine matrices, many
 * applications deal only in 'rigid-body' transformations comprising
 * rotation and translation without scaling or shearing. The 4x3 matrix
 * representing such a transformation has a special form of upper-left 3x3
 * sub-matrix known as an orthonormal matrix, characterised by its inverse
 * being equal to its transpose. Mathematically, this sub-matrix remains
 * orthonormal following any rigid-body transformation. Numerically,
 * however, after extended matrix composition some scale or shear factors
 * begin to accumulate due to rounding. To prevent the significant build-up
 * of such factors, \ref RwMatrixOrthoNormalize should be periodically applied
 * to ensure the matrix keeps its orthonormal form. The minimal satisfactory
 * frequency of orthonormalization will depend on the nature of the
 * application.
 *
 * Typically, a frequency of once every 128 frames is adequate.
 *
 * \param matrix  Pointer to matrix which will receive orthonormal form.
 * \param matrixIn  Pointer to matrix to be orthonormalized.
 *
 * \return Returns pointer to the orthonormalized matrix.
 *
 * \see RwMatrixInvert
 * \see RwMatrixSetIdentity
 *
 */

RwMatrix           *
RwMatrixOrthoNormalize(RwMatrix * matrix, const RwMatrix * matrixIn)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixOrthoNormalize"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(matrixIn);
    RWASSERT(RWMATRIXALIGNMENT(matrixIn));

    RWRETURN(MatrixOrthoNormalize(matrix, matrixIn));
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixRotateOneMinusCosineSine builds a rotation matrix from
 * the given axis and applies it to the specified matrix. The angle of
 * rotation is supplied by giving values for 1-cos(theta) and sin(theta) where
 * theta is the angle of rotation. This function should be used in preference
 * to \ref RwMatrixRotate, where possible, as it is substantially faster. The
 * combine operation may be pre-concatenation, post-concatenation or
 * replacement.
 *
 * \param matrix  Pointer to matrix to apply rotation to.
 * \param unitAxis  Pointer to a unit vector describing the axis of rotation.
 * \param oneMinusCosine  The value of 1-cos(theta), where theta is the angle of rotation.
 * \param sine  The value of sin(theta).
 * \param combineOp  Combination operator specifying how rotation is applied.
 *
 * \return Returns pointer to the new matrix if successful or NULL if there
 * is an error.
 *
 * \see RwMatrixRotate
 * \see RwMatrixTranslate
 * \see RwMatrixScale
 * \see RwMatrixTransform
 *
 */
RwMatrix           *
RwMatrixRotateOneMinusCosineSine(RwMatrix * matrix,
                                 const RwV3d * unitAxis,
                                 RwReal oneMinusCosine,
                                 RwReal sine, RwOpCombineType combineOp)
{
    RwMatrix            mRotate;
    RwMatrix            mLocal;
    RwV3d               vLeading;
    RwV3d               vScaled;
    RwV3d               vCrossed;

    RWAPIFUNCTION(RWSTRING("RwMatrixRotateOneMinusCosineSine"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(unitAxis);

    rwMatrixInitialize(&mRotate, rwMATRIXTYPEORTHONORMAL);
    /*
     * Rotatation matrix, for s =sin(theta), c = cos(theta) around unit
     * axis [a_x, a_y, a_Z] is:
     *
     * [ [ 1, 0, 0, 0 ]     [ [ a_x^2 - 1,  a_y a_x,   a_z a_x,  0 ]               [ [  0,   a_z, -a_y,  0 ]
     * [ 0, 1, 0, 0 ]   +   [  a_x a_y,  a_y^2 - 1,  a_z a_y,  0 ]   * (1 - c) +   [ -a_z,  0,  a_x,   0 ]   * s
     * [ 0, 0, 1, 0 ]       [  a_x a_z,   a_y a_z,  a_z^2 - 1, 0 ]                 [ a_y, -a_x,   0,   0 ]
     * [ 0, 0, 0, 1 ] ]     [     0,         0,         0,     1 ] ]               [  0,    0,    0,   1 ] ]
     * =
     *
     * [ [ 1 + (a_x^2 - 1) (1 - c), a_x a_y(1 - c) + a_z s,  a_x a_z(1 - c) - a_y s,  0 ]
     * [ a_y a_x(1 - c) - a_z s,  1 + (a_y^2 - 1) (1 - c), a_y a_z(1 - c) + a_x s,  0 ]
     * [ a_z a_x(1 - c) + a_y s,  a_z a_y(1 - c) - a_x s,  1 + (a_z^2 - 1) (1 - c), 0 ]
     * [            0,                       0,                       0,            1 ] ]
     */
    /*
     * Common sub expressions
     */
    vLeading.x = (1.0f - (((unitAxis->x) * (unitAxis->x))));
    vLeading.y = (1.0f - (((unitAxis->y) * (unitAxis->y))));
    vLeading.z = (1.0f - (((unitAxis->z) * (unitAxis->z))));
    RwV3dScaleMacro(&vLeading, &vLeading, oneMinusCosine);
    vCrossed.x = ((unitAxis->y) * (unitAxis->z));
    vCrossed.y = ((unitAxis->z) * (unitAxis->x));
    vCrossed.z = ((unitAxis->x) * (unitAxis->y));
    RwV3dScaleMacro(&vCrossed, &vCrossed, oneMinusCosine);
    RwV3dScaleMacro(&vScaled, unitAxis, sine);
    /*
     * Rotate matrix proper
     */
    mRotate.right.x = (1.0f - (vLeading.x));
    mRotate.right.y = ((vCrossed.z) + (vScaled.z));
    mRotate.right.z = ((vCrossed.y) - (vScaled.y));
    mRotate.up.x = ((vCrossed.z) - (vScaled.z));
    mRotate.up.y = (1.0f - (vLeading.y));
    mRotate.up.z = ((vCrossed.x) + (vScaled.x));
    mRotate.at.x = ((vCrossed.y) + (vScaled.y));
    mRotate.at.y = ((vCrossed.x) - (vScaled.x));
    mRotate.at.z = (1.0f - (vLeading.z));
    mRotate.pos.x = 0.0f;
    mRotate.pos.y = 0.0f;
    mRotate.pos.z = 0.0f;
    RWASSERT(rwMatrixIsOrthonormalPositive(&mRotate, _ORTHONORMAL_TOL));
    rwMatrixSetFlags(&mRotate,
                     rwMatrixGetFlags(&mRotate) &
                     ~rwMATRIXINTERNALIDENTITY);

    switch (combineOp)
    {
        case rwCOMBINEREPLACE:
            {
                /* Preserve the flags (but reset identity matrix bit) */
                mRotate.flags = matrix->flags;
                RwMatrixCopy(matrix, &mRotate);
                rwMatrixSetFlags(matrix,
                                 rwMatrixGetFlags(matrix) &
                                 ~rwMATRIXINTERNALIDENTITY);
                break;
            }
        case rwCOMBINEPRECONCAT:
            {
                RwMatrixMultiply(&mLocal, &mRotate, matrix);
                mLocal.flags = matrix->flags;
                RwMatrixCopy(matrix, &mLocal);
                rwMatrixSetFlags(matrix,
                                 rwMatrixGetFlags(matrix) &
                                 ~rwMATRIXINTERNALIDENTITY);
                break;
            }
        case rwCOMBINEPOSTCONCAT:
            {
                RwMatrixMultiply(&mLocal, matrix, &mRotate);
                mLocal.flags = matrix->flags;
                RwMatrixCopy(matrix, &mLocal);
                rwMatrixSetFlags(matrix,
                                 rwMatrixGetFlags(matrix) &
                                 ~rwMATRIXINTERNALIDENTITY);
                break;
            }
        default:
            {
                RWERROR((E_RW_BADPARAM,
                         RWSTRING("Invalid combination type")));
                matrix = (RwMatrix *) NULL;
            }
    }

    RWRETURN(matrix);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixRotate builds a rotation matrix from the given axis
 * and angle of rotation and applies it to the specified matrix. The combine
 * operation may be pre-concatenation, post-concatenation or replacement.
 *
 * \param matrix  Pointer to matrix to apply rotation to.
 * \param axis  Pointer to vector specifying axis of rotation.
 * \param angle  Angle of rotation in degrees.
 * \param combineOp  Combination operator specifying how the rotation is applied.
 *
 * \return Returns pointer to the new matrix if successful or NULL if there
 * is an error.
 *
 * \see RwMatrixTranslate
 * \see RwMatrixScale
 * \see RwMatrixTransform
 * \see RwMatrixRotateOneMinusCosineSine
 *
 */
RwMatrix           *
RwMatrixRotate(RwMatrix * matrix, const RwV3d * axis,
               RwReal angle, RwOpCombineType combineOp)
{
    RwV3d               unitAxis;
    RwReal              sinVal, oneMinusCosVal;
    RwReal              radians =
        ((angle) * ((RwReal) ((rwPI / 180.0))));
    RwReal              recip;

    RWAPIFUNCTION(RWSTRING("RwMatrixRotate"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(axis);

    _rwV3dNormalizeMacro(recip, &unitAxis, axis);
    sinVal = (RwReal) ((RwSin(((radians)))));
    oneMinusCosVal = (1.0f - ((RwReal) ((RwCos(((radians)))))));
    RwMatrixRotateOneMinusCosineSine(matrix,
                                     &unitAxis,
                                     oneMinusCosVal, sinVal, combineOp);
    /* Above function already did state count thing */

    RWRETURN(matrix);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixInvert performs a matrix inversion calculation on the
 * input matrix. Note that the input and output arguments must not point to
 * the same matrix.
 *
 * \param matrix  Pointer to matrix where the inverse will be stored.
 * \param matrixIn  Pointer to matrix which is to be inverted.
 *
 * \return Returns a pointer to the inverted matrix.
 *
 */
RwMatrix           *
RwMatrixInvert(RwMatrix * matrix, const RwMatrix * matrixIn)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixInvert"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(matrixIn);
    RWASSERT(RWMATRIXALIGNMENT(matrixIn));

    RwMatrixInvertMacro(matrix, matrixIn);

    RWRETURN(matrix);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixScale builds a scaling matrix from the given scale factors
 * and applies it to the specified matrix. The combine operation may be
 * pre-concatenation, post-concatenation or replacement.
 *
 * \param matrix  Pointer to matrix to apply scaling to.
 * \param scale  Pointer to vector specifying the scale factors.
 * \param comginOp  Combine operator specifiying how the scaling is applied:
 *
 * \li rwCOMBINEREPLACE     Replacement
 *
 * \li rwCOMBINEPRECONCAT   Pre-concatenation
 *
 * \li rwCOMBINEPOSTCONCAT  Post-concatenation
 *
 * \return Returns pointer to the new matrix if successful or NULL if there
 * is an error.
 *
 * \see RwMatrixRotate
 * \see RwMatrixTranslate
 * \see RwMatrixTransform
 * \see RwV3dTransformPoints
 * \see RwV3dTransformVectors
 *
 */
RwMatrix           *
RwMatrixScale(RwMatrix * matrix, const RwV3d * scale,
              RwOpCombineType combineOp)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixScale"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(scale);

    switch (combineOp)
    {
        case rwCOMBINEREPLACE:
            {
                RwMatrixSetIdentity(matrix);
                matrix->right.x = scale->x;
                matrix->up.y = scale->y;
                matrix->at.z = scale->z;
                break;
            }
        case rwCOMBINEPRECONCAT:
            {
                RwV3dScaleMacro(&matrix->right, &matrix->right,
                                scale->x);
                RwV3dScaleMacro(&matrix->up, &matrix->up, scale->y);
                RwV3dScaleMacro(&matrix->at, &matrix->at, scale->z);
                break;
            }
        case rwCOMBINEPOSTCONCAT:
            {
                matrix->right.x = ((matrix->right.x) * (scale->x));
                matrix->right.y = ((matrix->right.y) * (scale->y));
                matrix->right.z = ((matrix->right.z) * (scale->z));
                matrix->up.x = ((matrix->up.x) * (scale->x));
                matrix->up.y = ((matrix->up.y) * (scale->y));
                matrix->up.z = ((matrix->up.z) * (scale->z));
                matrix->at.x = ((matrix->at.x) * (scale->x));
                matrix->at.y = ((matrix->at.y) * (scale->y));
                matrix->at.z = ((matrix->at.z) * (scale->z));
                matrix->pos.x = ((matrix->pos.x) * (scale->x));
                matrix->pos.y = ((matrix->pos.y) * (scale->y));
                matrix->pos.z = ((matrix->pos.z) * (scale->z));
                break;
            }

        default:
            {
                RWERROR((E_RW_BADPARAM,
                         RWSTRING("Invalid combination type")));
                matrix = (RwMatrix *) NULL;
            }
    }

    /* Mark as dirty and it's not orthonormal any more */
    rwMatrixSetFlags(matrix,
                     rwMatrixGetFlags(matrix) &
                     ~(rwMATRIXINTERNALIDENTITY |
                       rwMATRIXTYPEORTHONORMAL));

    RWRETURN(matrix);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixTranslate builds a translation matrix from the given
 * vector and applies it to the specified matrix. The combine operation may
 * be pre-concatenation, post-concatenation or replacement.
 *
 * \param matrix  Pointer to matrix to apply translation to.
 * \param translation  Pointer to vector specifying the components of translation.
 * \param combineOp  Combination operator specifying how the translation is applied.
 *
 *  \li  rwCOMBINEREPLACE    Replacement
 *
 *  \li  rwCOMBINEPRECONCAT  Pre-concatenation
 *
 *  \li  rwCOMBINEPOSTCONCAT Post-concatenation
 *
 * \return Returns pointer to the new matrix if successful or NULL if there
 * is an error.
 *
 * \see RwMatrixRotate
 * \see RwMatrixScale
 * \see RwMatrixTransform
 * \see RwV3dTransformPoints
 * \see RwV3dTransformVectors
 *
 */
RwMatrix           *
RwMatrixTranslate(RwMatrix * matrix,
                  const RwV3d * translation, RwOpCombineType combineOp)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixTranslate"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(translation);

#if (0)
    if (!RWMATRIXALIGNMENT(matrix))
    {
        RWMESSAGE(("matrix %p rwMATRIXALIGNMENT %d  (((rwMATRIXALIGNMENT)-1) & ((RwUInt32)(matrix))) %d", matrix, rwMATRIXALIGNMENT, (((rwMATRIXALIGNMENT) - 1) & ((RwUInt32) (matrix)))));
    }
#endif /* (0) */

    switch (combineOp)
    {
        case rwCOMBINEREPLACE:
            {
                RwMatrixSetIdentity(matrix);
                matrix->pos.x = translation->x;
                matrix->pos.y = translation->y;
                matrix->pos.z = translation->z;
                break;
            }
        case rwCOMBINEPRECONCAT:
            {
                matrix->pos.x =
                    ((matrix->pos.x) +
                     (RwV3dDotDOWN((*translation), (*matrix), x)));
                matrix->pos.y =
                    ((matrix->pos.y) +
                     (RwV3dDotDOWN((*translation), (*matrix), y)));
                matrix->pos.z =
                    ((matrix->pos.z) +
                     (RwV3dDotDOWN((*translation), (*matrix), z)));
                break;
            }
        case rwCOMBINEPOSTCONCAT:
            {
                matrix->pos.x = ((matrix->pos.x) + (translation->x));
                matrix->pos.y = ((matrix->pos.y) + (translation->y));;
                matrix->pos.z = ((matrix->pos.z) + (translation->z));
                break;
            }
        default:
            {
                RWERROR((E_RW_BADPARAM,
                         RWSTRING("Invalid combination type")));
                matrix = (RwMatrix *) NULL;
            }
    }

    /* Mark as dirty */
    rwMatrixSetFlags(matrix,
                     rwMatrixGetFlags(matrix) &
                     ~rwMATRIXINTERNALIDENTITY);

    /* Done */
    RWRETURN(matrix);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixTransform calculates the result of applying a general
 * transformation matrix to the given matrix. The combine operation may be
 * pre-concatenation, post-concatenation or replacement.
 *
 * \param matrix  Pointer to matrix which to apply transformation to.
 * \param transform  Pointer to matrix which specifies the transformation.
 * \param combineOp  Combination operator specifying how the transformation is applied.
 *
 * \li   rwCOMBINEREPLACE    Replacement
 *
 * \li   rwCOMBINEPRECONCAT  Pre-concatenation
 *
 * \li   rwCOMBINEPOSTCONCAT Post-concatenation.
 *
 * \return Returns pointer to the new matrix if successful or NULL if there
 * is an error.
 *
 * \see RwMatrixRotate
 * \see RwMatrixTranslate
 * \see RwMatrixScale
 * \see RwV3dTransformPoints
 * \see RwV3dTransformVectors
 *
 */
RwMatrix           *
RwMatrixTransform(RwMatrix * matrix,
                  const RwMatrix * transform, RwOpCombineType combineOp)
{
    RwMatrix            mTmp;

    RWAPIFUNCTION(RWSTRING("RwMatrixTransform"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(transform);

    rwMatrixInitialize(&mTmp, rwMATRIXTYPENORMAL);
    switch (combineOp)
    {
        case rwCOMBINEREPLACE:
            {
                RwMatrixCopy(matrix, transform);
                matrix->flags = transform->flags;
                break;
            }
        case rwCOMBINEPRECONCAT:
            {
                RwMatrixMultiply(&mTmp, transform, matrix);
                mTmp.flags = matrix->flags;
                RwMatrixCopy(matrix, &mTmp);
                rwMatrixSetFlags(matrix,
                                 rwMatrixGetFlags(matrix) &
                                 ~(rwMATRIXTYPEORTHONORMAL |
                                   rwMATRIXINTERNALIDENTITY));
                break;
            }
        case rwCOMBINEPOSTCONCAT:
            {
                RwMatrixMultiply(&mTmp, matrix, transform);
                mTmp.flags = matrix->flags;
                RwMatrixCopy(matrix, &mTmp);
                rwMatrixSetFlags(matrix,
                                 rwMatrixGetFlags(matrix) &
                                 ~(rwMATRIXTYPEORTHONORMAL |
                                   rwMATRIXINTERNALIDENTITY));
                break;
            }
        default:
            {
                RWERROR((E_RW_BADPARAM,
                         RWSTRING("Invalid combination type")));
                matrix = (RwMatrix *) NULL;

            }
    }

    RWRETURN(matrix);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixQueryRotate determines the rotation component from a
 * matrix comprising only rotations and translations. The rotation is returned
 * as a unit vector along the axis of rotation, an angle of rotation and a
 * point on the axis which is the nearest to the origin (hence the line
 * joining this point with the origin is perpendicular to the axis).
 *
 * The rotation component has two possible descriptions since a rotation about
 * an axis of theta degrees is equivalent to a rotation about an axis pointing
 * in the opposite direction by an angle of 360-theta in the reverse direction.
 * The angle returned by \ref RwMatrixQueryRotate is always between 0 and 180
 * degrees and the direction of the axis of rotation returned is chosen to
 * ensure the angle is in this range. Note that only matrices known to be
 * composed solely of rotations and translations, i.e. OrthoNormal/rigid-body
 * transformations, should be queried with this function. The results from
 * querying matrices incorporating transforms such as scaling are unlikely to
 * be useful.
 *
 * \param matrix  Pointer to matrix to be investigated.
 * \param unitAxis  Pointer to vector which will receive the unit direction vector along
 * axis of rotation.
 * \param angle  Pointer to RwReal which will receive angle of rotation in degrees.
 * \param center  Pointer to vector which will receive center of line of rotation.
 *
 * \return Returns pointer to the queried matrix.
 *
 * \see RwMatrixRotate
 * \see RwMatrixRotateOneMinusCosineSine
 *
 */
const RwMatrix     *
RwMatrixQueryRotate(const RwMatrix * matrix, RwV3d * unitAxis,
                    RwReal * angle, RwV3d * center)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixQueryRotate"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWASSERT(unitAxis);
    RWASSERT(angle);
    RWASSERT(center);

    RWRETURN(MatrixQueryRotate(matrix, unitAxis, angle, center));
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixDestroy is used to delete a transformation matrix previously
 * created by the API function \ref RwMatrixCreate.
 * matrix.
 *
 * \param matrix  Pointer to matrix which is to be destroyed.
 *
 * \return Returns TRUE.
 *
 * \see RwMatrixCreate
 *
 */
RwBool
RwMatrixDestroy(RwMatrix * matrix)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixDestroy"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    RwFreeListFree(RWMATRIXGLOBAL(matrixFreeList), matrix);

    RWRETURN(TRUE);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixCreate creates a new transformation matrix initialized to
 * the identity matrix.  Note that the matrix must be destroyed when it is no
 * longer required by an application.
 *
 * \return Returns pointer to created matrix if successful or NULL if
 * there is an error.
 *
 * \see RwMatrixCopy
 * \see RwMatrixDestroy
 *
 */
RwMatrix           *
RwMatrixCreate(void)
{
    RwMatrix           *matrix = (RwMatrix *) NULL;

    RWAPIFUNCTION(RWSTRING("RwMatrixCreate"));
    RWASSERT(matrixModule.numInstances);

    matrix =
        (RwMatrix *) RwFreeListAlloc(RWMATRIXGLOBAL(matrixFreeList));
    RWASSERT(RWFREELISTALIGNED(matrix, RWMATRIXGLOBAL(matrixFreeList)));
    if (matrix)
    {
        rwMatrixInitializeIdentity(matrix, rwMATRIXTYPEORTHONORMAL);
    }

    RWASSERT(RWMATRIXALIGNMENT(matrix));
    RWRETURN(matrix);
}

#ifdef RWDEBUG

/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

   Accessing parts of a matrix

   !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
/**
 * \ingroup rwmatrix
 * \ref RwMatrixGetRight is used to retrieve the 'look-right' vector
 * of the specified transformation matrix.
 *
 * Note that this function is used for debug purposes only and, for
 * efficiency, is available as a macro for final release versions of an
 * application.
 *
 * \param matrix  Pointer to the matrix to get 'look-right' vector from.
 *
 * \return Returns pointer to 'look-right' vector.
 *
 * \see RwMatrixGetUp
 * \see RwMatrixGetAt
 * \see RwMatrixGetPos
 * \see RwMatrixUpdate
 * \see RwMatrixOptimize
 */
RwV3d              *
RwMatrixGetRight(RwMatrix * matrix)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixGetRight"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    RWRETURN(&matrix->right);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixGetUp is used to retrieve the 'look-up' vector of
 * the specified transformation matrix.
 *
 * Note that this function is used for debug purposes only and, for
 * efficiency, is available as a macro for final release versions of an
 * application.
 *
 * \param matrix  Pointer to the matrix to get 'look-up' vector from.
 *
 * \return Returns pointer to 'look-up' vector.
 *
 * \see RwMatrixGetRight
 * \see RwMatrixGetAt
 * \see RwMatrixGetPos
 * \see RwMatrixUpdate
 * \see RwMatrixOptimize
 */
RwV3d              *
RwMatrixGetUp(RwMatrix * matrix)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixGetUp"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    RWRETURN(&matrix->up);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixGetAt is used to retrieve the 'look-at' vector of the
 * specified transformation matrix.
 *
 * Note that this function is used for debug purposes only and, for
 * efficiency, is available as a macro for final release versions of an
 * application.
 *
 * \param matrix  Pointer to the matrix to get 'look-at' vector from.
 *
 * \return Returns pointer to 'look-at' vector.
 *
 * \see RwMatrixGetRight
 * \see RwMatrixGetUp
 * \see RwMatrixGetPos
 * \see RwMatrixUpdate
 * \see RwMatrixOptimize
 */
RwV3d              *
RwMatrixGetAt(RwMatrix * matrix)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixGetAt"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    RWRETURN(&matrix->at);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixGetPos is used to retrieve the 'position' vector from
 * the specified transformation matrix.
 *
 * Note that this function is used for debug purposes only and, for
 * efficiency, is available as a macro for final release versions of an
 * application.
 *
 * \param matrix  Pointer to the matrix to get 'position' vector from.
 *
 * \return Returns pointer to matrix 'position' vector.
 *
 * \see RwMatrixGetRight
 * \see RwMatrixGetUp
 * \see RwMatrixGetAt
 * \see RwMatrixUpdate
 * \see RwMatrixOptimize 
 */
RwV3d              *
RwMatrixGetPos(RwMatrix * matrix)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixGetPos"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    RWRETURN(&matrix->pos);
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixCopy is used to copy the contents of one matrix to
 * another. Both matrices must exist or have been created using
 * \ref RwMatrixCreate before calling this function.
 *
 * Note that this function is used for debug purposes only and,
 * for efficiency, is available as a macro for final release versions
 * of an application.
 *
 * \param dstMatrix  Pointer to the destination matrix.
 * \param srcMatrix  Pointer to the source matrix.
 *
 * \return None.
 *
 * \see RwMatrixCreate
 * \see RwMatrixMultiply
 * \see RwMatrixInvert
 * \see RwMatrixRotate
 * \see RwMatrixScale
 * \see RwMatrixTranslate
 * \see RwMatrixTransform
 *
 */
void
RwMatrixCopy(RwMatrix * dstMatrix, const RwMatrix * srcMatrix)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixCopy"));

    RWASSERT(matrixModule.numInstances);

    RWASSERT(dstMatrix);
    RWASSERT(RWMATRIXALIGNMENT(dstMatrix));
    RWASSERT(srcMatrix);
    RWASSERT(RWMATRIXALIGNMENT(srcMatrix));

    /* Copy the matrix type */
    RwMatrixCopyMacro(dstMatrix, srcMatrix);

    RWRETURNVOID();
}

/**
 * \ingroup rwmatrix
 * \ref RwMatrixSetIdentity is used to convert the specified matrix to
 * the identity matrix.
 *
 * Note that this function is used for debug purposes only and, for efficiency,
 * is available as a macro for final release versions of an application.
 *
 * \param matrix  Pointer to the matrix to reset to the identity matrix.
 *
 * \return None.
 *
 * \see RwMatrixCreate
 *
 */
void
RwMatrixSetIdentity(RwMatrix * matrix)
{
    RWAPIFUNCTION(RWSTRING("RwMatrixSetIdentity"));
    RWASSERT(matrixModule.numInstances);
    RWASSERT(matrix);
    RWASSERT(RWMATRIXALIGNMENT(matrix));

    matrix->right.x = matrix->up.y = matrix->at.z = 1.0f;
    matrix->right.y = matrix->right.z = matrix->up.x = 0.0f;
    matrix->up.z = matrix->at.x = matrix->at.y = 0.0f;
    matrix->pos.x = matrix->pos.y = matrix->pos.z = 0.0f;
    rwMatrixSetFlags(matrix, rwMatrixGetFlags(matrix) |
                     (RwInt32) (rwMATRIXINTERNALIDENTITY |
                                rwMATRIXTYPEORTHONORMAL));

    /*
     * RWASSERT(rwMatrixIsIdentity(matrix, _IDENTITY_TOL));
     */

    RWRETURNVOID();
}

#endif /* RWDEBUG */
