#include "matrix.h"
#include "database.h"

/*
 * Applies the transformation to point and returns the answer in result.
 * Transform is a 3*4 matrix of Reals, with the last column being the
 * translation.
 *
 * Params	trans		pointer to the transformation to apply
 *			p			the point to transform
 *			result		where to return the answer
 *
 * Globals	none
 *
 * Returns	the answer pointed to by result
 */
void applyTransform(t, p, result)
Transform *t;
Tupple p, *result;
{
	result->i = t->m[0][0]*p.i + t->m[0][1]*p.j + t->m[0][2]*p.k + t->m[0][3];
	result->j = t->m[1][0]*p.i + t->m[1][1]*p.j + t->m[1][2]*p.k + t->m[1][3];
	result->k = t->m[2][0]*p.i + t->m[2][1]*p.j + t->m[2][2]*p.k + t->m[2][3];
}

/*
 * given two 3x4 matrices, multiplies them together and returns the answer
 * in "r".  The fourth row is an implicit [0 0 0 1]
 *
 * Params	a, b	Transforms (3x4 matrices)
 *			r		where to put the result
 */
void matrixMult(a, b, r)
Transform *a, *b, *r;
{
	int i;

	for (i=0; i < 3; i++) {
		r->m[i][0] = a->m[i][0]*b->m[0][0] + a->m[i][1]*b->m[1][0]
						+ a->m[i][2]*b->m[2][0];
		r->m[i][1] = a->m[i][0]*b->m[0][1] + a->m[i][1]*b->m[1][1]
						+ a->m[i][2] * b->m[2][1];
		r->m[i][2] = a->m[i][0]*b->m[0][2] + a->m[i][1]*b->m[1][2] 
						+ a->m[i][2] * b->m[2][2];
		r->m[i][3] = a->m[i][0]*b->m[0][3] + a->m[i][1]*b->m[1][3] 
						+ a->m[i][2] * b->m[2][3] + a->m[i][3];
	}
}

/*
 * Finds the inverse of a square non-sigular matrix, based on the code in
 * "Applied Numerical Methods in C, Shoichiro Nakamura, 1993, PTR
 *  Prentice-Hall Inc, New Jersey"
 *
 *	"This program finds the inverse of a matrix A^-1 for a nonsingular square
 *	 matrix A.  The inverse of a matrix can be obtained as follows: Write A and
 *	 I (identity matrix) in an augmented forma as [A,I].  Then the inverse is
 *	 found where originally the identity matrix was written.  Pivoting does
 *	 not affect the inverse matrix cumputed.
 *		In the present program, Gauss eliniation is used rather than the
 *	 Gauss-Jordan elimination.  With Gauss elimination, each column of the
 *	 identity matrix originally in the augmented matrix is viewed as a set of
 *	 inhomogeneous terms.  Gauss elimination for each column is done not
 *	 separately but simultaneously.  When the Gauss eliniation is completed,
 *	 the columns for the orginal identity matrix are filled with the solution
 *	 of the Gauss elimination, which exactly comprises the inverse matrix."
 *
 * Params	t		Transform to be inverted (3x4)
 *			inv		Transmfor where answer will be stored
 *
 * Globals	none
 *
 * Returns	inverse in matrix i, and 1 on successful completion, else returns
 *			0.
 */
int matrixInvert(t, inv)
Transform *t, *inv;
{
	int i, j, jc, jr, k, kc, m, nv, pv;
	static Real eps, eps2;
	Real det, eps1, r, temp, tm, va;
	Real a[5][9];

	if (eps == 0.0) {
		eps = 1.0; eps1 = 1.0;
		while (eps1 > 0) {
			eps = eps / 2.0; eps1 = eps * 0.98 + 1.0; eps1 = eps1 - 1;
		}
		eps = eps * 2;
		/* printf("epsilon = %11.5e\n", eps); */
		eps2 = eps * 2;
	}

	/* set up matrix to work in */
	for (i=1; i<=4; i++)
		for (j=1; j<=8; j++)
			a[i][j] = 0.0;

	for (i=1; i < 4; i++) {
		for (j=1; j < 5; j++)
			a[i][j] = t->m[i-1][j-1];
		a[i][4+i] = 1.0;
	}
	a[4][4] = 1.0; a[4][8] = 1.0;

	det = 1.0;
	for (i=1; i < 4; i++) {
		pv = i;
		for (j=i+1; j <= 4; j++) {
			if (fabs(a[pv][i]) < fabs(a[j][i])) pv = j;
		}
		if (pv != i) {
			for (jc=1; jc <= 8; jc++) {
				tm = a[i][jc]; a[i][jc] = a[pv][jc]; a[pv][jc] = tm;
			}
			det = -det;
		}
		for (jr=i+1; jr <= 4; jr++) {
			if (a[jr][i] != 0.0) {
				r = a[jr][i]/a[i][i];
				for (kc=i+1; kc <= 8; kc++) {
					temp = a[jr][kc];
					a[jr][kc] = a[jr][kc] - r*a[i][kc];
					if (fabs(a[jr][kc]) < eps2*temp) a[jr][kc] = 0.0;
				}
			}
		}
	}

	for (i=1; i <= 4; i++) det = det * a[i][i];
	if (det == 0.0) return 0;
	
	/* printf("det = %11.5e\n", det); */
	if(a[4][4] != 0.0) {
		for (m=5; m <= 8; m++) {
			a[4][m] = a[4][m]/a[4][4];
			for (nv = 3; nv >= 1; nv--) {
				va = a[nv][m];
				for (k=nv+1; k <= 4; k++) va = va - a[nv][k]*a[k][m];
				a[nv][m] = va / a[nv][nv];
			}
		}

		/* copy inverse into inv */
		for (i=1; i <= 3; i++)
			for (j=5; j <= 8; j++)
				inv->m[i-1][j-5] = a[i][j];
		
		/* temporary debugging */
		/*
		for (i=1; i <= 4; i++) {
			for(j=5; j <= 8; j++)
				printf("%f ", a[i][j]);
			printf("\n");
		}
		*/
		return 1;
	}
}

/*
 * To transform the normal vector of a plane with loc2glob transform M, the
 * correct transform to apply is:
 *
 * N' = (M^-1)^T . N
 *
 * Params	trans		the inverse of loc2glob (glob2loc)
 *			n			the normal vector
 *			ng			where to return the results
 *
 * Globals	None
 *
 * Returns	the transformed normal in ng
 */
void transformNormal(trans, n, ng)
Transform *trans;
Tupple n, *ng;
{
	ng->i = trans->m[0][0] * n.i + trans->m[1][0] * n.j + trans->m[2][0] * n.k;
	ng->j = trans->m[0][1] * n.i + trans->m[1][1] * n.j + trans->m[2][1] * n.k;
	ng->k = trans->m[0][2] * n.i + trans->m[1][2] * n.j + trans->m[2][2] * n.k;

	normalize (ng);
}