//vectors.cpp
//vector and matrix math functions

#include "vectors.h"
#include "gamemem.h"
#include <math.h>
//Add and Subtract Matrices
void MatrixAdd(tMatrix3 x,tMatrix3 y,tMatrix3 m)
{
	int i,j;
	for(i=0;i<3;i++)
		for(j=0;j<3;j++)
			m[i][j]=x[i][j]+y[i][j];
}

void MatrixAdd(tMatrix4 x,tMatrix4 y,tMatrix4 m)
{
	int i,j;
	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
			m[i][j]=x[i][j]+y[i][j];
}

void MatrixSub(tMatrix3 x,tMatrix3 y,tMatrix3 m)
{
	int i,j;
	for(i=0;i<3;i++)
		for(j=0;j<3;j++)
			m[i][j]=x[i][j]-y[i][j];
}

void MatrixSub(tMatrix4 x,tMatrix4 y,tMatrix4 m)
{
	int i,j;
	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
			m[i][j]=x[i][j]-y[i][j];
}

//Multiplay Matrices with Scalars
void MatrixMult(tMatrix3 x,float y,tMatrix3 m)
{
	int i,j;
	for(i=0;i<3;i++)
		for(j=0;j<3;j++)
			m[i][j]=x[i][j]*y;
}

void MatrixMult(tMatrix4 x,float y,tMatrix4 m)
{
	int i,j;
	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
			m[i][j]=x[i][j]*y;
}

void MatrixMult(float y,tMatrix3 x,tMatrix3 m)
{
	int i,j;
	for(i=0;i<3;i++)
		for(j=0;j<3;j++)
			m[i][j]=x[i][j]*y;
}

void MatrixMult(float y,tMatrix4 x,tMatrix4 m)
{
	int i,j;
	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
			m[i][j]=x[i][j]*y;
}

//Convert a 3x3 matrix to a 4x4 matrix
void Matrix3ToMatrix4(tMatrix3 x,tMatrix4 m)
{
	m[0][0]=x[0][0];m[0][1]=x[0][1];m[0][2]=x[0][2];m[0][3]=0;
	m[1][0]=x[1][0];m[1][1]=x[1][1];m[1][2]=x[1][2];m[1][3]=0;
	m[2][0]=x[2][0];m[2][1]=x[2][1];m[2][2]=x[2][2];m[2][3]=0;
	m[3][0]=0;		m[3][1]=0;		m[3][2]=0;		m[3][3]=1;
}

//Copy matrices
void MatrixCopy(tMatrix3 src,tMatrix3 dst)
{
	MemoryMove(dst,src,sizeof(tMatrix3));
}

void MatrixCopy(tMatrix4 src,tMatrix4 dst)
{
	MemoryMove(dst,src,sizeof(tMatrix4));
}

void MatrixCopy(tMatrix3 src,tMatrix4 dst)
{
	Matrix3ToMatrix4(src,dst);
}

//Convert a vector of euler angles to a matrix
void EulerAnglesToMatrix(tVector3 euler,tMatrix3 dst)
{
	MatrixIdentity(dst);
	MatrixRotX(dst,euler.x);
	MatrixRotY(dst,euler.y);
	MatrixRotZ(dst,euler.z);
}

void EulerAnglesToMatrix(tVector3 euler,tMatrix4 dst)
{
	MatrixIdentity(dst);
	MatrixRotX(dst,euler.x);
	MatrixRotY(dst,euler.y);
	MatrixRotZ(dst,euler.z);
}

//Convert a matrix to a vector of euler angles
tVector3 MatrixToEulerAngles(tMatrix3 m)
{
	tVector3 euler;
	if(fabs(m[0][2])<=1)
		euler.y=-asin(m[0][2]);
	else
		euler.y=-asin(sign(m[0][2]));
	float C=cos(euler.y);
	float trX,trY;
	if(fabs(C)>0.005) 
	{
		trX=m[2][2]/C;
		trY=-m[1][2]/C;
		
		euler.x=-atan2(trY,trX);

		trX=m[0][0]/C;
		trY=-m[0][1]/C;
		
		euler.z=-atan2(trY,trX);
	}
	else //Gimbal Lock
	{
		euler.x=0;

		trX=m[1][1];
		trY=m[1][0];
		
		euler.z=atan2(trY,trX);		
	}
	return euler;
}

