#include "quat.h"

#define DELTA 0.00001

void QUATERNION::LoadMultIdent()
{
	w = 1.0f;
	x = y = z = 0.0f;
	cachematvalid = false;
}

QUATERNION::QUATERNION()
{
	LoadMultIdent();
	cachematvalid = false;
}

float QUATERNION::Norm()
{
	return sqrt(w*w+x*x+y*y+z*z);
}

void QUATERNION::Normalize()
{
	cachematvalid = false;
	
	float len = Norm();
	w = w / len;
	x = x / len;
	y = y / len;
	z = z / len;
}

void QUATERNION::GetMat(float * destmat)
{
	if (!cachematvalid)
	{
		Normalize();
		//NOTE:  We're assuming the quaternion is normalized here!
		
		float xx = x*x;
		float xy = x*y;
		float xz = x*z;
		float xw = x*w;
		
		float yy = y*y;
		float yz = y*z;
		float yw = y*w;
		
		float zz = z*z;
		float zw = z*w;
		
		destmat[0] = 1.0f - 2.0f*(yy+zz);
		destmat[1] = 2.0f*(xy+zw);
		destmat[2] = 2.0f*(xz-yw);
		destmat[3] = 0;
		
		destmat[4] = 2.0f*(xy-zw);
		destmat[5] = 1.0f-2.0f*(xx+zz);
		destmat[6] = 2.0f*(yz+xw);
		destmat[7] = 0;
		
		destmat[8] = 2.0f*(xz+yw);
		destmat[9] = 2.0f*(yz-xw);
		destmat[10] = 1.0f-2.0f*(xx+yy);
		destmat[11] = 0;
		
		destmat[12] = 0;
		destmat[13] = 0;
		destmat[14] = 0;
		destmat[15] = 1;
		
		//build cached matrix
		for (int i = 0; i < 16; i++)
		{
			cachemat[i] = destmat[i];
		}
		cachematvalid = true;
	}
	else
	{
		//send back the cached matrix
		for (int i = 0; i < 16; i++)
		{
			destmat[i] = cachemat[i];
		}
	}
	
	/*xx      = X * X;
    xy      = X * Y;
    xz      = X * Z;
    xw      = X * W;

    yy      = Y * Y;
    yz      = Y * Z;
    yw      = Y * W;

    zz      = Z * Z;
    zw      = Z * W;

    mat[0]  = 1 - 2 * ( yy + zz );
    mat[1]  =     2 * ( xy - zw );
    mat[2]  =     2 * ( xz + yw );

    mat[4]  =     2 * ( xy + zw );
    mat[5]  = 1 - 2 * ( xx + zz );
    mat[6]  =     2 * ( yz - xw );

    mat[8]  =     2 * ( xz - yw );
    mat[9]  =     2 * ( yz + xw );
    mat[10] = 1 - 2 * ( xx + yy );

    mat[3]  = mat[7] = mat[11 = mat[12] = mat[13] = mat[14] = 0;
    mat[15] = 1;*/
}

void QUATERNION::GetMat(GLdouble * destmat)
{
	if (!cachematvalid)
	{
		Normalize();
		//NOTE:  We're assuming the quaternion is normalized here!
		
		GLdouble xx = x*x;
		GLdouble xy = x*y;
		GLdouble xz = x*z;
		GLdouble xw = x*w;
		
		GLdouble yy = y*y;
		GLdouble yz = y*z;
		GLdouble yw = y*w;
		
		GLdouble zz = z*z;
		GLdouble zw = z*w;
		
		destmat[0] = 1.0f - 2.0f*(yy+zz);
		destmat[1] = 2.0f*(xy+zw);
		destmat[2] = 2.0f*(xz-yw);
		destmat[3] = 0;
		
		destmat[4] = 2.0f*(xy-zw);
		destmat[5] = 1.0f-2.0f*(xx+zz);
		destmat[6] = 2.0f*(yz+xw);
		destmat[7] = 0;
		
		destmat[8] = 2.0f*(xz+yw);
		destmat[9] = 2.0f*(yz-xw);
		destmat[10] = 1.0f-2.0f*(xx+yy);
		destmat[11] = 0;
		
		destmat[12] = 0;
		destmat[13] = 0;
		destmat[14] = 0;
		destmat[15] = 1;
		
		//build cached matrix
		for (int i = 0; i < 16; i++)
		{
			cachemat[i] = destmat[i];
		}
		cachematvalid = true;
	}
	else
	{
		//send back the cached matrix
		for (int i = 0; i < 16; i++)
		{
			destmat[i] = cachemat[i];
		}
	}
}

