#include <../source/TAL/objbase.h>

/* Scale matrix */
void	V_GLOBAL
mat3Scale(matrix3_t m,vec3_t scale)
{
	mat3Zero(m);
	m[0][0]=scale[0];
	m[1][1]=scale[1];
	m[2][2]=scale[2];
}

/* Rotation matrices */
void	V_GLOBAL
mat3RotateX(matrix3_t mat,vec_t radians)
{
	vec_t c,s;
	v_fsincos(&s,&c,radians);
	mat[0][0] = 1; mat[0][1] = 0; mat[0][2] = 0;
	mat[1][0] = 0; mat[1][1] = c; mat[1][2] = -s;
	mat[2][0] = 0; mat[2][1] = s; mat[2][2] = c;
}

void	V_GLOBAL
mat3RotateY(matrix3_t mat,vec_t radians)
{
	vec_t c,s;
	v_fsincos(&s,&c,radians);
	mat[0][0] = c; mat[0][1] = 0; mat[0][2] = -s;
	mat[1][0] = 0; mat[1][1] = 1; mat[1][2] = 0;
	mat[2][0] = s; mat[2][1] = 0; mat[2][2] = c;
}

void	V_GLOBAL
mat3RotateZ(matrix3_t mat,vec_t radians)
{
	vec_t c,s;
	v_fsincos(&s,&c,radians);
	mat[0][0] = c; mat[0][1] = -s; mat[0][2] = 0;
	mat[1][0] = s; mat[1][1] = c; mat[1][2] = 0;
	mat[2][0] = 0; mat[2][1] = 0; mat[2][2] = 1;
}

/*
 *	mat3Rotate - computes arbitrary axis rotation matrix.
 *	Original code: MesaGL
 *	NOTES.
 *		1. Assuming that 'dir' vector is normalized.
 *		2. Matrix rotates obects COUNTERCLOCKWISE, if look towards origin
 *		of 'dir'.
 */
void	V_GLOBAL
mat3Rotate(matrix3_t mat,vec3_t n,vec_t angle)
{
	vec_t s, c;
	vec_t xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
	v_fsincos(&s,&c,angle);
	/* Arbitrary axis rotation matrix.
	 * Author: Erich Boleyn (erich@uruk.org); see MesaGL code. */
	xx = n[0]*n[0];
	yy = n[1]*n[1];
	zz = n[2]*n[2];
	xy = n[0]*n[1];
	yz = n[1]*n[2];
	zx = n[2]*n[0];
	xs = n[0]*s;
	ys = n[1]*s;
	zs = n[2]*s;
	one_c = 1.0F - c;
	mat[0][0] = (one_c * xx) + c;
	mat[0][1] = (one_c * xy) - zs;
	mat[0][2] = (one_c * zx) + ys;
	mat[1][0] = (one_c * xy) + zs;
	mat[1][1] = (one_c * yy) + c;
	mat[1][2] = (one_c * yz) - xs;
	mat[2][0] = (one_c * zx) - ys;
	mat[2][1] = (one_c * yz) + xs;
	mat[2][2] = (one_c * zz) + c;
}

/* Computes  view matrix based on camera's 'dir' and 'up'.
 *	Assumes dir and up are normalized */
void	V_GLOBAL
mat3FromDirAndUp(matrix3_t mat,vec3_t dir,vec3_t up)
{
	vec3_t left;
	vec3Cross(left,up,dir);
	mat3SetRows(mat,left,up,dir);
}

/* Scale matrix */
void	V_GLOBAL
mat3x4Scale(matrix3x4_t m,vec3_t scale)
{
	mat3x4Zero(m);
	m[0][0]=scale[0];
	m[1][1]=scale[1];
	m[2][2]=scale[2];
}

/* Translation matrix */
void	V_GLOBAL
mat3x4Translate(matrix3x4_t m,vec3_t t)
{
	mat3x4Unit(m);
	m[0][3]=t[0];
	m[1][3]=t[1];
	m[2][3]=t[2];
}

/* Rotation matrices */
void	V_GLOBAL
mat3x4RotateX(matrix3x4_t mat,vec_t radians)
{
	vec_t c,s;
	v_fsincos(&s,&c,radians);
	mat[0][0] = 1; mat[0][1] = 0; mat[0][2] = 0;  mat[0][3] = 0;
	mat[1][0] = 0; mat[1][1] = c; mat[1][2] = -s; mat[1][3] = 0;
	mat[2][0] = 0; mat[2][1] = s; mat[2][2] = c;  mat[2][3] = 0;
}

void	V_GLOBAL
mat3x4RotateY(matrix3x4_t mat,vec_t radians)
{
	vec_t c,s;
	v_fsincos(&s,&c,radians);
	mat[0][0] = c; mat[0][1] = 0; mat[0][2] = -s; mat[0][3] = 0;
	mat[1][0] = 0; mat[1][1] = 1; mat[1][2] = 0;  mat[1][3] = 0;
	mat[2][0] = s; mat[2][1] = 0; mat[2][2] = c;  mat[2][3] = 0;
}

