// MathLibrary.cpp: implementation of the MathLibrary function.
//
//////////////////////////////////////////////////////////////////////

#include "MathLibrary.h"

#include "OpenGL.h"

/** Vector
 *
 */
void CopyVector(const LPVECTOR3D lpIn, LPVECTOR3D lpOut)
{
	lpOut->x = lpIn->x;
	lpOut->y = lpIn->y;
	lpOut->z = lpIn->z;
}

void AddVector(const LPVECTOR3D lpV1, const LPVECTOR3D lpV2, LPVECTOR3D lpOut)
{
	lpOut->x = lpV1->x + lpV2->x;
	lpOut->y = lpV1->y + lpV2->y;
	lpOut->z = lpV1->z + lpV2->z;
}

void SubtractVector(const LPVECTOR3D lpV1, const LPVECTOR3D lpV2, LPVECTOR3D lpOut)
{
	lpOut->x = lpV1->x - lpV2->x;
	lpOut->y = lpV1->y - lpV2->y;
	lpOut->z = lpV1->z - lpV2->z;
}

bool CompareVector(const LPVECTOR3D lpV1, const LPVECTOR3D lpV2)
{
	for (int i=0; i<3; i++)
	{
		if (fabs(lpV1->v[i] - lpV2->v[i]) > EQUAL_EPSILON)
			return false;
	}	
	
	return true;
}

float NormalizeVector(LPVECTOR3D lpV)
{
	int		i;
	float	length;

	if (fabs(lpV->y - 0.000215956f) < 0.0001f)
		i = 1;

	length = (float) sqrt(lpV->x * lpV->x + lpV->y * lpV->y + lpV->z * lpV->z);
	if (length == 0.0f)
		return 0;
		
	lpV->x /= (float) length;
	lpV->y /= (float) length;
	lpV->z /= (float) length;

	return (float) length;
}

float DotProductVector(const LPVECTOR3D lpV1, const LPVECTOR3D lpV2)
{
	return lpV1->x * lpV2->x + lpV1->y * lpV2->y + lpV1->z * lpV2->z;
}

void ClipRotations(LPVECTOR3D lpV)
{
	for (int i=0; i<3; i++)
	{
		while (lpV->v[i] >= Q_PI)
			lpV->v[i] -= Q_PI*2;

		while (lpV->v[i] < -Q_PI)
			lpV->v[i] += Q_PI*2;
	}
}


/** Angle
 *
 */
void AngleQuaternion(const LPVECTOR3D lpAngles, LPQUATERNION lpQ)
{
	float	angle;
	float	sr, sp, sy, cr, cp, cy;

	// FIXME: rescale the inputs to 1/2 angle
	angle = lpAngles->z * 0.5f;
	sy = (float) sin(angle);
	cy = (float) cos(angle);
	
	angle = lpAngles->y * 0.5f;
	sp = (float) sin(angle);
	cp = (float) cos(angle);

	angle = lpAngles->x * 0.5f;
	sr = (float) sin(angle);
	cr = (float) cos(angle);

	lpQ->x = sr*cp*cy-cr*sp*sy;
	lpQ->y = cr*sp*cy+sr*cp*sy;
	lpQ->z = cr*cp*sy-sr*sp*cy;
	lpQ->w = cr*cp*cy+sr*sp*sy;
}

void AngleMatrix(const LPVECTOR3D lpAngles, LPMATRIX lpM)
{
	float	angle;
	float	sr, sp, sy, cr, cp, cy;
	
	angle = lpAngles->z * (Q_PI2 / 360.0f);
	sy = (float) sin(angle);
	cy = (float) cos(angle);

	angle = lpAngles->y * (Q_PI2 / 360.0f);
	sp = (float) sin(angle);
	cp = (float) cos(angle);

	angle = lpAngles->x * (Q_PI2 / 360.0f);
	sr = (float) sin(angle);
	cr = (float) cos(angle);

	// m = (Z * Y) * X
	lpM->_00 = cp*cy;
	lpM->_10 = cp*sy;
	lpM->_20 = -sp;

	lpM->_01 = sr*sp*cy+cr*-sy;
	lpM->_11 = sr*sp*sy+cr*cy;
	lpM->_21 = sr*cp;

	lpM->_02 = (cr*sp*cy+-sr*-sy);
	lpM->_12 = (cr*sp*sy+-sr*cy);
	lpM->_22 = cr*cp;

	lpM->_03 = 0.0f;
	lpM->_13 = 0.0f;
	lpM->_23 = 0.0f;
}