void QUATERNION::GetAxisAngle(float * aangle)
{
	//aangle is (angle of rotation, x, y, z)
	
	/*float scale = x*x+y*y+z*z;
	if (scale < 0.00001)
	{
		aangle[0] = 0;
		aangle[1] = 1;
		aangle[2] = 0;
		aangle[3] = 0;
	}
	else
	{
		aangle[0] = 2*acos(w);
		aangle[1] = x/scale;
		aangle[2] = y/scale;
		aangle[3] = z/scale;
	}*/
	
	
	/*float scale = x*x+y*y+z*z;
	
	float cos_angle  = w;
    aangle[0]      = acos( cos_angle ) * 2;
    float sin_angle  = sqrt( 1.0 - cos_angle * cos_angle );


    if ( fabs( sin_angle ) < 0.00005 )
      scale = 1.0f;

    aangle[1] = x/scale;
    aangle[2] = y/scale;
	aangle[3] = z/scale;*/
	
    GLfloat	len;
    GLfloat tx, ty, tz;

	// cache variables
	tx = x;
	ty = y;
	tz = z;

	len = tx * tx + ty * ty + tz * tz;

    if (len > DELTA) 
	{
		aangle[1] = tx * (1.0f / len);
		aangle[2] = ty * (1.0f / len);
		aangle[3] = tz * (1.0f / len);
	    aangle[0] = (GLfloat)(2.0 * acos(w));
    }

    else {
		aangle[1] = 0.0;
		aangle[2] = 0.0;
		aangle[3] = 1.0;
	    aangle[0] = 0.0;
    }
}



void QUATERNION::Multiply(QUATERNION quat2)
{
	cachematvalid = false;
	
	//slow multiply
	float w2 = quat2.w;
	float x2 = quat2.x;
	float y2 = quat2.y;
	float z2 = quat2.z;
	w = w*w2 - x*x2 - y*y2 - z*z2;
	x = w*x2 + x*w2 + y*z2 - z*y2;
	y = w*y2 + y*w2 + z*x2 - x*z2;
	z = w*z2 + z*w2 + x*y2 - y*x2;
	
	/*//FAST multiply code
	float a = w;
	float b = x;
	float c = y;
	float d = z;
	float w2 = quat2.w;
	float x2 = quat2.x;
	float y2 = quat2.y;
	float z2 = quat2.z;
	
	float tmp_00, tmp_01, tmp_02, tmp_03, tmp_04, tmp_05, tmp_06, tmp_07, tmp_08, tmp_09;
	
	tmp_00 = (d - c) * (z2 - w2);
	tmp_01 = (a + b) * (x2 + y2);
	tmp_02 = (a - b) * (z2 + w2);
	tmp_03 = (c + d) * (x2 - y2);
	tmp_04 = (d - b) * (y2 - z2);
	tmp_05 = (d + b) * (y2 + z2);
	tmp_06 = (a + c) * (x2 - w2);
	tmp_07 = (a - c) * (x2 + w2);
	tmp_08 = tmp_05 + tmp_06 + tmp_07;
	tmp_09 = 0.5 * (tmp_04 + tmp_08);
	
	w = tmp_00 + tmp_09 - tmp_05;
	x = tmp_01 + tmp_09 - tmp_08;
	y = tmp_02 + tmp_09 - tmp_07;
	z = tmp_03 + tmp_09 - tmp_06;*/
}