tVector3 MatrixToEulerAngles(tMatrix4 m)
{
	tVector3 euler;
	euler.y=-asin(m[0][2]);
	float C=cos(euler.y);
	float trX,trY;
	if(fabs(C)>0.005) 
	{
		trX=m[2][2]/C;
		trY=-m[1][2]/C;
		
		euler.x=-atan2(trY,trX);

		trX=m[0][0]/C;
		trY=-m[0][1]/C;
		
		euler.z=-atan2(trY,trX);
	}
	else //Gimbal Lock
	{
		euler.x=0;

		trX=m[1][1];
		trY=m[1][0];
		
		euler.z=atan2(trY,trX);		
	}
	return euler;
}

//Multiply matrices
void MatrixMult(tMatrix3 x,tMatrix3 y,tMatrix3 m)
{
	int i,j,k;
	if(x==m||y==m){
		tMatrix3 target;
		for(j=0;j<3;j++)
		{
			target[0][j]=x[0][0]*y[0][j]
						+x[0][1]*y[1][j]
						+x[0][2]*y[2][j];
			target[1][j]=x[1][0]*y[0][j]
						+x[1][1]*y[1][j]
						+x[1][2]*y[2][j];
			target[2][j]=x[2][0]*y[0][j]
						+x[2][1]*y[1][j]
						+x[2][2]*y[2][j];
		}
		MemoryMove(m,target,sizeof(tMatrix3));
	}
	else		
		for(j=0;j<3;j++)
		{
			m[0][j]=x[0][0]*y[0][j]
					+x[0][1]*y[1][j]
					+x[0][2]*y[2][j];
			m[1][j]=x[1][0]*y[0][j]
					+x[1][1]*y[1][j]
					+x[1][2]*y[2][j];
			m[2][j]=x[2][0]*y[0][j]
					+x[2][1]*y[1][j]
					+x[2][2]*y[2][j];
		}
}

void MatrixMult(tMatrix3 x,tMatrix4 y,tMatrix4 m)
{
	tMatrix4 x4;
	Matrix3ToMatrix4(x,x4);
	MatrixMult(x4,y,m);
}

void MatrixMult(tMatrix4 x,tMatrix3 y,tMatrix4 m)
{
	tMatrix4 y4;
	Matrix3ToMatrix4(y,y4);
	MatrixMult(x,y4,m);
}

void MatrixMult(tMatrix4 x,tMatrix4 y,tMatrix4 m)
{
	int i,j,k;
	if(x==m||y==m){
		tMatrix4 target;
		for(j=0;j<4;j++)
		{
			target[0][j]=x[0][0]*y[0][j]
						+x[0][1]*y[1][j]
						+x[0][2]*y[2][j]
						+x[0][3]*y[3][j];
			target[1][j]=x[1][0]*y[0][j]
						+x[1][1]*y[1][j]
						+x[1][2]*y[2][j]
						+x[1][3]*y[3][j];
			target[2][j]=x[2][0]*y[0][j]
						+x[2][1]*y[1][j]
						+x[2][2]*y[2][j]
						+x[2][3]*y[3][j];
			target[3][j]=x[3][0]*y[0][j]
						+x[3][1]*y[1][j]
						+x[3][2]*y[2][j]
						+x[3][3]*y[3][j];
		}
		MemoryMove(m,target,sizeof(tMatrix4));
	}
	else		
	{
		for(j=0;j<4;j++)
		{
			m[0][j]=x[0][0]*y[0][j]
					+x[0][1]*y[1][j]
					+x[0][2]*y[2][j]
					+x[0][3]*y[3][j];
			m[1][j]=x[1][0]*y[0][j]
					+x[1][1]*y[1][j]
					+x[1][2]*y[2][j]
					+x[1][3]*y[3][j];
			m[2][j]=x[2][0]*y[0][j]
					+x[2][1]*y[1][j]
					+x[2][2]*y[2][j]
					+x[2][3]*y[3][j];
			m[3][j]=x[3][0]*y[0][j]
					+x[3][1]*y[1][j]
					+x[3][2]*y[2][j]
					+x[3][3]*y[3][j];
		}
	}
}

//makes an identity matrix
void MatrixIdentity(tMatrix3 m)
{
	m[0][0]=1;m[0][1]=0;m[0][2]=0;
	m[1][0]=0;m[1][1]=1;m[1][2]=0;
	m[2][0]=0;m[2][1]=0;m[2][2]=1;
}

void MatrixIdentity(tMatrix4 m)
{
	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;
}

//Translates a matrix
void MatrixTranslate(tMatrix4 target,float x,float y,float z)
{
	tMatrix4 m;
	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]=x;m[3][1]=y;m[3][2]=z;m[3][3]=1;
	MatrixMult(target,m,target);
}

void MatrixTranslateVector(tMatrix4 target,tVector3 v)
{
	tMatrix4 m;
	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]=v.x;m[3][1]=v.y;m[3][2]=v.z;m[3][3]=1;
	MatrixMult(target,m,target);
}

