#include <stdio.h>
#include "matrix.h"
/****************************************************************/
/*								*/
/*	DAVID COLE		ME171 - matrix.h matrix.c	*/
/*					3/23/89			*/
/*								*/
/*	Repaired by Stephen Heise	3/15/93			*/
/*	Updated by Michael Scott        4/14/97			*/
/*								*/
/****************************************************************/


/************************************************/
/*						*/
/*	matrix.c				*/
/*				 		*/
/*	A  file for assorted matrix operations. */
/*      Functions are declared in matrix.h      */
/*	Note N = 4 (homogeneous transformations */
/*                                              */
/************************************************/


int invert_matrix (double mat[N][N],double inv[N][N])

/*** Returns a value of 1 if mat is invertable. ***/
/*** Returns a value of 0 if mat is not invertable. ***/
/*** inv exits this function as the inverse of mat ***/
/*** mat exits this function as the identity matrix ***/
/*** N is #defined to be 4***/
{
	double	m,temp[N][N];
	int	i,j,col,finder;

	for (i=0; i<N; ++i)
		for (j=0; j<N; ++j)
			temp[i][j] = mat[i][j];

	identity_matrix (inv);

	for (i=0; i<N; ++i)
	{	finder = i;
		if (temp[i][i]==0.0)	/*** Find a row with a non-zero ***/
					/*** ith element and add it to  ***/
					/*** the ith row.               ***/

		{	while ( (finder<N) && (temp[finder][i]==0.0) )
				++finder;
			if (finder<N)
			for (col=0; col<N; ++col)
			{	temp[i][col] += temp[finder][col];
				inv[i][col] += inv[finder][col];
			}
		}
		if (finder<N)
		{	m = temp[i][i];
			for (col=0; col<N; ++col)
			{	temp[i][col] /= m;
				inv[i][col] /= m;
			}
			for (j=i+1; j<N; ++j)
			{	m = - temp[j][i];
				for (col=0; col<N; ++col)
				{	temp[j][col] += ( temp[i][col] * m );
					inv[j][col] += ( inv[i][col] * m );
				}
			}
		}
		else printf ("\nMatrix uninvertable.");
	}

	if (finder<N)
	for (i=0; i<N; ++i)
		for (j=i+1; j<N; ++j)
		{	m = - temp[i][j];
			for (col=0; col<N; ++col)
			{	temp[i][col] += ( temp[j][col] * m );
				inv[i][col] += ( inv[j][col] * m );
			}
		}

	if (finder<N)
	     return (1);
	else return (0);

} /*** End invert_matrix () ***/

void identity_matrix (double I[N][N])

/*** N is #defined; m_size is global variable ***/
{
	int	row,col;
	for (row=0; row<N; ++row)
		for (col=0; col<N; ++col)
			if (row == col)
			I[row][col] = 1.0;
			else I[row][col] = 0.0;
} /*** End identity_matrix () ***/

void sq_mat_by_mat(double a[N][N],double b[N][N],double c[N][N])

/***    n       n      n  ***/
/*** n [c] = n [a]  n [b] ***/
/*** [c] is the result of     ***/
/*** [b] premultiplied by [a] ***/
{
	int	arow,acol,brow,bcol; /*** 0 <= arow,acol,brow,bcol <= n-1 ***/

	for (arow=0; arow<N; ++arow)
		for (bcol=0; bcol<N; ++bcol)
			for (c[arow][bcol]=0.0,acol=brow=0; acol<N; ++acol,++brow)
				c[arow][bcol] += a[arow][acol] * b[brow][bcol];

} /*** End sq_mat_by_mat() ***/



void row_by_mat(double rowin[N],double mat[N][N],double rowout[N])

/*** [rowout] = [rowin][mat] ***/
{
	int	matrow,col;	/*** 0 <=   matrow,col    <= n-1 ***/

	for (col=0; col<N; ++col)
		for (rowout[col]=0.0, matrow=0; matrow<N; ++matrow)
			rowout[col] += mat[matrow][col] * rowin[matrow];

} /*** End row_by_mat() ***/



void mat_by_col(double colin[N],double mat[N][N],double colout[N])

/*** [colout] = [mat][colin] ***/
{
	int	row,matcol;	/*** 0 <=   row,matcol    <= n-1 ***/

	for (row=0; row<N; ++row)
		for (colout[row]=0.0, matcol=0; matcol<N; ++matcol)
				colout[row] += mat[row][matcol] * colin[matcol];

} /*** End mat_by_col() ***/

double	dot_product(double x[4],double y[4])
{
	double	val;
	int	i;

	for (val=0.0, i=0; i<N; ++i)
		val += x[i]*y[i];
	return (val);
} /*** End of dot_product() ***/


/*** End of file ***/