void QUATERNION::PostMultiply(QUATERNION quat2)
{
	cachematvalid = false;
	
	//slow multiply
	float w1 = quat2.w;
	float x1 = quat2.x;
	float y1 = quat2.y;
	float z1 = quat2.z;
	float w2 = w;
	float x2 = x;
	float y2 = y;
	float z2 = z;
	w = w1*w2 - x1*x2 - y1*y2 - z1*z2;
	x = w1*x2 + x1*w2 + y1*z2 - z1*y2;
	y = w1*y2 + y1*w2 + z1*x2 - x1*z2;
	z = w1*z2 + z1*w2 + x1*y2 - y1*x2;
	
	//FAST multiply code
	/*float a = quat2.w;
	float b = quat2.x;
	float c = quat2.y;
	float d = quat2.z;
	
	float tmp_00, tmp_01, tmp_02, tmp_03, tmp_04, tmp_05, tmp_06, tmp_07, tmp_08, tmp_09;
	
	tmp_00 = (d - c) * (z - w);
	tmp_01 = (a + b) * (x + y);
	tmp_02 = (a - b) * (z + w);
	tmp_03 = (c + d) * (x - y);
	tmp_04 = (d - b) * (y - z);
	tmp_05 = (d + b) * (y + z);
	tmp_06 = (a + c) * (x - w);
	tmp_07 = (a - c) * (x + w);
	tmp_08 = tmp_05 + tmp_06 + tmp_07;
	tmp_09 = 0.5 * (tmp_04 + tmp_08);
	
	w = tmp_00 + tmp_09 - tmp_05;
	x = tmp_01 + tmp_09 - tmp_08;
	y = tmp_02 + tmp_09 - tmp_07;
	z = tmp_03 + tmp_09 - tmp_06;*/
}

QUATERNION QUATERNION::operator* (const QUATERNION &quat2 )
{
	//below line stays true since we're not actually modifying our current quat
	//cachematvalid = false;
	
	QUATERNION qout;
	
	//slow multiply
	float w2 = quat2.w;
	float x2 = quat2.x;
	float y2 = quat2.y;
	float z2 = quat2.z;
	qout.w = w*w2 - x*x2 - y*y2 - z*z2;
	qout.x = w*x2 + x*w2 + y*z2 - z*y2;
	qout.y = w*y2 + y*w2 + z*x2 - x*z2;
	qout.z = w*z2 + z*w2 + x*y2 - y*x2;
		
	return qout;
}

//i believe a is in radians
void QUATERNION::SetAxisAngle(float a, float ax, float ay, float az)
{
	cachematvalid = false;
	
	//normalize axis
	float temp = ax*ax + ay*ay + az*az;
    float dist = (GLfloat)(1.0 / sqrt(temp));

    ax *= dist;
    ay *= dist;
    az *= dist;
	
	/*if (ax < 0.00001 && ay < 0.00001 && az < 0.00001)
	{
		LoadMultIdent();
	}
	else*/
	{
		float sina2 = sin(a/2);
		
		w   =   cos(a/2);
		x   =   ax * sina2;
		y   =   ay * sina2;
		z   =   az * sina2;
	}
	
	
	/*GLfloat temp, dist;

	// normalize
	temp = ax*ax + ay*ay + az*az;

    dist = (GLfloat)(1.0 / sqrt(temp));

    ax *= dist;
    ay *= dist;
    az *= dist;

	x = ax;
	y = ay;
	z = az;

	w = (GLfloat)cos(a / 2.0f);*/
	
}

void QUATERNION::SetEuler(float a, float b, float c)
{
	cachematvalid = false;
	
	QUATERNION qout;
	
    QUATERNION Qx, Qy, Qz;
	Qx.w = cos(a/2); Qx.x = sin(a/2); Qx.y = Qx.z = 0.0f;
    Qy.w = cos(b/2); Qy.x = Qy.z = 0.0f; Qy.y = sin(b/2);
    Qz.w = cos(c/2); Qz.x = Qz.y = 0.0f; Qz.z = sin(c/2);

	qout = Qx * Qy * Qz;
	
	*this = qout;
}

QUATERNION QUATERNION::ReturnConjugate()
{
	QUATERNION qtemp;
	qtemp.w = w;
	qtemp.x = -x;
	qtemp.y = -y;
	qtemp.z = -z;
	return qtemp;
}

void QUATERNION::LookAt(float eyeX, 
		   float eyeY, 
		   float eyeZ, 
		   float centerX, 
		   float centerY, 
		   float centerZ, 
		   float upX, 
		   float upY, 
		   float upZ)
{
	float tempmat[16];
	
	glPushMatrix();
	glLoadIdentity();
	gluLookAt(eyeX, eyeY, eyeZ, centerX, centerY, centerZ, upX, upY, upZ);
	glGetFloatv(GL_MODELVIEW_MATRIX , tempmat);
	glPopMatrix();
	
	SetMat(tempmat);
}

