/*
    Copyright 1999 by Nate 'm|d' Miller
    For non-commercial use only!

    File        -- matlib.c
    Date        -- 5/29/99
    Author      -- Nate 'm|d' Miller
    Contact     -- vandals1@home.com
    Web         -- http://members.home.com/vandals1

    Math routines that I used in various applictions.
*/
#include <math.h>
#include "mathlib.h"

// matrix functions
/*
    Function    -- matrixCopy
    Params      -- source and destination matrix
    Description -- copys matrix a into b
*/
void matrixCopy (matrix_t a, matrix_t b)
{
	int r, c;

	for (r = 0; r < 4; r ++)
		for (c = 0; c < 4; c ++)
			b[r * 4 + c] = a[r * 4 + c];
}
/*
    Function    -- matrixIdentity
    Params      -- A matrix
    Description -- loads the identity matrix
*/
void matrixIdentity (matrix_t p)
{
	#define M(row,col) p[row * 4 + col]

	M(0,0) = 1; M(0,1) = 0; M(0,2) = 0; M(0,3) = 0;
	M(1,0) = 0; M(1,1) = 1; M(1,2) = 0; M(1,3) = 0;
	M(2,0) = 0; M(2,1) = 0; M(2,2) = 1; M(2,3) = 0;
	M(3,0) = 0; M(3,1) = 0; M(3,2) = 0; M(3,3) = 1;

	#undef M
}
/*
    Function    -- matrixMultiply
    Params      -- 3 matricies, a * b = c
    Description -- multiplies 2 matricies together
*/
void matrixMultiply (matrix_t a, matrix_t b, matrix_t c)
{
	int x,y;
	
	#define A(row,col) a[row * 4 + col]
	#define B(row,col) b[row * 4 + col]
	#define C(row,col) c[row * 4 + col]

	for (x = 0; x < 4; x ++)
		for(y = 0; y < 4; y ++)
			C(x,y) = (A(x,0) * B(0,y)) + 
					(A(x,1) * B(1,y)) +
					(A(x,2) * B(2,y)) +
					(A(x,3) * B(3,y));
	#undef A
	#undef B
	#undef C
}
/*
    Function    -- matrixRotateX
    Params      -- matrix and angle
    Description -- rotates the matrix about the X axis
*/
void matrixRotateX (matrix_t p, vec_t angle)
{
	/*
	1     0        0         0
	0     cos(rx)  sin(rx)   0
	0     -sin(rx) cos(rx)   0
	0     0        0         1
	*/
	#define M(row,col) p[row * 4 + col]

	matrixIdentity (p);
	M(1,1) = (vec_t) cos (angle);
	M(1,2) = (vec_t) sin (angle);
	M(2,1) = (vec_t) -sin (angle);
	M(2,2) = (vec_t) cos (angle);

	#undef M
}
/*
    Function    -- matrixRotateY
    Params      -- matrix and angle
    Description -- rotates the matrix about the Y axis
*/
void matrixRotateY (matrix_t p, vec_t angle)
{
	/*
	cos(ry)  0     -sin(ry)   0
    0        1     0          0
    sin(ry)  0     cos(ry)    0
    0        0     0          1
	*/
	#define M(row,col) p[row * 4 + col]

	matrixIdentity (p);
	M(0,0) = (vec_t) cos (angle);
	M(0,2) = (vec_t) -sin (angle);
	M(2,0) = (vec_t) sin (angle);
	M(2,2) = (vec_t) cos (angle);

	#undef M
}
/*
    Function    -- matrixRotateZ
    Params      -- matrix and angle
    Description -- rotates the matrix about the Z axis
*/
void matrixRotateZ (matrix_t p, vec_t angle)
{
	/*
	cos(rz)  sin(rz)   0      0
    -sin(rz) cos(rz)   0      0
    0        0         1      0
    0        0         0      1
	*/
	#define M(row,col) p[row * 4 + col]

	matrixIdentity (p);
	M(0,0) = (vec_t) cos (angle);
	M(0,1) = (vec_t) sin (angle);
	M(1,0) = (vec_t) -sin (angle);
	M(1,1) = (vec_t) cos (angle);

	#undef M
}
/*
    Function    -- matrixRotate
    Params      -- matrix and array of containing x,y,z rotation angles
    Description -- rotates the matrix
*/
void matrixRotate (matrix_t m, vec3_t r)
{
	matrix_t x, y, z, t1, t2;

	matrixRotateX (x, degToRad (r[0]));
	matrixRotateY (y, degToRad (r[1]));
	matrixRotateZ (z, degToRad (r[2]));

	matrixMultiply (m,x,t1);
	matrixMultiply (t1,y,t2);
	matrixMultiply (t2,z,m);

}
/*
    Function    -- matrixMultiplyVector
    Params      -- matrix and vector
    Description -- multiplies a vector by a matrix
*/
void matrixMultiplyVector (matrix_t m, vec3_t v)
{
	vec3_t t;
	vec3_t n;

	vectorCopy (v, t);
	
	#define M(row,col) m[row * 4 + col]

	n[0] = t[0] * M(0,0) + t[1] * M(1,0) + t[2] * M(2,0);
	n[1] = t[0] * M(0,1) + t[1] * M(1,1) + t[2] * M(2,1);
	n[2] = t[0] * M(0,2) + t[1] * M(1,2) + t[2] * M(2,2);

	#undef M
  
	vectorCopy (n, v);
}
/*
    Function    -- matrixTranslate
    Params      -- matrix and vector
    Description -- translates a matrix
*/
void matrixTranslate (matrix_t m, vec3_t a)
{
	matrix_t t,d;
   
	#define M(row,col) t[row * 4 + col]

	matrixIdentity (t);

	M(3,0) = a[0]; 
	M(3,1) = a[1]; 
	M(3,2) = a[2];
	matrixMultiply (m,t,d);
	matrixCopy (d,m);

	#undef M
}
// vector stuff
/*
    Function    -- vectorClear
    Params      -- vector
    Description -- clears a vector
*/
void vectorClear (vec3_t a)
{
	a[0] = a[1] = a[2] = 0.0;
}
/*
    Function    -- vectorCopy
    Params      -- 2 vectors
    Description -- copys vector a into b
*/
void vectorCopy (vec3_t a, vec3_t b)
{
	b[0] = a[0]; 
	b[1] = a[1];
	b[2] = a[2];
}
/*
    Function    -- vectorCrossProduct
    Params      -- 2 vectors and one out vector
    Description -- computes cross product of 2 vectors
*/
void vectorCrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
{
	cross[0] = v1[1] * v2[2] - v1[2] * v2[1];
	cross[1] = v1[2] * v2[0] - v1[0] * v2[2];
	cross[2] = v1[0] * v2[1] - v1[1] * v2[0];
}
/*
    Function    -- vectorDot
    Params      -- 2 vectors 
    Description -- returns the dot product of 2 vectors
*/
vec_t vectorDot (vec3_t v1, vec3_t v2)
{
	return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}
