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

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

/*****************************************************************************
* Routine to generate a 4*4 unit matrix:                                     *
*****************************************************************************/
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;
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts.       *
*****************************************************************************/
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;
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts.   *
*****************************************************************************/
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;
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
* Note Teta must be given in radians.                                        *
*****************************************************************************/
void MatGenMatRotX1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

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

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
*****************************************************************************/
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;
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
* Note Teta must be given in radians.                                        *
*****************************************************************************/
void MatGenMatRotY1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

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

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
*****************************************************************************/
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;
}

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
* Note Teta must be given in radians.                                        *
*****************************************************************************/
void MatGenMatRotZ1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

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

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
*****************************************************************************/
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;
}

/*****************************************************************************
* Routine to multiply 2 4by4 matrices:                                       *
* Note MatRes might be one of Mat1 or Mat2 - it is only updated in the end!  *
*****************************************************************************/
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];
}

/*****************************************************************************
* Routine to add 2 4by4 matrices:					     *
* Note MatRes might be one of Mat1 or Mat2.				     *
*****************************************************************************/
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];
}

/*****************************************************************************
* Routine to subtract 2 4by4 matrices:					     *
* Note MatRes might be one of Mat1 or Mat2.				     *
*****************************************************************************/
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];
}

/*****************************************************************************
* Routine to scale a 4by4 matrix by constant:				     *
* Note MatRes might be Mat.						     *
*****************************************************************************/
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);
}

/*****************************************************************************
* Routine to multiply Vector by 4by4 matrix:                                 *
* The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1    *
* Note VRes might be Vec as it is only updated in the end.                   *
*****************************************************************************/
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;
}

/*****************************************************************************
* Routine to multiply Vector by 4by4 matrix:                                 *
* The Vector has 4 components (X, Y, Z, W).				     *
* Note VRes might be Vec as it is only updated in the end.                   *
*****************************************************************************/
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];
}

/*****************************************************************************
* Procedure to calculate the INVERSE of a given matrix M which is not        *
* modified. The matrix is assumed to be 4 by 4 (transformation matrix).      *
* Return TRUE if inverted matrix (InvM) do exists.			     *
*****************************************************************************/
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;
}