#include "MathGlobal.h"
#ifdef _WIN32
#include <math.h>
#include <float.h>
#include <algorithm>
#else
#include <math.h>
#include <float.h>
#include <algo.h>
#endif

//using namespace MG;

/// Rota un vector Direccion 0,0,1 como le indica el quaternion
void MG::quaternion2Dir(float* q,float* vDir){
	
	float m[16];
	float z[3];

	z[0]=0.0f;z[1]=0.0f;z[2]=1.0f;
	/// Obtenemos la Matriz a partir del Quaternion
	// Primera Columna
	m[0] = 1.0f - 2.0f * ( q[1] * q[1] + q[2] * q[2] ); 
	m[4] = 2.0f * (q[0] * q[1] + q[2] * q[3]);
	m[8] = 2.0f * (q[0] * q[2] - q[1] * q[3]);
	m[12] = 0.0f;  

	// Segunda Columna
	m[1] = 2.0f * ( q[0] * q[1] - q[2] * q[3] );  
	m[5] = 1.0f - 2.0f * ( q[0] * q[0] + q[2] * q[2] ); 
	m[9] = 2.0f * (q[2] * q[1] + q[0] * q[3] );  
	m[13] = 0.0f;  

	// Tercera Columna
	m[2] = 2.0f * ( q[0] * q[2] + q[1] * q[3] );
	m[6] = 2.0f * ( q[1] * q[2] - q[0] * q[3] );
	m[10] = 1.0f - 2.0f * ( q[0] * q[0] + q[1] * q[1] );  
	m[14] = 0.0f;  

	// Cuarta Columna
	m[3] = 0;  
	m[7] = 0;  
	m[11] = 0;  
	m[15] = 1.0f;

	multVM(z,m,vDir);
}

float MG::dot(float* v1,float* v2){
	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
}

void MG::mult(float* v1,float* v2,float* vResult){
	vResult[0] = v1[1] * v2[2] - v1[2] * v2[1];
	vResult[1] = v1[2] * v2[0] - v1[0] * v2[2];
	vResult[2] = v1[0] * v2[1] - v1[1] * v2[0];
}

void MG::multVM(float* vect,float* matrix,float* vResult){
	vResult[0] = matrix[0] * vect[0] + matrix[4] * vect[1] + matrix[8] * vect[2];
	vResult[1] = matrix[1] * vect[0] + matrix[5] * vect[1] + matrix[9] * vect[2];
	vResult[2] = matrix[2] * vect[0] + matrix[6] * vect[1] + matrix[10] * vect[2];
}

void MG::multPM(float* vect,float* matrix,float* vResult){
	vResult[0] = matrix[0] * vect[0] + matrix[4] * vect[1] + matrix[8] * vect[2] + matrix[12];
	vResult[1] = matrix[1] * vect[0] + matrix[5] * vect[1] + matrix[9] * vect[2] + matrix[13];
	vResult[2] = matrix[2] * vect[0] + matrix[6] * vect[1] + matrix[10] * vect[2] + matrix[14];	
}


void MG::normalize(float* v){
	float fLength = 1.f / (float)( sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]) + FLT_EPSILON );

	v[0] *= fLength;
	v[1] *= fLength;
	v[2] *= fLength;
}

void MG::sub(float* v1,float* v2,float* vRes){
	vRes[0] = v1[0] - v2[0];
	vRes[1] = v1[1] - v2[1];
	vRes[2] = v1[2] - v2[2];
}

float MG::length(float* v){
	return (float)(sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));	
}