void QUATERNION::SetMat(float * mat)
{
	float S;
	
	float T = 1 + mat[0] + mat[5] + mat[10];
	if ( T > 0.00000001 )
	{
		S = sqrt(T) * 2;
		x = ( mat[9] - mat[6] ) / S;
		y = ( mat[2] - mat[8] ) / S;
		z = ( mat[4] - mat[1] ) / S;
		w = 0.25 * S;
	}
	else
	{
		if ( mat[0] > mat[5] && mat[0] > mat[10] )  {       // Column 0: 
			S  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
			x = 0.25 * S;
			y = (mat[4] + mat[1] ) / S;
			z = (mat[2] + mat[8] ) / S;
			w = (mat[9] - mat[6] ) / S;
		} else if ( mat[5] > mat[10] ) {                    // Column 1: 
			S  = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
			x = (mat[4] + mat[1] ) / S;
			y = 0.25 * S;
			z = (mat[9] + mat[6] ) / S;
			w = (mat[2] - mat[8] ) / S;
		} else {                                            // Column 2:
			S  = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
			x = (mat[2] + mat[8] ) / S;
			y = (mat[9] + mat[6] ) / S;
			z = 0.25 * S;
			w = (mat[4] - mat[1] ) / S;
		}
	}
}

/*void QUATERNION::SetMat(float * m1)
{
	w = sqrt(1.0 + m1[0] + m1[5] + m1[10]) / 2.0;
	double w4 = (4.0 * w);
	x = (m1[9] - m1[6]) / w4 ;
	y = (m1[2] - m1[8]) / w4 ;
	z = (m1[4] - m1[1]) / w4 ;
}*/

VERTEX QUATERNION::RotateVec(VERTEX vec)
{
	QUATERNION dirconj;
	
	QUATERNION dir;
	dir.w = w;
	dir.x = x;
	dir.y = y;
	dir.z = z;

	dirconj = dir.ReturnConjugate();
	QUATERNION qtemp;
	qtemp.w = 0;
	qtemp.x = vec.x;
	qtemp.y = vec.y;
	qtemp.z = vec.z;
	QUATERNION qout;
	qout = dir * qtemp * dirconj;
	
	VERTEX vout;
	
	vout.x += qout.x;
	vout.y += qout.y;
	vout.z += qout.z;
	
	return vout;
}

float QUATERNION::GetAngleBetween(QUATERNION quat2)
{
	//establish a forward vector
	VERTEX forward;
	forward.x = 0;
	forward.y = 0;
	forward.z = 1;
	
	//create vectors for quats
	VERTEX vec1, vec2;
	vec1 = RotateVec(forward);
	vec2 = quat2.RotateVec(forward);
	
	//return the angle between the vectors
	return acos(vec1.dot(vec2));
}

VERTEX QUATERNION::GetEuler()
{
	VERTEX vout;
	QUATERNION q1 = *this;
	
	float sqw = q1.w*q1.w;
    float sqx = q1.x*q1.x;
    float sqy = q1.y*q1.y;
    float sqz = q1.z*q1.z;
	
    vout.z = atan(2.0 * (q1.x*q1.y + q1.z*q1.w)/(sqx - sqy - sqz + sqw));
    vout.x = atan(2.0 * (q1.y*q1.z + q1.x*q1.w)/(-sqx - sqy + sqz + sqw));
    vout.y = asin(-2.0 * (q1.x*q1.z - q1.y*q1.w));
	
	return vout;
}

QUATERNION QUATERNION::Slerp(QUATERNION quat2, float t)
{
	QUATERNION qout, p1, p2;
	float theta = GetAngleBetween(quat2);
	
	//qout = (*this).MultScalar(sin((1.0f-t)*theta)).Add(quat2.MultScalar(sin(t*theta))).MultScalar(1.0f/sin(theta));
	
	p1 = (*this).MultScalar((sin((1.0f-t)*theta))/sin(theta));
	p2 = quat2.MultScalar(sin(theta*t)/sinh(theta));
	qout = p1.Add(p2);
	
	
	return qout;
}

QUATERNION QUATERNION::Add(QUATERNION quat2)
{
	QUATERNION qout;
	
	qout.w = w + quat2.w;
	qout.x = x + quat2.x;
	qout.y = y + quat2.y;
	qout.z = z + quat2.z;
	
	return qout;
}

QUATERNION QUATERNION::MultScalar(float scalar)
{
	QUATERNION qout;
	
	qout.w = w * scalar;
	qout.x = x * scalar;
	qout.y = y * scalar;
	qout.z = z * scalar;
	
	return qout;
}

