/*
 * ExNihilo 3D Engine
 * 
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * Please read AUTHORS file !!!
 * 
 * $Id: ExQuaternion.cpp,v 1.4 2002/06/16 00:51:09 binny Exp $
 *
 */

#include "ExQuaternion.h"

ExQuaternion::ExQuaternion()
{
	SetQuaternion(1,0,0,0);
}

ExQuaternion::ExQuaternion(float w,float x,float y,float z)
{
	SetQuaternion(w,x,y,z);
}

ExQuaternion::~ExQuaternion()
{

}

//////////////////////////////////////////////////////////////////////
// Methode
//////////////////////////////////////////////////////////////////////
void ExQuaternion::SetQuaternion(float w,float x,float y,float z)
{
	qw=w;
	qx=x;
	qy=y;
	qz=z;
}

void ExQuaternion::Conjugate(void)
{

}

void ExQuaternion::LoadIdentity(void)
{
	qx=-qx;
	qy=-qy;
	qz=-qz;
}
float ExQuaternion::GetMagnitude(void)
{
	return(sqrt(qw*qw+qx*qx+qy*qy+qz*qz));
	
}
void ExQuaternion::Normalize(void)
{
	float factor=GetMagnitude();
	float scaleby(1.0/sqrt(factor));
	qw=qw*scaleby;
	qx=qx*scaleby;
	qy=qy*scaleby;
	qz=qz*scaleby;
}

void ExQuaternion::SetEuler(float yaw,float pitch,float roll)
{
	float cosY = cosf(yaw/2.0f);
	float sinY = sinf(yaw/2.0f);
	float cosP = cosf(pitch/2.0f);
	float sinP = sinf(pitch/2.0f);
	float cosR = cosf(roll/2.0f);
	float sinR = sinf(roll/2.0f);

	qw=cosR*sinP*cosY+sinR*cosP*sinY;
	qx=cosR*cosP*sinY-sinR*sinP*cosY;
	qy=sinR*cosP*cosY-cosR*sinP*sinY;
	qz=cosR*cosP*cosY+sinR*sinP*sinY;

}
void ExQuaternion::CreateFromAxisAngle(float X, float Y, float Z, float degree) 
{ 
	// This function takes an angle and an axis of rotation, then converts
	// it to a quaternion.  An example of an axis and angle is what we pass into
	// glRotatef().  That is an axis angle rotation.  It is assumed an angle in 
	// degrees is being passed in.  Instead of using glRotatef(), we can now handle
	// the rotations our self.

	// The equations for axis angle to quaternions are such:

	// w = cos( theta / 2 )
	// x = X * sin( theta / 2 )
	// y = Y * sin( theta / 2 )
	// z = Z * sin( theta / 2 )

	// First we want to convert the degrees to radians 
	// since the angle is assumed to be in radians
	float angle = float((degree / 180.0f) * PI);

	// Here we calculate the sin( theta / 2) once for optimization
	float result = (float)sin( angle / 2.0f );
		
	// Calcualte the w value by cos( theta / 2 )
	qw = (float)cos( angle / 2.0f );

	// Calculate the x, y and z of the quaternion
	qx = float(X * result);
	qy = float(Y * result);
	qz = float(Z * result);
}