void	V_GLOBAL
mat3x4RotateZ(matrix3x4_t mat,vec_t radians)
{
	vec_t c,s;
	v_fsincos(&s,&c,radians);
	mat[0][0] = c; mat[0][1] = -s; mat[0][2] = 0; mat[0][3] = 0;
	mat[1][0] = s; mat[1][1] = c;  mat[1][2] = 0; mat[1][3] = 0;
	mat[2][0] = 0; mat[2][1] = 0;  mat[2][2] = 1; mat[2][3] = 0;
}

/*
 *	See notes for mat3Rotate
 */
void	V_GLOBAL
mat3x4Rotate(matrix3x4_t mat,vec3_t n,vec_t angle)
{
	 vec_t s, c;
	 vec_t xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
	 v_fsincos(&s,&c,angle);
	 xx = n[0]*n[0];
	 yy = n[1]*n[1];
	 zz = n[2]*n[2];
	 xy = n[0]*n[1];
	 yz = n[1]*n[2];
	 zx = n[2]*n[0];
	 xs = n[0]*s;
	 ys = n[1]*s;
	 zs = n[2]*s;
	 one_c = 1.0F - c;
	 mat[0][0] = (one_c * xx) + c;
	 mat[0][1] = (one_c * xy) - zs;
	 mat[0][2] = (one_c * zx) + ys;
	 mat[1][0] = (one_c * xy) + zs;
	 mat[1][1] = (one_c * yy) + c;
	 mat[1][2] = (one_c * yz) - xs;
	 mat[2][0] = (one_c * zx) - ys;
	 mat[2][1] = (one_c * yz) + xs;
	 mat[2][2] = (one_c * zz) + c;
	 mat[0][3] = 0;
	 mat[1][3] = 0;
	 mat[2][3] = 0;
}

/* Same as mat3FromDirAndUp */
void	V_GLOBAL
mat3x4FromDirAndUp(matrix3x4_t mat,vec3_t dir,vec3_t up)
{
	vec3_t left;
	vec3Cross(left,up,dir);
	mat3x4SetRows(mat,left,up,dir);
}

/*
 *	Computes view matrix for given camera parameters.
 *	Assumes 'dir' and 'up' normalized.
 */
void	V_GLOBAL
mat3x4FromPointDirAndUp(matrix3x4_t mat,vec3_t pt,vec3_t dir,vec3_t up)
{
	mat3x4FromDirAndUp(mat,dir,up);
	mat[0][3] = - mat[0][0] * pt[0] - mat[0][1] * pt[1] - mat[0][2] * pt[2];
	mat[1][3] = - mat[1][0] * pt[0] - mat[1][1] * pt[1] - mat[1][2] * pt[2];
	mat[2][3] = - mat[2][0] * pt[0] - mat[2][1] * pt[1] - mat[2][2] * pt[2];
}

/*
 *	vec3Perpendicular - computes vector which is perpendicular to given v
 */
void	V_GLOBAL
vec3Perpendicular(vec3_t dest,vec3_t v)
{
	vec3_t k;
	if(vec3MaxAbsInd(v) != 2)
	{
		vec3Set(k,0,0,1);
		vec3Cross(dest,k,v);
	}
	else
	{
		vec3Set(k,0,1,0);
		vec3Cross(dest,k,v);
	}
	vec3Unit(dest,dest);
}

/*
 *	vec3RotateAround - rotation of vector 'pt' around arbitrary 'dir'
 */
void	V_GLOBAL
vec3RotateAround(vec3_t dst,vec3_t dir,vec3_t point,vec_t radians)
{
	vec3_t nd;
	matrix3_t rot;
	vec3Unit(nd,dir);
	mat3Rotate(rot,nd,radians);
	mat3VecMul(dst,rot,point);
}

/*
 *	Computes foot of given point
 */
void	V_GLOBAL
vec3ProjectOnPlane(vec3_t dest,vec3_t pt,plane3_t *plane)
{
	line3_t l;
	vec3Copy(l.origin,pt);
	vec3Copy(l.dir,plane->normal);
	vec3FromLineAndPlane(dest,&l,plane);
}

/*
 *	Computes cross point of line and plane
 */
void	V_GLOBAL
vec3FromLineAndPlane(vec3_t dest,line3_p line,plane3_p plane)
{
	vec_t nd, t;
	nd = vec3Dot(plane->normal,line->dir);
	if(IsZero(nd))
	{
		vec3Set(dest,0,0,1);
		return;
	}
	t = - (vec3Dot(plane->normal,line->origin) - plane->dist) / nd;
	vec3Copy(dest,line->dir);
	vec3Scale(dest,dest,t);
	vec3Add(dest,dest,line->origin);
}