void AngleIMatrix(const LPVECTOR3D lpAngles, LPMATRIX lpM)
{
	float	angle;
	float	sr, sp, sy, cr, cp, cy;
	
	angle = lpAngles->z * (Q_PI*2 / 360.0f);
	sy = (float) sin(angle);
	cy = (float) cos(angle);

	angle = lpAngles->y * (Q_PI*2 / 360.0f);
	sp = (float) sin(angle);
	cp = (float) cos(angle);

	angle = lpAngles->x * (Q_PI*2 / 360.0f);
	sr = (float) sin(angle);
	cr = (float) cos(angle);

	// m = (Z * Y) * X
	lpM->_00 = cp*cy;
	lpM->_10 = sr*sp*cy+cr*-sy;
	lpM->_20 = (cr*sp*cy+-sr*-sy);

	lpM->_01 = cp*sy;
	lpM->_11 = sr*sp*sy+cr*cy;
	lpM->_21 = (cr*sp*sy+-sr*cy);

	lpM->_02 = -sp;
	lpM->_12 = sr*cp;
	lpM->_22 = cr*cp;

	lpM->_03 = 0.0;
	lpM->_13 = 0.0;
	lpM->_23 = 0.0;
}


/**
 * Quaternion
 */
 
// specified the amount of error until quaternion spherical interploation falls back on linear
#define QUAT_SLERP_DELTA .01f

void QuaternionSlerp(const LPQUATERNION lpFrom, const LPQUATERNION lpTo, const float t, LPQUATERNION lpOut)
{
	float omega;
	float sinom;
	float scale0;
	float scale1;

	QUATERNION q;
	
	// calc cosine
	float cosom = lpFrom->x * lpTo->x + lpFrom->y * lpTo->y + lpFrom->z * lpTo->z + lpFrom->w * lpTo->w;

	// adjust signs (if necessary) 
	if (cosom < 0.f) 
	{
		cosom = -cosom;
		
		q.x = -lpTo->x;
		q.y = -lpTo->y;
		q.z = -lpTo->z;
		q.w = -lpTo->w;
	} 
	else 
	{
		q.x = lpTo->x;
		q.y = lpTo->y;
		q.z = lpTo->z;
		q.w = lpTo->w;
	}

	// calculate coefficients 
	if ((1.f - cosom) > QUAT_SLERP_DELTA) 
	{
		// standard case (slerp) 
		omega = (float) acos(cosom);
		sinom = (float) sin(omega);

		scale0 = (float) sin((1.f - t) * omega) / sinom;
		scale1 = (float) sin(t * omega) / sinom;
	} 
	else 
	{
		//"from" and "to" quaternions are very close
		//  ... so we can do a linear interpolation
		scale0 = 1.f - t;
		scale1 = t;
	}

	// calculate final values 
	lpOut->x = scale0 * lpFrom->x + scale1 * q.x;
	lpOut->y = scale0 * lpFrom->y + scale1 * q.y;
	lpOut->z = scale0 * lpFrom->z + scale1 * q.z;
	lpOut->w = scale0 * lpFrom->w + scale1 * q.w;
}

void QuaternionMatrix(const LPQUATERNION lpQ, LPMATRIX lpM)
{
	float x2, y2, z2, wx, wy, wz, xx, xy, xz, yy, yz, zz;

	x2 = lpQ->x + lpQ->x;
	y2 = lpQ->y + lpQ->y;
	z2 = lpQ->z + lpQ->z;

	wx = lpQ->w * x2;   wy = lpQ->w * y2;   wz = lpQ->w * z2;
	xx = lpQ->x * x2;   xy = lpQ->x * y2;   xz = lpQ->x * z2;
	yy = lpQ->y * y2;   yz = lpQ->y * z2;   zz = lpQ->z * z2;

	lpM->_00 = 1.0f - (yy + zz);
	lpM->_10 = xy + wz;
	lpM->_20 = xz - wy;

	lpM->_01 = xy - wz;
	lpM->_11 = 1.0f - (xx + zz);
	lpM->_21 = yz + wx;

	lpM->_02 = xz + wy;
	lpM->_12 = yz - wx;
	lpM->_22 = 1.0f - (xx + yy);
}

