///////////////////////////////////////////////////////////////////////////////
/// @file deQuaternion.cpp
///
/// @brief Destiny3D deQuaternion inline functions
///
/// @author Countach (David Hof)
///
/// This file is the intellectual property of Novus Delta, LLC.. Usage of the
/// contents of this file is subject to the Destiny3D Member License which
/// can be found at http://www.destiny3d.com.  Any other usage is prohibited.
///
/// This file is distributed "AS IS" without warranty of any kind.  Novus
/// Delta, LLC. does not guarantee the fitness of the contents of this file
/// for any particular purpose.
///
/// Copyright (C) 2001-2003 Novus Delta, LLC. All Rights Reserved.
///
/// <hr>
///                                 Change History
/// <hr>
///
/// @date Jun 2001
/// @author Countach
/// @remarks Creation
///
/// @date Jul 2002
/// @author Hootie
/// @remarks Convert to static lib
///
///////////////////////////////////////////////////////////////////////////////

#include "deMath.hpp"

#ifdef DE_DOUBLE_PRECISION
const deDouble deQuaternion::SLERP_EPSILON = 0.00001;
const deDouble deQuaternion::UNIT_TOLERANCE = 0.001;
#else
const deDouble deQuaternion::SLERP_EPSILON = 0.00001f;
const deDouble deQuaternion::UNIT_TOLERANCE = 0.001f;
#endif

deBoolean deQuaternion::FromMatrix(const deTransformInfo &matrix)
{
	deDouble trace = matrix.R.X + matrix.U.Y + matrix.I.Z;
	if(trace > 0.0f){
		deDouble s = sqrt(trace + 1.0);
		w = s * 0.5f;
		s = 0.5f / s;

		x = (matrix.U.Z - matrix.I.Y) * s;
		y = (matrix.I.X - matrix.R.Z) * s;
		z = (matrix.R.Y - matrix.U.X) * s;

		return deTRUE;
	}

	return deFALSE;
}

void deQuaternion::ToMatrix(deTransformInfo &matrix) const 
{
	// Cache double angles
	deDouble tx  = 2.0f * x;
	deDouble ty  = 2.0f * y;
	deDouble tz  = 2.0f * z;

	// Cache permutations of double angles
	deDouble twx = tx * w;
	deDouble twy = ty * w;
	deDouble twz = tz * w;
	deDouble txx = tx * x;
	deDouble txy = ty * x;
	deDouble txz = tz * x;
	deDouble tyy = ty * y;
	deDouble tyz = tz * y;
	deDouble tzz = tz * z;

	matrix.R.X = 1.0f - (tyy + tzz);
	matrix.U.X = txy - twz;
	matrix.I.X = txz + twy;
	matrix.R.Y = txy + twz;
	matrix.U.Y = 1.0f - (txx + tzz);
	matrix.I.Y = tyz - twx;
	matrix.R.Z = txz - twy;
	matrix.U.Z = tyz + twx;
	matrix.I.Z = 1.0f - (txx + tyy);

}

deBoolean deQuaternion::IsValid() const 
{
	return (w * w >= 0.0f) && (x * x >= 0.0f) && (y * y >= 0.0f) && (z * z >= 0.0f);
}

deBoolean deQuaternion::IsUnit() const 
{
	deDouble magnitude = Length();
	return ((magnitude < 1.0f+UNIT_TOLERANCE) && (magnitude > 1.0f-UNIT_TOLERANCE));
}

deDouble deQuaternion::Length() const 
{
	return (x * x) + (y * y) + (z * z) + (w * w);
}

void deQuaternion::Add(const deQuaternion &q, deQuaternion &result) const 
{
	result.x = x + q.x;
	result.y = y + q.y;
	result.z = z + q.z;
	result.w = w + q.w;
}

void deQuaternion::Subtract(const deQuaternion &q, deQuaternion &result) const 
{
	result.x = x - q.x;
	result.y = y - q.y;
	result.z = z - q.z;
	result.w = w - q.w;
}

