#include "palFactory.h"

/*
	Abstract:
		PAL - Physics Abstraction Layer. 
		Implementation File (geom)

	Author: 
		Adrian Boeing
	Revision History:
		Version 0.1 :19/10/07 split from pal.cpp
	TODO:
*/

#ifdef MEMDEBUG
#include <crtdbg.h>
#define new new(_NORMAL_BLOCK,__FILE__, __LINE__)
#endif

void palGeometry::GetPosition(palVector3& res) {
	palMatrix4x4 loc;
	loc = GetLocationMatrix();
	res.x=loc._41;
	res.y=loc._42;
	res.z=loc._43;
}
////////////////////////////////////////
//step1: set the m_pbody
//step2: call init -> 
//		set type
//		set position
//		set properties
//		set mass
//		calculate inertia
//OR: set pos, set properties & mass

palGeometry::palGeometry() {
	m_pBody=NULL;
	mat_identity(&m_mOffset);
}
/*
void palGeometry::SetPosition(Float x, Float y, Float z) {
	m_fPosX = x;
	m_fPosY = y;
	m_fPosZ = z;
	palMatrix4x4 loc;
	memset(loc._mat,0,sizeof(palMatrix4x4));
	loc._11=1; loc._22=1; loc._33=1; loc._44=1;
	loc._41=x;
	loc._42=y;
	loc._43=z;
	SetPosition(loc);
}
*/

palMatrix4x4& palGeometry::GetLocationMatrix() {
	palMatrix4x4 bodyloc=m_pBody->GetLocationMatrix();
	mat_multiply(&m_mLoc,&bodyloc,&m_mOffset);
	return m_mLoc;
}


void palGeometry::SetPosition(palMatrix4x4 &location) {
	m_mLoc = location;
	if (m_pBody) {
		palMatrix4x4 temp;
		palMatrix4x4 bodyloc=m_pBody->GetLocationMatrix();
		mat_invert(&temp,&bodyloc);
		mat_multiply(&m_mOffset,&temp,&m_mLoc);
	}
}

void palGeometry::SetMass(Float mass) {
	m_fMass = mass;
	CalculateInertia();
}

Float palGeometry::GetMass() {
	return m_fMass;
}
/*
void palGeometry::GenericInit(void* params, ...) {
	va_list arg_ptr;
	va_start(params, arg_ptr);
	iGenericInit(params, arg_ptr);
	va_end(arg_ptr);
};
*/
//palSphereGeometry::palSphereGeometry(palBody *pbody) {	m_pBody = pbody;}

//void palSphereGeometry::Init(Float x, Float y, Float z, Float radius, Float mass) {
void palSphereGeometry::Init(palMatrix4x4 &pos, Float radius, Float mass) {
	m_Type = PAL_GEOM_SPHERE;
	palGeometry::SetPosition(pos);//m_Loc = pos;
//	palGeometry::SetPosition(x,y,z);
	m_fRadius = radius;
	palGeometry::SetMass(mass);
	CalculateInertia();
}

void palSphereGeometry::GenericInit(palMatrix4x4 &pos, void *param_array) {
	Float *p = (Float *)param_array;
	Init(pos,p[0],p[1]);
}

void palBoxGeometry::GenericInit(palMatrix4x4 &pos, void *param_array) {
	Float *p = (Float *)param_array;
	Init(pos,p[0],p[1],p[2],p[3]);
}

void palCylinderGeometry::GenericInit(palMatrix4x4 &pos, void *param_array) {
	Float *p = (Float *)param_array;
	Init(pos,p[0],p[1],p[2]);
}

/*
void palSphereGeometry::iGenericInit(void *param,va_list arg_ptr) {
}
*/

void palSphereGeometry::CalculateInertia() {
	Float i = 2 * m_fRadius *  m_fRadius / 5;
	m_fInertiaXX = m_fMass * i;
	m_fInertiaYY = m_fMass * i;
	m_fInertiaZZ = m_fMass * i;
}

//palBoxGeometry::palBoxGeometry(palBody *pbody) {	m_pBody = pbody;}

//void palBoxGeometry::Init(Float x, Float y, Float z, Float width, Float height, Float depth, Float mass) {
void palBoxGeometry::Init(palMatrix4x4 &pos, Float width, Float height, Float depth, Float mass) {
	m_Type = PAL_GEOM_BOX;
	//palGeometry::SetPosition(x,y,z);
	palGeometry::SetPosition(pos);//m_Loc = pos;
	m_fWidth = width;
	m_fHeight = height;
	m_fDepth = depth;
	palGeometry::SetMass(mass);
	CalculateInertia();
}

void palBoxGeometry::CalculateInertia() {
	Float lx = m_fWidth;
	Float ly = m_fHeight;
	Float lz = m_fDepth;
/*	Float i0= 1/(12 * (ly*ly + lz*lz));
	Float i1= 1/(12 * (lx*lx + lz*lz));
	Float i2= 1/(12 * (lx*lx + ly*ly));*/

	Float i0= (1/Float(12)) * (ly*ly + lz*lz);
	Float i1= (1/Float(12)) * (lx*lx + lz*lz);
	Float i2= (1/Float(12)) * (lx*lx + ly*ly);

	m_fInertiaXX = m_fMass * i0;
	m_fInertiaYY = m_fMass * i1;
	m_fInertiaZZ = m_fMass * i2;
}

/*
   float mass = 3.0f; 
   float Ixx = 0.7f * mass * (y * y + z * z) / 12.0f; 
   float Iyy = 0.7f * mass * (x * x + z * z) / 12.0f; 
   float Izz = 0.7f * mass * (x * x + y * y) / 12.0f; 
   NewtonBodySetMassMatrix (body, mass, Ixx, Iyy, Izz);
*/

//void palCylinderGeometry::Init(Float x, Float y, Float z, Float radius, Float length, Float mass) {
void palCylinderGeometry::Init(palMatrix4x4 &pos, Float radius, Float length, Float mass) {
	m_Type = PAL_GEOM_CYLINDER;
//	palGeometry::SetPosition(x,y,z);
	palGeometry::SetPosition(pos);//m_Loc = pos;
	m_fRadius = radius;
	m_fLength = length;
	palGeometry::SetMass(mass);
	CalculateInertia();
}

void palCylinderGeometry::CalculateInertia() {
	Float d = m_fRadius*2;
	Float i0 = 1/Float(48) * (3 * d * d + 4 * m_fLength * m_fLength);
	Float i2 = d * d / Float(8);
	m_fInertiaXX = m_fMass * i0;
	m_fInertiaYY = m_fMass * i0;
	m_fInertiaZZ = m_fMass * i2;
}


void palConvexGeometry::Init(palMatrix4x4 &pos, const Float *pVertices, int nVertices, Float mass) {
	m_Type = PAL_GEOM_CONVEX;
	palGeometry::SetPosition(pos);//m_Loc = pos;
	palGeometry::SetMass(mass);
	m_vfVertices.resize(nVertices*3);
	for (int i=0;i<nVertices*3;i++) {
		m_vfVertices[i] = pVertices[i];
	}
	//CalculateInertia();
}

