/*****************************************************************************
* General matrix manipulation routines.					     *
*									     *
* Written by:  Gershon Elber			       Ver 1.0,	June 1988    *
*****************************************************************************/

#include <math.h>
#include "irit_sm.h"
#include "genmat.h"

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a 4*4 unit matrix:                                     M
*                                                                            *
* PARAMETERS:                                                                M
*   Mat:      Matrix to initialize as a unit matrix.                         M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatGenUnitMat, transformations, unit matrix                              M
*****************************************************************************/
void MatGenUnitMat(MatrixType Mat)
{
    int i, j;

    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++)
	    if (i == j)
		Mat[i][j] = 1.0;
	    else
		Mat[i][j] = 0.0;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts.       M
*									     *
* PARAMETERS:                                                                M
*   Tx, Ty, Tz:  Translational amounts requested.                            M
*   Mat:         Matrix to initialize as a translation matrix.               M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatGenMatTrans, transformations, translation                             M
*****************************************************************************/
void MatGenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
{
    MatGenUnitMat(Mat);                             /* Make it unit matrix. */
    Mat[3][0] = Tx;
    Mat[3][1] = Ty;
    Mat[3][2] = Tz;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts.   M
*									     *
* PARAMETERS:                                                                M
*   Sx, Sy, Sz:  Scaling factors requested.                                  M
*   Mat:         Matrix to initialize as a scaling matrix.                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatGenMatScale, transformations, scaling                                 M
*****************************************************************************/
void MatGenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
{
    MatGenUnitMat(Mat);                              /* Make it unit matrix. */
    Mat[0][0] = Sx;
    Mat[1][1] = Sy;
    Mat[2][2] = Sz;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a 4*4 matrix to uniformly scale Scale amount.	     M
*									     *
* PARAMETERS:                                                                M
*   Scale:       Uniform scaling factor requested.                           M
*   Mat:         Matrix to initialize as a scaling matrix.                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatGenMatUnifScale, transformations, scaling                             M
*****************************************************************************/
void MatGenMatUnifScale(RealType Scale, MatrixType Mat)
{
    MatGenMatScale(Scale, Scale, Scale, Mat);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a 4*4 matrix to Rotate around the X axis by Teta       M
* radians.                                                                   M
*                                                                            *
* PARAMETERS:                                                                M
*   Teta:       Amount of rotation, in radians.                              M
*   Mat:        Matrix to initialize as a rotation matrix.                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatGenMatRotX1, transformations, rotation                                M
*****************************************************************************/
void MatGenMatRotX1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

    CTeta = cos(Teta);
    STeta = sin(Teta);
    MatGenMatRotX(CTeta, STeta, Mat);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a 4*4 matrix to Rotate around the X axis by Teta,      M
* given the sin and cosine of Teta                                           M
*                                                                            *
* PARAMETERS:                                                                M
*   SinTeta, CosTeta:  Amount of rotation, given as sine and cosine of Teta. M
*   Mat:               Matrix to initialize as a rotation matrix.            M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatGenMatRotX, transformations, rotation                                 M
*****************************************************************************/
void MatGenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
    MatGenUnitMat(Mat);                              /* Make it unit matrix. */
    Mat[1][1] = CosTeta;
    Mat[1][2] = SinTeta;
    Mat[2][1] = -SinTeta;
    Mat[2][2] = CosTeta;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a 4*4 matrix to Rotate around the Y axis by Teta       M
* radians.                                                                   M
*                                                                            *
* PARAMETERS:                                                                M
*   Teta:       Amount of rotation, in radians.                              M
*   Mat:        Matrix to initialize as a rotation matrix.                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatGenMatRotY1, transformations, rotation                                M
*****************************************************************************/
void MatGenMatRotY1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

    CTeta = cos(Teta);
    STeta = sin(Teta);
    MatGenMatRotY(CTeta, STeta, Mat);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a 4*4 matrix to Rotate around the Y axis by Teta,      M
* given the sin and cosine of Teta                                           M
*                                                                            *
* PARAMETERS:                                                                M
*   SinTeta, CosTeta:  Amount of rotation, given as sine and cosine of Teta. M
*   Mat:               Matrix to initialize as a rotation matrix.            M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatGenMatRotY, transformations, rotation                                 M
*****************************************************************************/
void MatGenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
    MatGenUnitMat(Mat);                              /* Make it unit matrix. */
    Mat[0][0] = CosTeta;
    Mat[0][2] = -SinTeta;
    Mat[2][0] = SinTeta;
    Mat[2][2] = CosTeta;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a 4*4 matrix to Rotate around the Z axis by Teta       M
* radians.                                                                   M
*                                                                            *
* PARAMETERS:                                                                M
*   Teta:       Amount of rotation, in radians.                              M
*   Mat:        Matrix to initialize as a rotation matrix.                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatGenMatRotZ1, transformations, rotation                                M
*****************************************************************************/
void MatGenMatRotZ1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

    CTeta = cos(Teta);
    STeta = sin(Teta);
    MatGenMatRotZ(CTeta, STeta, Mat);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to generate a 4*4 matrix to Rotate around the Z axis by Teta,      M
* given the sin and cosine of Teta                                           M
*                                                                            *
* PARAMETERS:                                                                M
*   SinTeta, CosTeta:  Amount of rotation, given as sine and cosine of Teta. M
*   Mat:               Matrix to initialize as a rotation matrix.            M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatGenMatRotZ, transformations, rotation                                 M
*****************************************************************************/
void MatGenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
    MatGenUnitMat(Mat);                              /* Make it unit matrix. */
    Mat[0][0] = CosTeta;
    Mat[0][1] = SinTeta;
    Mat[1][0] = -SinTeta;
    Mat[1][1] = CosTeta;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to multiply 2 4by4 matrices.                                       M
*  MatRes may be one of Mat1 or Mat2 - it is only updated in the end.        M
*                                                                            *
* PARAMETERS:                                                                M
*   MatRes:      Result of matrix product.                                   M
*   Mat1, Mat2:  The two operand of the matrix product.                      M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatMultTwo4by4, transformations, matrix product                          M
*****************************************************************************/
void MatMultTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
    int i, j, k;
    MatrixType MatResTemp;

    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++) {
	   MatResTemp[i][j] = 0;
	   for (k = 0; k < 4; k++)
	       MatResTemp[i][j] += Mat1[i][k] * Mat2[k][j];
	}
    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++)
	    MatRes[i][j] = MatResTemp[i][j];
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to add 2 4by4 matrices.					     M
*   MatRes may be one of Mat1 or Mat2.					     M
*                                                                            *
* PARAMETERS:                                                                M
*   MatRes:      Result of matrix addition.                                  M
*   Mat1, Mat2:  The two operand of the matrix addition.                     M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatAddTwo4by4, transformations, matrix addition                          M
*****************************************************************************/
void MatAddTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
    int i, j;

    for (i = 0; i < 4; i++)
        for (j = 0; j < 4; j++)
            MatRes[i][j] = Mat1[i][j] + Mat2[i][j];
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to subtract 2 4by4 matrices.					     M
*   MatRes may be one of Mat1 or Mat2.					     M
*                                                                            *
* PARAMETERS:                                                                M
*   MatRes:      Result of matrix subtraction.                               M
*   Mat1, Mat2:  The two operand of the matrix subtraction.                  M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatSubTwo4by4, transformations, matrix subtraction                       M
*****************************************************************************/
void MatSubTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
{
    int i, j;

    for (i = 0; i < 4; i++)
        for (j = 0; j < 4; j++)
	    MatRes[i][j] = Mat1[i][j] - Mat2[i][j];
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to scale a 4by4 matrix.					     M
*   MatRes may be Mat.							     M
*                                                                            *
* PARAMETERS:                                                                M
*   MatRes:      Result of matrix scaling.                                   M
*   Mat:         The two operand of the matrix scaling.                      M
*   Scale:       Scalar value to multiple matrix with.                       M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatScale4by4, transformations, matrix scaling                            M
*****************************************************************************/
void MatScale4by4(MatrixType MatRes, MatrixType Mat, RealType *Scale)
{
    int i, j;

    for (i = 0; i < 4; i++)
        for (j = 0; j < 4; j++)
	    MatRes[i][j] = Mat[i][j] * (*Scale);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to multiply an XYZ Vector by 4by4 matrix:                          M
*   The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1  M
*   VRes may be Vec as it is only updated in the end.                        M
*                                                                            *
* PARAMETERS:                                                                M
*   VRes:      Result of vector - matrix product.                            M
*   Vec:       Vector to transfrom using Matrix.                             M
*   Mat:       Transformation matrix.                                        M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatMultVecby4by4, transformations, vector matrix product                 M
*****************************************************************************/
void MatMultVecby4by4(VectorType VRes, VectorType Vec, MatrixType Mat)
{
    int i, j;
    RealType
	CalcW = Mat[3][3];
    VectorType VTemp;

    for (i = 0; i < 3; i++) {
	VTemp[i] = Mat[3][i];         /* Initiate it with the weight factor. */
	for (j = 0; j < 3; j++)
	    VTemp[i] += Vec[j] * Mat[j][i];
    }

    for (i = 0; i < 3; i++)
	CalcW += Vec[i] * Mat[i][3];
    if (CalcW == 0)
	CalcW = 1 / INFINITY;

    for (i = 0; i < 3; i++)
	VRes[i] = VTemp[i] / CalcW;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to multiply a WXYZ Vector by 4by4 matrix:                          M
*   The Vector has only 4 components (W, X, Y, Z).                           M
*   VRes may be Vec as it is only updated in the end.                        M
*                                                                            *
* PARAMETERS:                                                                M
*   VRes:      Result of vector - matrix product.                            M
*   Vec:       Vector to transfrom using Matrix.                             M
*   Mat:       Transformation matrix.                                        M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatMultWVecby4by4, transformations, vector matrix product                M
*****************************************************************************/
void MatMultWVecby4by4(RealType VRes[4], RealType Vec[4], MatrixType Mat)
{
    int i, j;
    RealType VTemp[4];

    for (i = 0; i < 4; i++) {
        VTemp[i] = 0.0;
        for (j = 0; j < 4; j++)
	    VTemp[i] += Vec[j] * Mat[j][i];
    }

    for (i = 0; i < 4; i++)
	VRes[i] = VTemp[i];
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Routine to compute the INVERSE of a given matrix M which is not modified.  M
*   The matrix is assumed to be 4 by 4 (transformation matrix).		     M
*   Return TRUE if inverted matrix (InvM) do exists.			     M
*                                                                            *
* PARAMETERS:                                                                M
*   M:          Original matrix to invert.                                   M
*   InvM:       Inverted matrix will be placed here.                         M
*                                                                            *
* RETURN VALUE:                                                              M
*   int:        TRUE if inverse exists, FALSE otherwise.                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MatInverseMatrix, transformations, matrix inverse                        M
*****************************************************************************/
int MatInverseMatrix(MatrixType M, MatrixType InvM)
{
    MatrixType A;
    int i, j, k;
    RealType V;

    MAT_COPY(A, M);			/* Prepare temporary copy of M in A. */
    MatGenUnitMat(InvM);			     /* Make it unit matrix. */

    for (i = 0; i < 4; i++) {
	V = A[i][i];				      /* Find the new pivot. */
	k = i;
	for (j = i + 1; j < 4; j++)
	    if (ABS(A[j][i]) > ABS(V)) {
	        /* Find maximum on col i, row i+1..n */
	        V = A[j][i];
	        k = j;
	    }
	j = k;

	if (i != j)
	    for (k = 0; k < 4; k++) {
		SWAP(RealType, A[i][k], A[j][k]);
		SWAP(RealType, InvM[i][k], InvM[j][k]);
            }

	for (j = i + 1; j < 4; j++) {	 /* Eliminate col i from row i+1..n. */
            V = A[j][i] / A[i][i];
	    for (k = 0; k < 4; k++) {
                A[j][k]    -= V * A[i][k];
                InvM[j][k] -= V * InvM[i][k];
	    }
	}
    }

    for (i = 3; i >= 0; i--) {			       /* Back Substitution. */
	if (A[i][i] == 0)
	    return FALSE;					   /* Error. */

	for (j = 0; j < i; j++) {	 /* Eliminate col i from row 1..i-1. */
            V = A[j][i] / A[i][i];
	    for (k = 0; k < 4; k++) {
                /* A[j][k] -= V * A[i][k]; */
                InvM[j][k] -= V * InvM[i][k];
	    }
	}
    }

    for (i = 0; i < 4; i++)		    /* Normalize the inverse Matrix. */
	for (j = 0; j < 4; j++)
            InvM[i][j] /= A[i][i];

    return TRUE;
}
