#include <math.h>

/*#define fpa */


identity(mat) 
	float mat[4][4];
{
	fill_mat(mat, 0.0);
	mat[0][0] = mat[1][1] = mat[2][2] = mat[3][3] = 1.0;
}

mat_cp(src, dst) 
	float src[4][4], dst[4][4];
{
	register short i;
	register float *psrc = &src[0][0];
	register float *pdst = &dst[0][0];

	for (i=0; i<16; i++)
		*pdst++ = *psrc++;
}

my_fill_mat(mat, val,dimx,dimy) 
	float *mat;
	float val;
	int dimx,dimy;
{
	register int n;
	register float *m;
	register float v;

	v = val;
	n = dimx * dimy;
	m = mat;
	while (n--) {
		*m = v;
		m++;
	}
}

fill_mat(mat, val) 
	float mat[4][4];
	float val;
{
	register short i;
	register float *pmat = &mat[0][0];

	for (i=0; i<16; i++)
		*pmat++ = val;
}

/*
 * we make a number of assumptions in the name of speed here:
 * We assume that A,B,C will always be Nx4 matricies.
 * Furthermore, if B is a 4x4, then we have a special routine
 *  (mxml4) which is blindingly fast -- but only for the FPA.
 */

#ifdef fpa

my_mat_mult(A, B, C,dima,dimb)
	float	A[][4];
	float	B[][4];
	float	C[][4]; 
	register int dima, dimb;
{
	float	dot4();
	register int i;
	register float *dst;
	register float *avec;
	register float *bvec;

	if (dimb == 4) {
		transform4(A, B, C, dima);
		return;
	}
	/*
	 * C[i][j] = dotp(&A[i][0], &B[0][j]);
	 */
	dst = C[0];
	avec = A[0];
	bvec = B[0];
	for (i = 0; i < dima; i++) {
		if (dimb == 4) {
			mxml4(avec, bvec, dst);
			dst += dimb;
		} else 
		{
			register int j;

			bvec = B[0];
			for (j = 0; j < dimb; j++) {
				*dst++ = dot4(avec, bvec);
				bvec++;
			}
		}
		avec += dimb;
	}
}

#else

my_mat_mult(A, B, C,dima,dimb)
	float A[][4];
	float B[][4]; 
	float C[][4]; 
	int dima;
	int dimb;
{
	register short i, j, k;

	my_fill_mat(C, 0.0,dima,dimb);

	for (i = 0; i < dima; i++) {
		for (j = 0; j < dimb; j++) {
			for (k = 0; k < 4; k++) {
				C[i][j] += A[i][k] * B[k][j];
			}
		}
	}
}

#endif

mat_mult(A, B, C)
	float A[4][4], B[4][4], C[4][4]; 
{
	register short i, j, k;

	fill_mat(C, 0.0);

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