/*************************************************************************/
/*                                                                       */
/*  Copyright (c) 1994 Stanford University                               */
/*                                                                       */
/*  All rights reserved.                                                 */
/*                                                                       */
/*  Permission is given to use, copy, and modify this software for any   */
/*  non-commercial purpose as long as this copyright notice is not       */
/*  removed.  All other uses, including redistribution in whole or in    */
/*  part, are forbidden without prior written permission.                */
/*                                                                       */
/*  This software is provided with absolutely no warranty and no         */
/*  support.                                                             */
/*                                                                       */
/*************************************************************************/


/*
 * NAME
 *	matrix.c
 *
 * DESCRIPTION
 *	This file contains routines for matrix math, including common graphics
 *	related matrix operations, and also some vector operations.
 */


#include <stdio.h>
#include <math.h>
#include "rt.h"


typedef REAL	GJMATRIX[4][8]; 	/* Matrix for Gauss-Jordan inversion.*/


/*
 * NAME
 *	VecNorm - normalize vector to unity length
 *
 * SYNOPSIS
 *	VOID	VecNorm(V)
 *	Point	V;			// Vector to normalize.
 *
 * RETURNS
 *	Nothing.
 */

VOID	VecNorm(V)
POINT	V;
	{
	REAL	l;

	l = VecLen(V);
	if (l > 0.0000001)
		{
		V[0] /= l;
		V[1] /= l;
		V[2] /= l;
		}
	}


/*
 * NAME
 *	VecMatMult - multiply a vector by a matrix
 *
 * SYNOPSIS
 *	VOID	VecMatMult(Vt, M, V)
 *	POINT	Vt;			// Transformed vector.
 *	MATRIX	M;			// Transformation matrix.
 *	POINT	V;			// Input vector.
 *
 * RETURNS
 *	Nothing.
 */

VOID	VecMatMult(Vt, M, V)
POINT	Vt;
MATRIX	M;
POINT	V;
	{
	INT	i, j;
	POINT	tvec;

	/* tvec = M * V */

	for (i = 0; i < 4; i++)
		{
		tvec[i] = 0.0;
		for (j = 0; j < 4; j++)
			tvec[i] += V[j] * M[j][i];
		}

	/* copy tvec to Vt */

	for (i = 0; i < 4; i++)
		Vt[i] = tvec[i];
	}


/*
 * NAME
 *	PrintMatrix - print values from matrix to stdout
 *
 * SYNOPSIS
 *	VOID	PrintMatrix(M, s)
 *	MATRIX	M;			// Matrix to print.
 *	CHAR	*s;			// Title string.
 *
 * RETURNS
 *	Nothing.
 */

VOID	PrintMatrix(M, s)
MATRIX	M;
CHAR	*s;
	{
	INT	i, j;

	printf("\n%s\n", s);

	for (i = 0; i < 4; i++)
		{
		printf("\t");

		for (j = 0; j < 4; j++)
			printf("%f  ", M[i][j]);

		printf("\n");
		}
	}


/*
 * NAME
 *	MatrixIdentity - set a matrix to the identity matrix
 *
 * SYNOPSIS
 *	VOID	MatrixIdentity(M)
 *	MATRIX	M;
 *
 * RETURNS
 *	Nothing.
 */

VOID	MatrixIdentity(M)
MATRIX	M;
	{
	INT	i, j;

	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++)
			M[i][j] = 0.0;

	M[0][0] = 1.0;
	M[1][1] = 1.0;
	M[2][2] = 1.0;
	M[3][3] = 1.0;
	}


/*
 * NAME
 *	MatrixCopy - copy one matrix to another
 *
 * SYNOPSIS
 *	VOID	MatrixCopy(A, B)
 *	MATRIX	A;			// Destination matrix.
 *	MATRIX	B;			// Source matrix.
 *
 * RETURNS
 *	Nothing.
 */

VOID	MatrixCopy(A, B)
MATRIX	A, B;
	{
	INT	i, j;

	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++)
			A[i][j] = B[i][j];
	}


/*
 * NAME
 *	MatrixTranspose - transpose the elements of a matrix
 *
 * SYNOPSIS
 *	VOID	MatrixTranspose(MT, M)
 *	MATRIX	MT;			// Transposed matrix.
 *	MATRIX	M;			// Original matrix.
 *
 * RETURNS
 *	Nothing.
 */

VOID	MatrixTranspose(MT, M)
MATRIX	MT;
MATRIX	M;
	{
	INT	i, j;
	MATRIX	tmp;

	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++)
			tmp[j][i] = M[i][j];

	MatrixCopy(MT, tmp);
	}


/*
 * NAME
 *	MatrixMult - multiply two matrices
 *
 * SYNOPSIS
 *	VOID	MatrixMult(C, A, B)
 *	MATRIX	C, A, B;		// C = A*B
 *
 * RETURNS
 *	Nothing.
 */

VOID	MatrixMult(C, A, B)
MATRIX	C, A, B;
	{
	INT	i, j, k;
	MATRIX	T;			/* Temporary matrix.		     */

	/* T = A*B */

	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++)
			{
			T[i][j] = 0.0;
			for (k = 0; k < 4; k++)
				T[i][j] += A[i][k] * B[k][j];
			}

	/* copy T to C */

	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++)
			C[i][j] = T[i][j];
	}