//Scales a matrix
void MatrixScale(tMatrix3 target,float x,float y,float z)
{
	tMatrix3 m;
	m[0][0]=x;m[0][1]=0;m[0][2]=0;
	m[1][0]=0;m[1][1]=y;m[1][2]=0;
	m[2][0]=0;m[2][1]=0;m[2][2]=z;
	MatrixMult(target,m,target);
}

void MatrixScale(tMatrix4 target,float x,float y,float z)
{
	tMatrix4 m;
	m[0][0]=x;m[0][1]=0;m[0][2]=0;m[0][3]=0;
	m[1][0]=0;m[1][1]=y;m[1][2]=0;m[1][3]=0;
	m[2][0]=0;m[2][1]=0;m[2][2]=z;m[2][3]=0;
	m[3][0]=0;m[3][1]=0;m[3][2]=0;m[3][3]=1;
	MatrixMult(target,m,target);
}

void MatrixScaleVector(tMatrix3 target,tVector3 v)
{
	tMatrix3 m;
	m[0][0]=v.x;m[0][1]=0;m[0][2]=0;
	m[1][0]=0;m[1][1]=v.y;m[1][2]=0;
	m[2][0]=0;m[2][1]=0;m[2][2]=v.z;
	MatrixMult(target,m,target);
}

void MatrixScaleVector(tMatrix4 target,tVector3 v)
{
	tMatrix4 m;
	m[0][0]=v.x;m[0][1]=0;m[0][2]=0;m[0][3]=0;
	m[1][0]=0;m[1][1]=v.y;m[1][2]=0;m[1][3]=0;
	m[2][0]=0;m[2][1]=0;m[2][2]=v.z;m[2][3]=0;
	m[3][0]=0;m[3][1]=0;m[3][2]=0;m[3][3]=1;
	MatrixMult(target,m,target);
}

//Rotates a matrix
void MatrixRotX(tMatrix3 target,float r)
{
	tMatrix3 m;
	m[0][0]=1;m[0][1]=0;m[0][2]=0;
	m[1][0]=0;m[1][1]=cos(r);m[1][2]=sin(r);
	m[2][0]=0;m[2][1]=-sin(r);m[2][2]=cos(r);
	MatrixMult(target,m,target);
}

void MatrixRotX(tMatrix4 target,float r)
{
	tMatrix4 m;
	m[0][0]=1;m[0][1]=0;m[0][2]=0;m[0][3]=0;
	m[1][0]=0;m[1][1]=cos(r);m[1][2]=sin(r);m[1][3]=0;
	m[2][0]=0;m[2][1]=-sin(r);m[2][2]=cos(r);m[2][3]=0;
	m[3][0]=0;m[3][1]=0;m[3][2]=0;m[3][3]=1;
	MatrixMult(target,m,target);
}

void MatrixRotY(tMatrix3 target,float r)
{
	tMatrix3 m;
	m[0][0]=cos(r);m[0][1]=0;m[0][2]=-sin(r);
	m[1][0]=0;m[1][1]=1;m[1][2]=0;
	m[2][0]=sin(r);m[2][1]=0;m[2][2]=cos(r);
	MatrixMult(target,m,target);
}

void MatrixRotY(tMatrix4 target,float r)
{
	tMatrix4 m;
	m[0][0]=cos(r);m[0][1]=0;m[0][2]=-sin(r);m[0][3]=0;
	m[1][0]=0;m[1][1]=1;m[1][2]=0;m[1][3]=0;
	m[2][0]=sin(r);m[2][1]=0;m[2][2]=cos(r);m[2][3]=0;
	m[3][0]=0;m[3][1]=0;m[3][2]=0;m[3][3]=1;
	MatrixMult(target,m,target);
}

void MatrixRotZ(tMatrix3 target,float r)
{
	tMatrix3 m;
	m[0][0]=cos(r);m[0][1]=sin(r);m[0][2]=0;
	m[1][0]=-sin(r);m[1][1]=cos(r);m[1][2]=0;
	m[2][0]=0;m[2][1]=0;m[2][2]=1;
	MatrixMult(target,m,target);
}

void MatrixRotZ(tMatrix4 target,float r)
{
	tMatrix4 m;
	m[0][0]=cos(r);m[0][1]=sin(r);m[0][2]=0;m[0][3]=0;
	m[1][0]=-sin(r);m[1][1]=cos(r);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;
	MatrixMult(target,m,target);
}

//Transposes a matrix
void MatrixTranspose(tMatrix3 x,tMatrix3 dst)
{
	int i,j;
	for(i=0;i<3;i++)
		for(j=0;j<3;j++)
			dst[i][j]=x[j][i];
}

