/*****************************************************************************
* 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 GenUnitMat(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 GenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
{
     GenUnitMat(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 GenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
{
     GenUnitMat(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 GenMatRotX1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

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

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
*****************************************************************************/
void GenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
     GenUnitMat(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 GenMatRotY1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

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

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
*****************************************************************************/
void GenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
     GenUnitMat(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 GenMatRotZ1(RealType Teta, MatrixType Mat)
{
    RealType CTeta, STeta;

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

/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
*****************************************************************************/
void GenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
     GenUnitMat(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 MultTwo4by4(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 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 MultVecby4by4(RealType VRes[3], RealType Vec[3], MatrixType Mat)
{
    int i, j;
    RealType CalcW = Mat[3][3];
    RealType VTemp[3];

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