void ExQuaternion::CreateMatrix(float *pMatrix)
{
	// Make sure the matrix has allocated memory to store the rotation data
	if(!pMatrix) return;

	// This function is a necessity when it comes to doing almost anything
	// with quaternions.  Since we are working with OpenGL, which uses a 4x4
	// homogeneous matrix, we need to have a way to take our quaternion and
	// convert it to a rotation matrix to modify the current model view matrix.
	// We pass in a 4x4 matrix, which is a 1D array of 16 floats.  This is how OpenGL
	// allows us to pass in a matrix to glMultMatrixf(), so we use a single dimensioned array.
	// After about 300 trees murdered and 20 packs of chalk depleted, the
	// mathematicians came up with these equations for a quaternion to matrix converion:
	//
	//     ¦        2     2												 ¦
    //     ¦ 1 - (2y  + 2z )   2xy + 2zw         2xz - 2yw			0	 ¦
    //     ¦															 ¦
    //     ¦                          2     2							 ¦
    // M = ¦ 2xy - 2zw         1 - (2x  + 2z )   2zy + 2xw			0	 ¦
    //     ¦															 ¦
    //     ¦                                            2     2			 ¦
    //     ¦ 2xz + 2yw         2yz - 2xw         1 - (2x  + 2y )	0	 ¦
    //     ¦															 ¦
	//     ¦															 ¦
	//     ¦ 0				   0				 0					1	 |													 ¦
	//     ¦															 ¦
	// 
	// This is of course a 4x4 matrix.  Notice that a rotational matrix can just
	// be a 3x3 matrix, but since OpenGL uses a 4x4 matrix, we need to conform to the man.
	// Remember that the identity matrix of a 4x4 matrix has a diagonal of 1's, where
	// the rest of the indices are 0.  That is where we get the 0's lining the sides, and
	// the 1 at the bottom-right corner.  Since OpenGL matrices are row by column, we fill
	// in our matrix accordingly below.
	
	// First row
	pMatrix[ 0] = 1.0f - 2.0f * ( qy * qy + qz * qz );  
	pMatrix[ 1] = 2.0f * ( qx * qy - qw * qz );  
	pMatrix[ 2] = 2.0f * ( qx * qz + qw * qy );  
	pMatrix[ 3] = 0.0f;  

	// Second row
	pMatrix[ 4] = 2.0f * ( qx * qy + qw * qz );  
	pMatrix[ 5] = 1.0f - 2.0f * ( qx * qx + qz * qz );  
	pMatrix[ 6] = 2.0f * ( qy * qz - qw * qx );  
	pMatrix[ 7] = 0.0f;  

	// Third row
	pMatrix[ 8] = 2.0f * ( qx * qz - qw * qy );  
	pMatrix[ 9] = 2.0f * ( qy * qz + qw * qx );  
	pMatrix[10] = 1.0f - 2.0f * ( qx * qx + qy * qy );  
	pMatrix[11] = 0.0f;  

	// Fourth row
	pMatrix[12] = 0;  
	pMatrix[13] = 0;  
	pMatrix[14] = 0;  
	pMatrix[15] = 1.0f;

	// Now pMatrix[] is a 4x4 homogeneous matrix that can be applied to an OpenGL Matrix
}

//////////////////////////////////////////////////////////////////////
// Operator
//////////////////////////////////////////////////////////////////////
ExQuaternion ExQuaternion::operator* (const ExQuaternion &Q)
{
	ExQuaternion result;
	result.qw=(Q.qw*qw)-(Q.qx*qx)-(Q.qy*qy)-(Q.qz*qz);
	result.qx=(Q.qw*qx)-(Q.qx*qw)-(Q.qy*qz)-(Q.qz*qy);
	result.qy=(Q.qw*qy)-(Q.qy*qw)-(Q.qz*qx)-(Q.qx*qz);
	result.qz=(Q.qw*qz)-(Q.qz*qw)-(Q.qx*qy)-(Q.qz*qx);
	return result;
}
ExQuaternion ExQuaternion::operator+ (const ExQuaternion &Q)
{
	 return ExQuaternion(qw+Q.qw,qx+Q.qx,qy+Q.qy,qz+Q.qz);
}
//////////////////////////////////////////////////////////////////////
// friends methode
//////////////////////////////////////////////////////////////////////

std::ostream& operator<<(std::ostream& s,const ExQuaternion &q)
{
Guard(friend std::ostream& operator<<(std::ostream& s,const ExQuaternion &q))
	s<<"W:"<<q.qw<<"X:"<<q.qx<<"Y:"<<q.qy<<"Z:"<<q.qz<<std::endl;
	return s;
UnGuard
}