void MatrixQuaternion(const LPMATRIX lpM, LPQUATERNION lpOut)
{
	QUATERNION  q;
	int i, j, k;
	static int nxt[3] = {1, 2, 0};

	float tr = lpM->_00 + lpM->_11 + lpM->_22;
	if (tr > 0.0f) 
	{
		float s = (float) sqrt(tr + 1.0f);
		lpOut->w = s / 2.0f;
		s = 0.5f / s;
	
		lpOut->x = (lpM->_12 - lpM->_21) * s;
		lpOut->y = (lpM->_20 - lpM->_02) * s;
		lpOut->z = (lpM->_01 - lpM->_10) * s;
	} 
	else 
	{
		i = 0;
		if (lpM->_11 > lpM->_00)
			i = 1;
		if (lpM->_22 > lpM->m[i][i])
			i = 2;

		j = nxt[i];
		k = nxt[j];
		
		float s = (float) sqrt((lpM->m[i][i] - (lpM->m[j][j] + lpM->m[k][k])) + 1.0f);
		q.q[i] = s / 2.0f;
		if (s != 0.0f)
			s = 0.5f / s;

		q.w    = (lpM->m[j][k] - lpM->m[k][j]) * s;
		q.q[j] = (lpM->m[i][j] + lpM->m[j][i]) * s;
		q.q[k] = (lpM->m[i][k] + lpM->m[k][i]) * s;

		lpOut->w = q.w;
		lpOut->x = q.x;
		lpOut->y = q.y;
		lpOut->z = q.z;
	}
}

/** Matrix
 *
 */
void MatrixCopy(const LPMATRIX lpM, LPMATRIX lpOut)
{
	lpOut->_00 = lpM->_00;
	lpOut->_01 = lpM->_01;
	lpOut->_02 = lpM->_02;
	lpOut->_03 = lpM->_03;

	lpOut->_10 = lpM->_10;
	lpOut->_11 = lpM->_11;
	lpOut->_12 = lpM->_12;
	lpOut->_13 = lpM->_13;
	
	lpOut->_20 = lpM->_20;
	lpOut->_21 = lpM->_21;
	lpOut->_22 = lpM->_22;
	lpOut->_23 = lpM->_23;
}

void MatrixIdentity(LPMATRIX lpOut)
{
	lpOut->_00 = 1.0f;
	lpOut->_01 = 0.0f;
	lpOut->_02 = 0.0f;
	lpOut->_03 = 0.0f;
	
	lpOut->_10 = 0.0f;
	lpOut->_11 = 1.0f;
	lpOut->_12 = 0.0f;
	lpOut->_13 = 0.0f;

	lpOut->_20 = 0.0f;
	lpOut->_21 = 0.0f;
	lpOut->_22 = 1.0f;
	lpOut->_23 = 0.0f;
}

void MatrixTranslation(LPVECTOR3D lpV, LPMATRIX lpOut)
{
	lpOut->_00 = 1.0f;
	lpOut->_01 = 0.0f;
	lpOut->_02 = 0.0f;
	lpOut->_03 = lpV->x;

	lpOut->_10 = 0.0f;
	lpOut->_11 = 1.0f;
	lpOut->_12 = 0.0f;
	lpOut->_13 = lpV->y;
	
	lpOut->_20 = 0.0f;
	lpOut->_21 = 0.0f;
	lpOut->_22 = 1.0f;
	lpOut->_23 = lpV->z;
}

void MatrixScale(LPVECTOR3D lpV, LPMATRIX lpOut)
{
	lpOut->_00 = lpV->x;
	lpOut->_01 = 0.0f;
	lpOut->_02 = 0.0f;
	lpOut->_03 = 0.0f;
	
	lpOut->_10 = 0.0f;
	lpOut->_11 = lpV->y;
	lpOut->_12 = 0.0f;
	lpOut->_13 = 0.0f;

	lpOut->_20 = 0.0f;
	lpOut->_21 = 0.0f;
	lpOut->_22 = lpV->z;
	lpOut->_23 = 0.0f;
}

void MatrixRotateX(float angle, LPMATRIX lpOut)
{
	float sinx = (float) sin(angle);
	float cosx = (float) cos(angle);

	lpOut->_00 = 1.0f;
	lpOut->_01 = 0.0f;
	lpOut->_02 = 0.0f;
	lpOut->_03 = 0.0f;

	lpOut->_10 = 0.0f;
	lpOut->_11 = cosx;
	lpOut->_12 = sinx;
	lpOut->_13 = 0.0f;

	lpOut->_20 = 0.0f;
	lpOut->_21 = -sinx;
	lpOut->_22 = cosx;
	lpOut->_23 = 0.0f;
}