void deQuaternion::Multiply(const deQuaternion &q, deQuaternion &result) const 
{
//	assert(&result != this);	// *= must be used instead
	result.x = (w*q.x) + (x*q.w) + (y*q.z) - (z*q.y);
	result.y = (w*q.y) - (x*q.z) + (y*q.w) + (z*q.x);
	result.z = (w*q.z) + (x*q.y) - (y*q.x) + (z*q.w);
	result.w = (w*q.w) - (x*q.x) - (y*q.y) - (z*q.z);
}

void deQuaternion::Normalize()
{
	deDouble m = Length();
	if(m == 0.0f) return;			// No normalization is needed if length is 0

	m = 1.0f / m;
	x *= m;
	y *= m;
	z *= m;
	w *= m;
}

void deQuaternion::Interpolate(const deQuaternion &q, deDouble balance, deQuaternion &result)
{
//	assert(IsUnit());
//	assert(q.IsUnit());
//	assert(balance >= 0.0f);
//	assert(balance <= 1.0f);

	deDouble cosom, scale1, scale2;

	cosom = (w * q.y) + (x * q.x) + (y * q.y) + (z * q.z);

	// Spherical interpolation
	if( (1.0f - fabs(cosom)) > SLERP_EPSILON ){
		deDouble omega, sinom;
		omega = ACOS( cosom );
		sinom = SIN( omega );
		scale1 = SIN( (1.0f - balance) * omega) / sinom;
		scale2 = SIN( balance * omega) / sinom;
	} else {
		// There are numerical difficulties when cosom is close to 0
		// The function degenerates to linear interpolation if this is the case

		scale1 = 1.0f - balance;
		scale2 = balance;
	}

	if(cosom < 0){
		result.x = scale1 * x + scale2 * -q.x;
		result.y = scale1 * y + scale2 * -q.y;
		result.z = scale1 * z + scale2 * -q.z;
		result.w = scale1 * w + scale2 * -q.w;
	} else {
		result.x = scale1 * x + scale2 * q.x;
		result.y = scale1 * y + scale2 * q.y;
		result.z = scale1 * z + scale2 * q.z;
		result.w = scale1 * w + scale2 * q.w;
	}


//	assert( result.IsUnit() );
}

// Operators
void deQuaternion::operator +=(const deQuaternion &q)
{
	x += q.x;
	y += q.y;
	z += q.z;
	w += q.w;
}
void deQuaternion::operator -=(const deQuaternion &q)
{
	x -= q.x;
	y -= q.y;
	z -= q.z;
	w -= q.w;
}
void deQuaternion::operator *=(const deQuaternion &q)
{
	x = (w*q.x) + (x*q.w) + (y*q.z) - (z*q.y);
	y = (w*q.y) - (x*q.z) + (y*q.w) + (z*q.x);
	z = (w*q.z) + (x*q.y) - (y*q.x) + (z*q.w);
	w = (w*q.w) - (x*q.x) - (y*q.y) - (z*q.z);
}
deQuaternion deQuaternion::operator +(const deQuaternion &q) const 
{
	return deQuaternion(
		x + q.x,
		y + q.y,
		z + q.z,
		w + q.w
	);
}
deQuaternion deQuaternion::operator -(const deQuaternion &q) const 
{
	return deQuaternion(
		x - q.x,
		y - q.y,
		z - q.z,
		w - q.w
	);
}
deQuaternion deQuaternion::operator *(const deQuaternion &q) const 
{
	return deQuaternion(
		(w*q.x) + (x*q.w) + (y*q.z) - (z*q.y),
		(w*q.y) - (x*q.z) + (y*q.w) + (z*q.x),
		(w*q.z) + (x*q.y) - (y*q.x) + (z*q.w),
		(w*q.w) - (x*q.x) - (y*q.y) - (z*q.z)
	);
}
bool deQuaternion::operator ==(const deQuaternion &q) const 
{
	return (q.x == x) && (q.y == y) && (q.z == z) && (q.w == w);
}