QUATERNION QUATERNION::QuatSlerp(QUATERNION quat2, float t)
{
	float           to1[4];
	float        omega, cosom, sinom, scale0, scale1;


	// calc cosine
	cosom = x * quat2.x + y * quat2.y + z * quat2.z
			   + w * quat2.w;


	// adjust signs (if necessary)
	if ( cosom <0.0 ){ cosom = -cosom; to1[0] = - quat2.x;
	to1[1] = - quat2.y;
	to1[2] = - quat2.z;
	to1[3] = - quat2.w;
	} else  {
	to1[0] = quat2.x;
	to1[1] = quat2.y;
	to1[2] = quat2.z;
	to1[3] = quat2.w;
	}


	// calculate coefficients


   if ( (1.0 - cosom) > DELTA ) {
			// standard case (slerp)
			omega = acos(cosom);
			sinom = sin(omega);
			scale0 = sin((1.0 - t) * omega) / sinom;
			scale1 = sin(t * omega) / sinom;


	} else {        
// "from" and "to" quaternions are very close 
	//  ... so we can do a linear interpolation
			scale0 = 1.0 - t;
			scale1 = t;
	}
	// calculate final values
	QUATERNION qout;
	qout.x = scale0 * x + scale1 * to1[0];
	qout.y = scale0 * y + scale1 * to1[1];
	qout.z = scale0 * z + scale1 * to1[2];
	qout.w = scale0 * w + scale1 * to1[3];
	return qout;
}

//i was getting angry because the quats couldn't do this, so i added it....
void QUATERNION::Rotate(float a, float ax, float ay, float az)
{
	//NOTE: ax, ay, az is assumed to be a UNIT VECTOR!!
	
	QUATERNION qtemp;
	qtemp.SetAxisAngle(a, ax, ay, az);
	PostMultiply(qtemp);
}

void QUATERNION::DebugPrint()
{
	cout << x << "," << y << "," << z << "," << w << endl;
}

VERTEX VERTEX::operator+ (VERTEX v)
{
	VERTEX result;
	
	result.x = x + v.x;
	result.y = y + v.y;
	result.z = z + v.z;
	
	return result;
}

VERTEX VERTEX::operator- (VERTEX v)
{
	VERTEX result;
	
	result.x = x - v.x;
	result.y = y - v.y;
	result.z = z - v.z;
	
	return result;
}

void VERTEX::Set(float nx, float ny, float nz)
{
	x = nx;
	y = ny;
	z = nz;
}

float VERTEX::len()
{
	//optimization!
	if (x == 0 && y == 0 && z == 0)
		return 0;
	
	float sl = x*x+y*y+z*z;
	
	if (sl < 0)
	{
		ofstream err((settings.GetSettingsDir() + "/logs/vertex.log").c_str());
		err << x << endl << y << endl << z << endl << sl << endl;
		err.close();
		assert(0);
	}
	else if (sl >=0 )
	{	
		return sqrt(sl);
	}
	else
	{
		//last ditch effort to fix the problem
		if (!(x < 0 || x >= 0))
			x = 1;
		if (!(y < 0 || y >= 0))
			y = 1;
		if (!(z < 0 || z >= 0))
			z = 1;
		sl = x*x+y*y+z*z;
		
		if (!(sl >= 0))
		{
			ofstream err((settings.GetSettingsDir() + "/logs/vertex.log").c_str());
			err << x << endl << y << endl << z << endl << sl << endl;
			err.close();
			assert(0);
			return 0;
		}
		else
			return sqrt(sl);
	}
}

VERTEX VERTEX::cross(VERTEX b)
{
	VERTEX result;

	result.x = y*b.z - z*b.y;
	//result.y = x*b.z - z*b.x;
	result.y = z*b.x - x*b.z;
	result.z = x*b.y - y*b.x;

	return result;
}

VERTEX VERTEX::normalize()
{
	VERTEX new_vec;
	float vec_len = len();
	
	if (vec_len == 0.0f)
	{
		new_vec.x = 0.0f;
		new_vec.y = 0.0f;
		new_vec.z = 0.0f;
		return new_vec;
	}
	
	float inv_vec_len = 1.0f/vec_len;

	new_vec.x = x*inv_vec_len;
	new_vec.y = y*inv_vec_len;
	new_vec.z = z*inv_vec_len;

	return new_vec;
}