/*
    Function    -- vectorSubtract
    Params      -- 2 vectors and one out vector
    Description -- subtracts vector b from a
*/
void vectorSubtract (vec3_t va, vec3_t vb, vec3_t out)
{
	out[0] = va[0] - vb[0];
	out[1] = va[1] - vb[1];
	out[2] = va[2] - vb[2];
}
/*
    Function    -- vectorAdd
    Params      -- 2 vectors and one out vector
    Description -- adds two vectors together
*/
void vectorAdd (vec3_t va, vec3_t vb, vec3_t out)
{
	out[0] = va[0] + vb[0];
	out[1] = va[1] + vb[1];
	out[2] = va[2] + vb[2];
}
/*
    Function    -- vectorNormalize
    Params      -- normals vector and normalized out vector
    Description -- returns length, and puts normalized vector in out
*/
vec_t vectorNormalize (vec3_t in, vec3_t out)
{
	vec_t	length, ilength;

	length = (float)sqrt (in[0] * in[0] + in[1] * in[1] + in[2] * in[2]);

	if (length == 0)
	{
		vectorClear (out);
		return 0;
	}

	ilength = (float)1.0 / length;
	out[0] = in[0] * ilength;
	out[1] = in[1] * ilength;
	out[2] = in[2] * ilength;

	return length;
}
/*
    Function    -- vectorScaleV
    Params      -- vector and scaling vector
    Description -- scales a vector by another
*/
void vectorScaleV (vec3_t a, vec3_t s)
{
	a[0] *= s[0];
	a[1] *= s[1];
	a[2] *= s[2];
}
/*
    Function    -- vectorScale
    Params      -- vector and scaling factor
    Description -- scales a vector
*/
void vectorScale (vec3_t v, vec_t scale)
{
	v[0] *= scale;
	v[1] *= scale;
	v[2] *= scale;
}

