/*
 * 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: ExMath.cpp,v 1.8 2002/07/21 18:49:33 binny Exp $
 *
 */

#include "ExMath.h"

ExCMatrix4x4 MatriceByVec3D(const ExCMatrix4x4 &m,const ExCVec3D &v)
{
	ExCMatrix4x4 RetMat;
	return RetMat;
}

float GetDotProduct(const ExCVec3D& Vec1,const ExCVec3D& Vec2)
{
	return((Vec1.m_Vector[0]*Vec2.m_Vector[0])+(Vec1.m_Vector[1]*Vec2.m_Vector[1])+(Vec1.m_Vector[2]*Vec2.m_Vector[2]));
}

ExCVec3D GetCrossProduct(const ExCVec3D& Vec1,const ExCVec3D& Vec2)
{
	ExCVec3D CrossProduct;
	CrossProduct.m_Vector[0]=((Vec1.m_Vector[1]*Vec2.m_Vector[2])-(Vec1.m_Vector[2]*Vec2.m_Vector[1]));
	CrossProduct.m_Vector[1]=((Vec1.m_Vector[2]*Vec2.m_Vector[0])-(Vec1.m_Vector[0]*Vec2.m_Vector[2]));
	CrossProduct.m_Vector[2]=((Vec1.m_Vector[0]*Vec2.m_Vector[1])-(Vec1.m_Vector[1]*Vec2.m_Vector[0]));
	return CrossProduct;
}

ExCVec3D GetVecNormale(const ExCVec3D& Vec1)
{
	ExCVec3D VecNorm;
	VecNorm=Vec1;
	VecNorm=VecNorm/VecNorm.GetVectorLenght();
	return VecNorm;
}

ExCMatrix4x4 GetMatrixFromQuaternion(const ExQuaternion& Q)
{
	ExCMatrix4x4 RetMatrix;
	
	float w=Q.qw;
	float x=Q.qx;
	float y=Q.qy;
	float z=Q.qz;


	float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;


	// calculate coefficients
	x2 = Q.qx + Q.qx; y2 = Q.qy + Q.qy; 
	z2 = Q.qz + Q.qz;
	xx = Q.qx * x2;   xy = Q.qx * y2;   xz = Q.qx * z2;
	yy = Q.qy * y2;   yz = Q.qy * z2;   zz = Q.qz * z2;
	wx = Q.qw * x2;   wy = Q.qw * y2;   wz = Q.qw * z2;


	RetMatrix.m_Matrix[0] = 1.0 - (yy + zz); 	
	RetMatrix.m_Matrix[1] = xy - wz;
	RetMatrix.m_Matrix[2] = xz + wy;		
	RetMatrix.m_Matrix[3] = 0.0;
	 
	RetMatrix.m_Matrix[4] = xy + wz;		
	RetMatrix.m_Matrix[5] = 1.0 - (xx + zz);
	RetMatrix.m_Matrix[6] = yz - wx;		
	RetMatrix.m_Matrix[7] = 0.0;


	RetMatrix.m_Matrix[8] = xz - wy;		
	RetMatrix.m_Matrix[9] = yz + wx;
	RetMatrix.m_Matrix[10] = 1.0 - (xx + yy);		
	RetMatrix.m_Matrix[11] = 0.0;


	RetMatrix.m_Matrix[12] = 0;			
	RetMatrix.m_Matrix[13] = 0;
	RetMatrix.m_Matrix[14] = 0;			
	RetMatrix.m_Matrix[15] = 1;


	return RetMatrix;
}

ExQuaternion GetQuaternionFromEuler(float x,float y,float z)
{
	ExQuaternion QResult;
	double roll= DegreesToRadians(x);
	double pitch = DegreesToRadians(y);
	double yaw = DegreesToRadians(z);

	double cyaw,cpitch,croll,syaw,spitch,sroll;
	double cyawcpitch,syawspitch,cyawspitch,syawcpitch;

	cyaw = cos(0.5f * yaw);
	cpitch = cos(0.5f * pitch);
	croll = cos(0.5f * roll);
	syaw = sin(0.5f * yaw);
	spitch = sin(0.5f * pitch);
	sroll = sin(0.5f * roll);

	cyawcpitch = cyaw*cpitch;
	syawspitch = syaw*pitch;
	cyawspitch = cyaw*spitch;
	syawcpitch = syaw*cpitch;

	
	QResult.qw=(float)(cyawcpitch * croll + syawspitch * sroll);
	QResult.qx=(float)(cyawcpitch * sroll - syawspitch * croll);
	QResult.qy=(float)(cyawspitch * croll + syawcpitch * sroll);
	QResult.qz=(float)(cyawcpitch * croll - cyawspitch * sroll);
	return QResult;
}

ExCMatrix4x4 GetMatrixFromEuler(float roll,float pitch,float yaw)
{
Guard(ExCMatrix4x4 GetMatrixFromEuler(float roll,float pitch,float yaw))
	ExCMatrix4x4 RetMatrix;
	
	float A,B,C,D,E,F,AD,BD;
			
	A=Cos[(int)roll];
	//cout<<"cos roll "<<A<<endl;
	B=Sin[(int)roll];
	//cout<<"sin roll "<<B<<endl;
	C=Cos[(int)pitch];
	//cout<<"cos pitch "<<C<<endl;
	D=Sin[(int)pitch];
	//cout<<"sin pitch "<<D<<endl;
	E=Cos[(int)yaw];
	//cout<<"cos yaw "<<E<<endl;
	F=Sin[(int)yaw];
	//cout<<"sin yaw "<<F<<endl;

	AD=A*D;
	BD=B*D;


	RetMatrix.m_Matrix[0] =	C*E;
	RetMatrix.m_Matrix[1] =-C*F;
	RetMatrix.m_Matrix[2] =-D;
	RetMatrix.m_Matrix[3] =0.0;

	RetMatrix.m_Matrix[4] =-BD * E + A * F;
	RetMatrix.m_Matrix[5] = BD * F + A * E;
	RetMatrix.m_Matrix[6] =-B * C;
	RetMatrix.m_Matrix[7] =0.0;

	RetMatrix.m_Matrix[8] = AD * E + B * F;
	RetMatrix.m_Matrix[9] =-AD * F + B * E;
	RetMatrix.m_Matrix[10]= A * C;
	RetMatrix.m_Matrix[11]=0.0;

	RetMatrix.m_Matrix[12]=0.0;
	RetMatrix.m_Matrix[13]=0.0;
	RetMatrix.m_Matrix[14]=0.0;
	RetMatrix.m_Matrix[15]=1.0;

	return RetMatrix;
UnGuard
}