VERTEX::VERTEX()
{
	x = y = z = 0.0f;
}

void VERTEX::Scale(float scalar)
{
	x = x*scalar;
	y = y*scalar;
	z = z*scalar;
}

VERTEX VERTEX::ScaleR(float scalar)
{
	VERTEX result;
	
	result.x = x*scalar;
	result.y = y*scalar;
	result.z = z*scalar;
	
	return result;
}

VERTEX VERTEX::InvertR()
{
	VERTEX result;
	
	result.x = -x;
	result.y = -y;
	result.z = -z;
	
	return result;
}

float VERTEX::dot(VERTEX b)
{
	return x*b.x + y*b.y + z*b.z;
}

VERTEX VERTEX::interpolatewith(VERTEX other, float percent)
{
	VERTEX vout;
	
	vout.x = (other.x - x)*percent + x;
	vout.y = (other.y - y)*percent + y;
	vout.z = (other.z - z)*percent + z;
	
	return vout;
}

float * VERTEX::v3()
{
	float3[0] = x;
	float3[1] = y;
	float3[2] = z;
	
	return float3;
}

void VERTEX::DebugPrint(ostream & out)
{
	out << x << "," << y << "," << z << endl;
}

void VERTEX::zero()
{
	x = y = z = 0.0f;
}

void VERTEX::Set(float * f3)
{
	x = f3[0];
	y = f3[1];
	z = f3[2];
}

bool VERTEX::nan()
{
	if (!(x < 0 || x >= 0) || !(y < 0 || y >= 0) || !(z < 0 || z >= 0))
		return true;
	
	return false;
}

bool VERTEX::equals(VERTEX other)
{
	return (x == other.x && y == other.y && z == other.z);
}

VERTEX QUATERNION::RelativeMove(VERTEX input)
{
	QUATERNION dirconj, dir;
	VERTEX position;

	dirconj = ReturnConjugate();
	dir.w = w;
	dir.x = x;
	dir.y = y;
	dir.z = z;
	QUATERNION qtemp;
	qtemp.w = 0;
	qtemp.x = input.x;
	qtemp.y = input.y;
	qtemp.z = input.z;
	QUATERNION qout;
	qout = dirconj * qtemp * dir;
	
	position.x = qout.x;
	position.y = qout.y;
	position.z = qout.z;
	return position;
}

void MATRIX3::ProjectionMatrix(VERTEX normal)
{
	float a = normal.x;
	float b = normal.y;
	float c = normal.z;
	
	row[0].x = b*b+c*c;
	row[0].y = -a*b;
	row[0].z = -a*c;
	
	row[1].x = -b*a;
	row[1].y = a*a+c*c;
	row[1].z = -b*c;
	
	row[2].x = -c*a;
	row[2].y = -c*b;
	row[2].z = a*a+b*b;
}

VERTEX MATRIX3::Multiply(VERTEX vert)
{
	VERTEX out;
	
	/*out.zero();
	out.x += row[0].x*vert.x;
	out.x += row[0].y*vert.y;
	out.x += row[0].z*vert.z;*/
	
	out.x = row[0].dot(vert);
	out.y = row[1].dot(vert);
	out.z = row[2].dot(vert);
	
	return out;
}

//---------- VERTEXD
VERTEXD VERTEXD::operator+ (VERTEXD v)
{
	VERTEXD result;
	
	result.x = x + v.x;
	result.y = y + v.y;
	result.z = z + v.z;
	
	return result;
}

VERTEXD VERTEXD::operator- (VERTEXD v)
{
	VERTEXD result;
	
	result.x = x - v.x;
	result.y = y - v.y;
	result.z = z - v.z;
	
	return result;
}

void VERTEXD::Set(double nx, double ny, double nz)
{
	x = nx;
	y = ny;
	z = nz;
}

double VERTEXD::len()
{
	//optimization!
	if (x == 0 && y == 0 && z == 0)
		return 0;
	
	double sl = x*x+y*y+z*z;
	
	if (sl < 0)
	{
		ofstream err((settings.GetSettingsDir() + "/logs/vertex.log").c_str());
		err << x << endl << y << endl << z << endl << sl << endl;
		err.close();
		assert(0);
	}
	else if (sl >=0 )
	{	
		return sqrt(sl);
	}
	else
	{
		//last ditch effort to fix the problem
		if (!(x < 0 || x >= 0))
			x = 1;
		if (!(y < 0 || y >= 0))
			y = 1;
		if (!(z < 0 || z >= 0))
			z = 1;
		sl = x*x+y*y+z*z;
		
		if (!(sl >= 0))
		{
			ofstream err((settings.GetSettingsDir() + "/logs/vertex.log").c_str());
			err << x << endl << y << endl << z << endl << sl << endl;
			err.close();
			assert(0);
			return 0;
		}
		else
			return sqrt(sl);
	}
}