/*
 * NAME
 *	MatrixInverse - invert matrix using Gaussian elimination with partial pivoting
 *
 * SYNOPSIS
 *	VOID	MatrixInverse(Minv, Mat)
 *	MATRIX	Minv;			// Inverted matrix.
 *	MATRIX	Mat;			// Original matrix.
 *
 * RETURNS
 *	Nothing.
 */

VOID	MatrixInverse(Minv, Mat)
MATRIX	Minv;
MATRIX	Mat;
	{
	INT		i, j, k;	/* Indices.			     */
	GJMATRIX	gjmat;		/* Inverse calculator.		     */
	REAL		tbuf[8];	/* Row holder.			     */
	REAL		pval, aval;	/* Pivot candidates.		     */
	INT		prow;		/* Pivot row number.		     */
	REAL		c;		/* Pivot scale factor.		     */
	MATRIX		tmp;

	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++)
			gjmat[i][j] = Mat[i][j];


	k = 0;
	for (i = 4; i < 8; i++)
		{
		for (j = 4; j < 8; j++)
			if (i == j)
				gjmat[k][j] = 1.0;
			else
				gjmat[k][j] = 0.0;
		k++ ;
		}


	/* Gaussian elimination. */

	for (i = 0; i < 3; i++)
		{
		pval = ABS(gjmat[i][i]);
		prow = i;

		for (j = i + 1; j < 4; j++)
			{
			aval = ABS(gjmat[j][i]);
			if (aval > pval)
				{
				pval = aval;
				prow = j;
				}
			}

		if (i != prow)
			{
			for (k = 0; k < 8; k++)
				tbuf[k] = gjmat[i][k];

			for (k = 0; k < 8; k++)
				gjmat[i][k] = gjmat[prow][k];

			for (k = 0; k < 8; k++)
				gjmat[prow][k] = tbuf[k];
			}

		for (j = i + 1; j < 4; j++)
			{
			c = gjmat[j][i] / gjmat[i][i];
			gjmat[j][i] = 0.0;

			for (k = i + 1; k < 8; k++)
				gjmat[j][k] = gjmat[j][k] - c * gjmat[i][k];
			}
		}


	/*  Zero columns  */

	for (i = 0; i < 3; i++)
		for (j = i + 1; j < 4; j++)
			{
			c = gjmat[i][j] / gjmat[j][j];
			gjmat[i][j] = 0.0;

			for (k = j + 1; k < 8; k++)
				gjmat[i][k] = gjmat[i][k] - c * gjmat[j][k];
			}


	for (i = 0; i < 4; i++)
		for (k = 4; k < 8; k++) 		    /* normalize row */
			gjmat[i][k] /= gjmat[i][i];


	/*  Generate inverse matrix. */

	for (i = 0; i < 4; i++)
		for (j = 4; j < 8; j++)
			Minv[i][j - 4] = gjmat[i][j];

	MatrixMult(tmp, Mat, Minv);
	}


/*
 * NAME
 *	Translate - build translation matrix
 *
 * SYNOPSIS
 *	VOID	Translate(M, dx, dy, dz)
 *	MATRIX	M;			// New matrix.
 *	REAL	dx, dy, dz;		// Translation values.
 *
 * RETURNS
 *	Nothing.
 */

VOID	Translate(M, dx, dy, dz)
MATRIX	M;
REAL	dx, dy, dz;
	{
	MatrixIdentity(M);

	M[3][0] = dx;
	M[3][1] = dy;
	M[3][2] = dz;
	}


/*
 * NAME
 *	Scale - build scaling matrix
 *
 * SYNOPSIS
 *	VOID	Scale(M, sx, sy, sz)
 *	MATRIX	M;			// New matrix.
 *	REAL	sx, sy, sz;		// Scaling factors.
 *
 * RETURNS
 *	Nothing.
 */

VOID	Scale(M, sx, sy, sz)
MATRIX	M;
REAL	sx, sy, sz;
	{
	MatrixIdentity(M);

	M[0][0] = sx;
	M[1][1] = sy;
	M[2][2] = sz;
	}


/*
 * NAME
 *	Rotate - build rotation matrix
 *
 * SYNOPSIS
 *	VOID	Rotate(axis, M, angle)
 *	INT	axis;			// Axis of rotation.
 *	MATRIX	M;			// New matrix.
 *	REAL	angle;			// Angle of rotation.
 *
 * RETURNS
 *	Nothing.
 */

VOID	Rotate(axis, M, angle)
INT	axis;
MATRIX	M;
REAL	angle;
	{
	REAL	cosangle;
	REAL	sinangle;

	MatrixIdentity(M);

	cosangle = cos(angle);
	sinangle = sin(angle);

	switch (axis)
		{
		case X_AXIS:
			M[1][1] =  cosangle;
			M[1][2] =  sinangle;
			M[2][1] = -sinangle;
			M[2][2] =  cosangle;
			break;

		case Y_AXIS:
			M[0][0] =  cosangle;
			M[0][2] = -sinangle;
			M[2][0] =  sinangle;
			M[2][2] =  cosangle;
			break;

		case Z_AXIS:
			M[0][0] =  cosangle;
			M[0][1] =  sinangle;
			M[1][0] = -sinangle;
			M[1][1] =  cosangle;
			break;

		default:
			printf("Unknown rotation axis %ld.\n", axis);
			exit(5);
			break;
		}
	}