void NormalizePlane(float Plane[6][4], int side)
{
	float magnitude = (float)sqrt( Plane[side][0] * Plane[side][0] + 
								   Plane[side][1] * Plane[side][1] + 
								   Plane[side][2] * Plane[side][2] );
    Plane[side][0] /= magnitude;
	Plane[side][1] /= magnitude;
	Plane[side][2] /= magnitude;
	Plane[side][3] /= magnitude; 
}

ExCVec3D GetAxisFromQuaternion(const ExQuaternion& Q)
{
	ExCVec3D vec;
	float m;
	vec.SetValue(Q.qx,Q.qy,Q.qz);
	m=vec.GetVectorLenght();

	return vec/m;


}

ExCVec3D  GetNewVecFromEuler(ExCVec3D force,float roll,float pitch,float yaw)
{
	ExCVec3D Result,VecX,VecY,VecZ;

	//roll angle x
	//pitch angle y
	//yaw angle z

	// calcul new vec pour x
	/*VecX.m_Vector[0]=force.m_Vector[0]*cos(pitch);
	VecX.m_Vector[1]=0.0f;
	VecX.m_Vector[2]=force.m_Vector[0]*sin(pitch);
*/
	VecY.m_Vector[0]=0.0f;
	VecY.m_Vector[1]=force.m_Vector[1]*sin(roll);
	VecY.m_Vector[2]=force.m_Vector[1]*cos(roll);

	VecZ.m_Vector[0]=force.m_Vector[2]*cos(yaw);
	VecZ.m_Vector[1]=force.m_Vector[2]*sin(yaw);
	VecZ.m_Vector[2]=0.0f;

	Result=VecX+VecY+VecZ;

	//std::cout<<"leng of result:"<<Result.GetVectorLenght()<<std::endl;
	//std::cout<<"leng of force:"<<force.GetVectorLenght()<<std::endl;

	//std::cout<<"Result:"<<Result<<std::endl;
	//std::cout<<"Force:"<<force<<std::endl;
	return Result;
}

void ProjectPointOnPlane( ExCVec3D dst, const ExCVec3D p, const ExCVec3D normal )
{
	float d;
	ExCVec3D n;
	float inv_denom;

	inv_denom = 1.0F / GetDotProduct( normal, normal );

	d = GetDotProduct( normal, p ) * inv_denom;

	n.m_Vector[0] = normal.m_Vector[0] * inv_denom;
	n.m_Vector[1] = normal.m_Vector[1] * inv_denom;
	n.m_Vector[2] = normal.m_Vector[2] * inv_denom;

	dst.m_Vector[0] = p.m_Vector[0] - d * n.m_Vector[0];
	dst.m_Vector[1] = p.m_Vector[1] - d * n.m_Vector[1];
	dst.m_Vector[2] = p.m_Vector[2] - d * n.m_Vector[2];
}

/*
** assumes "src" is normalized
*/
void PerpendicularVector( ExCVec3D dst, const ExCVec3D src )
{
	int	pos;
	int i;
	float minelem = 1.0F;
	ExCVec3D tempvec;

	/*
	** find the smallest magnitude axially aligned vector
	*/
	for ( pos = 0, i = 0; i < 3; i++ )
	{
		if ( fabs( src.m_Vector[i] ) < minelem )
		{
			pos = i;
			minelem = fabs( src.m_Vector[i] );
		}
	}
	tempvec.SetValue(0.0f,0.0f,0.0f);
	tempvec.m_Vector[pos] = 1.0F;

	/*
	** project the point onto the plane defined by src
	*/
	ProjectPointOnPlane( dst, tempvec, src );

	/*
	** normalize the result
	*/
	dst.GetVecNormale();
}

float getRandomMinMax( float fMin, float fMax )
{
    float fRandNum = (float)rand () / RAND_MAX;
    return fMin + (fMax - fMin) * fRandNum;
}

ExCVec3D getRandomVector( void )
{
	ExCVec3D Vec;

    // Pick a random Z between -1.0f and 1.0f.
    Vec.m_Vector[2] = getRandomMinMax( -1.0f, 1.0f );
    
    // Get radius of this circle
    float radius = (float)sqrt(1 - Vec.m_Vector[2] * Vec.m_Vector[2]);
    
    // Pick a random point on a circle.
    float t = getRandomMinMax( -PI, PI );

    // Compute matching X and Y for our Z.
    Vec.m_Vector[0] = (float)cosf(t) * radius;
    Vec.m_Vector[1]= (float)sinf(t) * radius;

	return Vec;
}

float DistanceBeteweenTwoPoint(ExCVec3D p1,ExCVec3D p2)
{
	return sqrt(fabsf((p2.GetX()-p1.GetX())+(p2.GetY()-p1.GetY())+(p2.GetZ()-p1.GetZ())));
}