VERTEXD VERTEXD::cross(VERTEXD b)
{
	VERTEXD result;

	result.x = y*b.z - z*b.y;
	//result.y = x*b.z - z*b.x;
	result.y = z*b.x - x*b.z;
	result.z = x*b.y - y*b.x;

	return result;
}

VERTEXD VERTEXD::normalize()
{
	VERTEXD new_vec;
	double vec_len = len();
	
	if (vec_len == 0.0f)
	{
		new_vec.x = 0.0f;
		new_vec.y = 0.0f;
		new_vec.z = 0.0f;
		return new_vec;
	}

	new_vec.x = x/vec_len;
	new_vec.y = y/vec_len;
	new_vec.z = z/vec_len;

	return new_vec;
}

VERTEXD::VERTEXD()
{
	x = y = z = 0.0f;
}

void VERTEXD::Scale(double scalar)
{
	x = x*scalar;
	y = y*scalar;
	z = z*scalar;
}

VERTEXD VERTEXD::ScaleR(double scalar)
{
	VERTEXD result;
	
	result.x = x*scalar;
	result.y = y*scalar;
	result.z = z*scalar;
	
	return result;
}

VERTEXD VERTEXD::InvertR()
{
	VERTEXD result;
	
	result.x = -x;
	result.y = -y;
	result.z = -z;
	
	return result;
}

double VERTEXD::dot(VERTEXD b)
{
	return x*b.x + y*b.y + z*b.z;
}

VERTEXD VERTEXD::interpolatewith(VERTEXD other, double percent)
{
	VERTEXD vout;
	
	vout.x = (other.x - x)*percent + x;
	vout.y = (other.y - y)*percent + y;
	vout.z = (other.z - z)*percent + z;
	
	return vout;
}

double * VERTEXD::v3()
{
	double3[0] = x;
	double3[1] = y;
	double3[2] = z;
	
	return double3;
}

void VERTEXD::DebugPrint()
{
	cout << x << "," << y << "," << z << endl;
}

void VERTEXD::zero()
{
	x = y = z = 0.0f;
}

void VERTEXD::Set(double * f3)
{
	x = f3[0];
	y = f3[1];
	z = f3[2];
}

void VERTEXD::Set(float * f3)
{
	x = f3[0];
	y = f3[1];
	z = f3[2];
}

bool VERTEXD::nan()
{
	if (!(x < 0 || x >= 0) || !(y < 0 || y >= 0) || !(z < 0 || z >= 0))
		return true;
	
	return false;
}

VERTEXD::VERTEXD(VERTEX other)
{
	x = other.x;
	y = other.y;
	z = other.z;
}

VERTEXD& VERTEXD:: operator= (const VERTEX & other)
{
	x = other.x;
	y = other.y;
	z = other.z;
	
	return *this;
}

void AABB::SetFromCorners(VERTEX c1, VERTEX c2)
{
	//ensure c1 is smaller than c2
	float temp;
	if (c1.x > c2.x)
	{
		temp = c1.x;
		c1.x = c2.x;
		c2.x = temp;
	}
	if (c1.y > c2.y)
	{
		temp = c1.y;
		c1.y = c2.y;
		c2.y = temp;
	}
	if (c1.z > c2.z)
	{
		temp = c1.z;
		c1.z = c2.z;
		c2.z = temp;
	}
	
	float fluff = 0.0001;
	VERTEX fluffer;
	fluffer.Set(fluff, fluff, fluff);
	
	//add in some fudge factor
	c1 = c1 + fluffer.ScaleR(-1.0);
	c2 = c2 + fluffer;
	
	pos = c1;
	size = c2 - c1;
	center = pos + size.ScaleR(0.5);
}