void MatrixTranspose(tMatrix4 x,tMatrix4 dst)
{
	int i,j;
	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
			dst[i][j]=x[j][i];
}

//Verifies if a matrix is a valid rotation matrix (Orthonormal base).
int MatrixVerify(tMatrix3 m)
{
	tVector3 *m0=(tVector3*)m[0];
	float len=m0->x*m0->x+m0->y*m0->y+m0->z*m0->z;
	if(len>=1+kMatrixTolerance||len<=1-kMatrixTolerance)return false;
	tVector3 v=(*(tVector3*)m[1]%*(tVector3*)m[2])-*(tVector3*)m[0];
	return(v.x*v.x+v.y*v.y+v.z*v.z<=kMatrixTolerance);
}

int MatrixVerify(tMatrix4 m)
{
	tVector3 v=(*(tVector3*)m[1]%*(tVector3*)m[2])-*(tVector3*)m[0];
	return(v.x*v.x+v.y*v.y+v.z*v.z<=kMatrixTolerance);
}


//Readjusts matrices to be a valid rotation matrix
//needed as floating point calculations always carry small errors, so matrices
//will drift apart more and more
void MatrixReAdjust(tMatrix3 m)
{
	*(tVector3*)m[2]=!*(tVector3*)m[2];
	*(tVector3*)m[0]=!(*(tVector3*)m[1]%*(tVector3*)m[2]);
	*(tVector3*)m[1]=!(*(tVector3*)m[2]%*(tVector3*)m[0]);
}

void MatrixReAdjust(tMatrix4 m)
{
	*(tVector3*)m[2]=!*(tVector3*)m[2];
	*(tVector3*)m[0]=!(*(tVector3*)m[1]%*(tVector3*)m[2]);
	*(tVector3*)m[1]=!(*(tVector3*)m[2]%*(tVector3*)m[0]);
}

//converts a rotation vector to a rotation matrix.
//a rotation vector defines a rotation by the length of a vector
//around the axis given by the direction of a vector.
void RotationVectorToMatrix(tVector3 axis,tMatrix4 mat) 
{
	float angle=~axis;
	if(!angle)
	{
		MatrixIdentity(mat);
		return;
	}
	else axis=axis/angle;
	tMatrix3 axist;
	int i,j;
	for(i=0;i<3;i++)
		for(j=0;j<3;j++)
			axist[i][j]=((float*)&axis)[i]*((float*)&axis)[j];
	tMatrix3 m1,m2,s;
	s[0][0]=0;		s[0][1]=-axis.z;	s[0][2]=axis.y;
	s[1][0]=axis.z;	s[1][1]=0;			s[1][2]=-axis.x;
	s[2][0]=-axis.y;s[2][1]=axis.x;		s[2][2]=0;
	MatrixIdentity(m1);
	MatrixSub(m1,axist,m1);
	MatrixMult(m1,cos(-angle),m1);
	MatrixAdd(m1,axist,m1);
	MatrixMult(s,sin(-angle),m2);
	MatrixAdd(m1,m2,m1);
	Matrix3ToMatrix4(m1,mat);
}

void RotationVectorToMatrix(tVector3 axis,tMatrix3 mat) 
{
	float angle=~axis;
	if(!angle)
	{
		MatrixIdentity(mat);
		return;
	}
	else axis=axis/angle;
	tMatrix3 axist;
	int i,j;
	for(i=0;i<3;i++)
		for(j=0;j<3;j++)
			axist[i][j]=((float*)&axis)[i]*((float*)&axis)[j];
	tMatrix3 m1,m2,s;
	s[0][0]=0;		s[0][1]=-axis.z;	s[0][2]=axis.y;
	s[1][0]=axis.z;	s[1][1]=0;			s[1][2]=-axis.x;
	s[2][0]=-axis.y;s[2][1]=axis.x;		s[2][2]=0;
	MatrixIdentity(m1);
	MatrixSub(m1,axist,m1);
	MatrixMult(m1,cos(-angle),m1);
	MatrixAdd(m1,axist,m1);
	MatrixMult(s,sin(-angle),m2);
	MatrixAdd(m1,m2,mat);
}


//tests if two vectors are equal
int VectorEqual(tVector3 a,tVector3 b)
{
	return(a.x==b.x&&a.y==b.y&&a.z==b.z);
}

int VectorEqual(tVector2 a,tVector2 b)
{
	return(a.x==b.x&&a.y==b.y);
}

//tests if a vector is zero
int VectorZero(tVector2 v)
{
	return v.x==0&&v.y==0;
}

int VectorZero(tVector3 v)
{
	return v.x==0&&v.y==0&&v.z==0;
}