void MatrixRotateY(float angle, LPMATRIX lpOut)
{
	float siny = (float) sin(angle);
	float cosy = (float) cos(angle);

	lpOut->_00 = cosy;
	lpOut->_01 = 0.0f;
	lpOut->_02 = -siny;
	lpOut->_03 = 0.0f;
	
	lpOut->_10 = 0.0f;
	lpOut->_11 = 1.0f;
	lpOut->_12 = 0.0f;
	lpOut->_13 = 0.0f;

	lpOut->_20 = siny;
	lpOut->_21 = 0.0f;
	lpOut->_22 = cosy;
	lpOut->_23 = 0.0f;
}

void MatrixRotateZ(float angle, LPMATRIX lpOut)
{
	float sinz = (float) sin(angle);
	float cosz = (float) cos(angle);

	lpOut->_00 = cosz;
	lpOut->_01 = sinz;
	lpOut->_02 = 0.0f;
	lpOut->_03 = 0.0f;

	lpOut->_10 = -sinz;
	lpOut->_11 = cosz;
	lpOut->_12 = 0.0f;
	lpOut->_13 = 0.0f;

	lpOut->_20 = 0.0f;
	lpOut->_21 = 0.0f;
	lpOut->_22 = 1.0f;
	lpOut->_23 = 0.0f;
}

void MatrixMultiply(const LPMATRIX lpM1, const LPMATRIX lpM2, LPMATRIX lpOut)
{
	lpOut->_00 = lpM1->_00 * lpM2->_00 + lpM1->_01 * lpM2->_10 + lpM1->_02 * lpM2->_20;
	lpOut->_01 = lpM1->_00 * lpM2->_01 + lpM1->_01 * lpM2->_11 + lpM1->_02 * lpM2->_21;
	lpOut->_02 = lpM1->_00 * lpM2->_02 + lpM1->_01 * lpM2->_12 + lpM1->_02 * lpM2->_22;
	lpOut->_03 = lpM1->_00 * lpM2->_03 + lpM1->_01 * lpM2->_13 + lpM1->_02 * lpM2->_23 + lpM1->_03;

	lpOut->_10 = lpM1->_10 * lpM2->_00 + lpM1->_11 * lpM2->_10 + lpM1->_12 * lpM2->_20;
	lpOut->_11 = lpM1->_10 * lpM2->_01 + lpM1->_11 * lpM2->_11 + lpM1->_12 * lpM2->_21;
	lpOut->_12 = lpM1->_10 * lpM2->_02 + lpM1->_11 * lpM2->_12 + lpM1->_12 * lpM2->_22;
	lpOut->_13 = lpM1->_10 * lpM2->_03 + lpM1->_11 * lpM2->_13 + lpM1->_12 * lpM2->_23 + lpM1->_13;
	
	lpOut->_20 = lpM1->_20 * lpM2->_00 + lpM1->_21 * lpM2->_10 + lpM1->_22 * lpM2->_20;
	lpOut->_21 = lpM1->_20 * lpM2->_01 + lpM1->_21 * lpM2->_11 + lpM1->_22 * lpM2->_21;
	lpOut->_22 = lpM1->_20 * lpM2->_02 + lpM1->_21 * lpM2->_12 + lpM1->_22 * lpM2->_22;
	lpOut->_23 = lpM1->_20 * lpM2->_03 + lpM1->_21 * lpM2->_13 + lpM1->_22 * lpM2->_23 + lpM1->_23;
}

void MatrixGL(const LPMATRIX lpM) 
{
	float m[16];

	m[0]  = lpM->_00;
	m[1]  = lpM->_10;
	m[2]  = lpM->_20;
	m[3]  = 0.0f;
	
	m[4]  = lpM->_01;
	m[5]  = lpM->_11;
	m[6]  = lpM->_21;
	m[7]  = 0.0f;
	
	m[8]  = lpM->_02;
	m[9]  = lpM->_12;
	m[10] = lpM->_22;
	m[11] = 0.0f;
	
	m[12] = lpM->_03;
	m[13] = lpM->_13;
	m[14] = lpM->_23;
	m[15] = 1.0f;

	// Apply to the current OpenGL matrix.
	//glLoadMatrixf(m);
	glMultMatrixf(m);
}

/** Transform
 *
 */
void TransformVector(const LPVECTOR3D lpV, const LPMATRIX lpM, LPVECTOR3D lpOut)
{
	lpOut->x = (lpV->x * lpM->_00 + lpV->y * lpM->_01 + lpV->z * lpM->_02) + lpM->_03;
	lpOut->y = (lpV->x * lpM->_10 + lpV->y * lpM->_11 + lpV->z * lpM->_12) + lpM->_13;
	lpOut->z = (lpV->x * lpM->_20 + lpV->y * lpM->_21 + lpV->z * lpM->_22) + lpM->_23;
}