/*
 *	Computes reflection of point in given plane
 */
void	V_GLOBAL
vec3PlaneReflection(vec3_t dest,plane3_p pl,vec3_t src)
{
	/*  Compute footpoint */
	vec3_t cross;
	vec3ProjectOnPlane(cross,src,pl);
	/*  Compute reflection: dst = 2 * cross - src */
	vec3Copy(dest,cross);
	vec3Add(dest,dest,dest);
	vec3Sub(dest,dest,src);
}

/*
 *	Computes equation of line by given two points
 */
void	V_GLOBAL
line3FromPoints(line3_p dest,vec3_t p1,vec3_t p2)
{
	vec3Copy(dest->origin,p1);
	vec3Sub(dest->dir,p2,p1);
	vec3Unit(dest->dir,dest->dir);
}

/*
 *	Makes equation of line by direction and origin
 */
void	V_GLOBAL
line3FromDirAndOrigin(line3_p dest,vec3_t dir,vec3_t origin)
{
	vec3Copy(dest->origin,origin);
	vec3Copy(dest->dir,dir);
	vec3Unit(dest->dir,dest->dir);
}

/*
 *	Computes plane's equation by given three points
 */
void	V_GLOBAL
plane3FromPoints(plane3_p dest,vec3_t a,vec3_t b,vec3_t c)
{
	/*	orientation of vectors ab, ac, pl->nd.normal is the same
	 *	as orientation of basis */
	vec3_t ab,ac;
	vec3Copy(ab,b);
	vec3Sub(ab,ab,a);
	vec3Copy(ac,c);
	vec3Sub(ac,ac,a);
	vec3Cross(dest->normal,ab,ac);
	vec3Unit(dest->normal,dest->normal);
	dest->dist = vec3Dot(dest->normal,a);
}

/*
 *	Computes equation of plane parallel to given lines
 */
void	V_GLOBAL
plane3FromLines(plane3_p dest,line3_p l1,line3_p l2)
{
	vec3Cross(dest->normal,l1->dir,l2->dir);
	vec3Unit(dest->normal,dest->normal);
	dest->dist = vec3Dot(dest->normal,l1->origin);
}

/*
 *	Bounding boxes stuff
 */
void	V_GLOBAL
bboxClear(vec3_t mins,vec3_t maxs)
{
	vec3Set(mins,MAXVEC,MAXVEC,MAXVEC);
	vec3Negate(maxs,mins);
}

void	V_GLOBAL
bboxAddPoint(vec3_t mins,vec3_t maxs,vec3_t p)
{
	int i;
	for(i=0;i<3;i++)
	{
		if(mins[i] > p[i])
			mins[i] = p[i];
		if(maxs[i] < p[i])
			maxs[i] = p[i];
	}
}

/*
 *	Calculatates transform from [in_mins;in_maxs] to [out_mins;out_maxs].
 */
void	V_GLOBAL
bboxTransformFromBoxes(matrix3x4_t dest,vec3_t in_mins,vec3_t in_maxs,
	vec3_t out_mins,vec3_t out_maxs)
{
	int i;
	vec_t in_len, out_len;
	mat3x4Zero(dest);
	for(i=0;i<3;i++)
	{
		in_len=in_maxs[i]-in_mins[i];
		out_len=out_maxs[i]-out_mins[i];
		if(IsZero(in_len))
			dest[i][i]=1;
		else
		{
			in_len=1/in_len;
			dest[i][i]=out_len * in_len;
			dest[i][3]= - (in_mins[i]*out_maxs[i] - 
				in_maxs[i]*out_mins[i]) * in_len;
		}
	}
}

/*
 * 22.feb.2006
 * Computes sherical coords of unit lenght 'unit_dir'.
 * Returns: *theta_ret - latitude, *phi_ret - longitude.
 */
void V_GLOBAL
vec3Dir2Angles(vec3_t unit_dir,vec_p theta_ret,vec_p phi_ret)
{
	const vec_t zero_diff = 1e-2;
	vec2_t n1;
	/* check if dir colinear to z axis */
	if(IsEqualEpsilon(unit_dir[2],1,zero_diff))
	{
		*theta_ret = *phi_ret = 0;
		return;
	}
	if(IsEqualEpsilon(unit_dir[2],-1,zero_diff))
	{
		*theta_ret = M_PI;
		*phi_ret = 0;
		return;
	}
	/* general case */
	*theta_ret = (vec_t)acos(unit_dir[2]);
	vec2Copy(n1,unit_dir);
	vec2Unit(n1,n1);
	*phi_ret = (vec_t)acos(n1[0]);
	if(n1[1] < 0)
		*phi_ret = M_2PI - *phi_ret;
}