bool AABB::IntersectRay(VERTEX orig, VERTEX dir)
{
	if (orig.x < pos.x)
		return false;
	if (orig.z < pos.z)
		return false;
	if (orig.x > (pos+size).x)
		return false;
	if (orig.z > (pos+size).z)
		return false;
	
	return true;
	
	VERTEX abs_segdir, abs_cross;
	float f;
	VERTEX diff;
	diff = orig - center;
	
	abs_segdir = dir;
	abs_segdir.flabs();
	VERTEX abs_diff;
	abs_diff = diff;
	abs_diff.flabs();
	
	if ( abs_diff.x > size.x && diff.x*dir.x>=0.0f )
		return false;
	if ( abs_diff.y > size.y && diff.y*dir.y>=0.0f )
		return false;
	if ( abs_diff.z > size.z && diff.z*dir.z>=0.0f )
		return false;
	
	VERTEX cross;
	cross = dir.cross(diff);
	abs_cross = cross;
	abs_cross.flabs();
	
	f = size.y*abs_segdir.z + size.z*abs_segdir.y;
	if ( abs_cross.x > f ) return false;
		
	f = size.z*abs_segdir.x + size.x*abs_segdir.z;
	if ( abs_cross.y > f ) return false;
		
	f = size.x*abs_segdir.y + size.y*abs_segdir.x;
	if ( abs_cross.z > f ) return false;
	
	return true;
}

bool AABB::IntersectSegment(VERTEX orig, VERTEX dir, float seglen)
{
	if (seglen>3e30f) return IntersectRay(orig, dir); // infinite ray
	VERTEX abs_segdir, abs_diff, abs_cross; 
	
	VERTEX segdir;
	segdir = dir.ScaleR(0.5f * seglen);
	VERTEX seg_center;
	seg_center = orig + segdir;
	VERTEX diff;
	diff = seg_center - center;
	
	abs_segdir.x = fabs(segdir.x);
	abs_segdir.y = fabs(segdir.y);
	abs_segdir.z = fabs(segdir.z);
	abs_diff.x=fabs(diff.x);
	abs_diff.y=fabs(diff.y);
	abs_diff.z=fabs(diff.z);
	float f=size.x + abs_segdir.x;
	if (abs_diff.x > f) return false;
	f=size.y + abs_segdir.y;
	if (abs_diff.y > f) return false;
	f=size.z + abs_segdir.z;
	if (abs_diff.z > f) return false;
	
	VERTEX cross;
	cross = segdir.cross(diff);
	
	abs_cross.x = fabs(cross.x);
	abs_cross.y = fabs(cross.y);
	abs_cross.z = fabs(cross.z);
	
	f = size.y*abs_segdir.z + size.z*abs_segdir.y;
	if ( abs_cross.x > f ) return false;
		
	f = size.z*abs_segdir.x + size.x*abs_segdir.z;
	if ( abs_cross.y > f ) return false;
		
	f = size.x*abs_segdir.y + size.y*abs_segdir.x;
	if ( abs_cross.z > f ) return false;
	
	return true;
}

void AABB::DebugPrint()
{
	cout << "pos:    ";
	pos.DebugPrint();
	cout << "size:   ";
	size.DebugPrint();
	cout << "center: ";
	center.DebugPrint();
}

bool AABB::IntersectAABB(AABB & other)
{
	VERTEX otherc1 = other.GetPos();
	VERTEX otherc2 = otherc1 + other.GetSize();
	
	VERTEX c1 = pos;
	VERTEX c2 = pos + size;
	
	//bias checks for non-collisions
	if (c1.x > otherc2.x || c2.x < otherc1.x)
		return false;
	
	if (c1.z > otherc2.z || c2.z < otherc1.z)
		return false;
	
	if (c1.y > otherc2.y || c2.y < otherc1.y)
		return false;
	
	return true;
}

void AABB::GetVerts(VERTEX * eightverts)
{
	VERTEX temp;
	
	eightverts[0] = pos;
	
	temp.Set(size.x,0,0);
	eightverts[1] = pos+temp;
	
	temp.Set(size.x,size.y,0);
	eightverts[2] = pos+temp;
	
	temp.Set(0,size.y,0);
	eightverts[3] = pos+temp;
	
	temp.Set(0,0,size.z);
	eightverts[4] = pos+temp;
	
	temp.Set(size.x,0,size.z);
	eightverts[5] = pos+temp;
	
	temp.Set(size.x,size.y,size.z);
	eightverts[6] = pos+temp;
	
	temp.Set(0,size.y,size.z);
	eightverts[7] = pos+temp;
}
