#include "palFactory.h"

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

	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
////////////////////////////////////////
palBody::palBody() {
	m_pMaterial = NULL;
	memset(&m_mLoc,0,sizeof(palMatrix4x4));
	m_mLoc._11 = 1;
	m_mLoc._22 = 1;
	m_mLoc._33 = 1;
	m_mLoc._44 = 1;
}

void palBody::Cleanup() {
	while (!m_Geometries.empty()) {
			delete *(m_Geometries.begin());
			m_Geometries.erase(m_Geometries.begin());
	}
	//how to find the links which reference me?
}

void palBody::SetPosition(Float x, Float y, Float z) {
	m_fPosX = x;
	m_fPosY = y;
	m_fPosZ = z;
	palMatrix4x4 loc = GetLocationMatrix();
	//don't reset position
	//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;
	loc._44=1;
	SetPosition(loc);
}

void palBody::SetOrientation(Float roll, Float pitch, Float yaw) {
	Float sinroll = (Float)sin(roll), cosroll = (Float)cos(roll);
	Float sinpitch = (Float)sin(pitch), cospitch = (Float)cos(pitch);
	Float sinyaw = (Float)sin(yaw), cosyaw = (Float)cos(yaw);

	palMatrix4x4 loc = GetLocationMatrix();
	

	loc._11= cosroll*cosyaw;
	loc._12= cosroll*sinyaw*sinpitch - sinroll*cospitch;
	loc._13= sinroll*sinpitch + cosroll*sinyaw*cospitch;
	loc._14= 0.0f;
	loc._21= sinroll*cosyaw;
	loc._22= cosroll*cospitch + sinroll*sinyaw*sinpitch;
	loc._23= sinroll*sinyaw*cospitch - cosroll*sinpitch;
	loc._24= 0.0f;
	loc._31= -sinyaw;
	loc._32= cosyaw*sinpitch;
	loc._33= cosyaw*cospitch;
	loc._34= 0.0f;
	//dont adjust position
	loc._44= 1.0f;
	SetPosition(loc);
}

void palBody::SetPosition(Float x, Float y, Float z, Float roll, Float pitch, Float yaw) {
	m_fPosX = x;
	m_fPosY = y;
	m_fPosZ = z;

	Float sinroll = (Float)sin(roll), cosroll = (Float)cos(roll);
	Float sinpitch = (Float)sin(pitch), cospitch = (Float)cos(pitch);
	Float sinyaw = (Float)sin(yaw), cosyaw = (Float)cos(yaw);

	palMatrix4x4 loc;
	

	loc._11= cosroll*cosyaw;
	loc._12= cosroll*sinyaw*sinpitch - sinroll*cospitch;
	loc._13= sinroll*sinpitch + cosroll*sinyaw*cospitch;
	loc._14= 0.0f;
	loc._21= sinroll*cosyaw;
	loc._22= cosroll*cospitch + sinroll*sinyaw*sinpitch;
	loc._23= sinroll*sinyaw*cospitch - cosroll*sinpitch;
	loc._24= 0.0f;
	loc._31= -sinyaw;
	loc._32= cosyaw*sinpitch;
	loc._33= cosyaw*cospitch;
	loc._34= 0.0f;
	loc._41= x;
	loc._42= y;
	loc._43= z;
	loc._44= 1.0f;
	SetPosition(loc);
	
}

void palBody::GetPosition(palVector3& res) {
	palMatrix4x4 loc;
	loc = GetLocationMatrix();
	res.x=loc._41;
	res.y=loc._42;
	res.z=loc._43;
} 	

void palBody::SetPosition(palMatrix4x4 &location) {
	m_mLoc = location;
}

//void SetPosition(Float x, Float y, Float z);
//void SetPosition(Float x, Float y, Float z, Float roll, Float pitch, Float yaw);
/*
palMatrix4x4& palBody::GetLocationMatrix() {
	return m_mLoc;
}
*/
void palBody::SetMaterial(palMaterial *material) {
	m_pMaterial = material;
}


void palBody::ApplyImpulse(Float fx, Float fy, Float fz) {
	Float im = 1/m_fMass;
	palVector3 v;
	GetLinearVelocity(v);
	v.x += fx * im;
	v.y += fy * im;
	v.z += fz * im;
	SetLinearVelocity(v);
}

void palBody::ApplyAngularImpulse(Float ix, Float iy, Float iz) {
	palVector3 ii;
	vec_set(&ii,ix,iy,iz);

	palVector3 invInertia;
	vec_set(&invInertia,1/m_Geometries[0]->m_fInertiaXX,
						1/m_Geometries[0]->m_fInertiaYY, 
						1/m_Geometries[0]->m_fInertiaZZ);
	

	palMatrix4x4 pos;
	pos=this->GetLocationMatrix();

	palMatrix4x4 inertiaScaled;
	inertiaScaled=this->GetLocationMatrix();
	mat_scale3x3(&inertiaScaled,&invInertia);

	palMatrix4x4 posT;
	mat_transpose(&posT,&pos);

	palMatrix4x4 inertiaWorld;
	mat_multiply(&inertiaWorld,&inertiaScaled,&posT);

	palVector3 angularvel;
	vec_mat_mul(&angularvel,&inertiaWorld,&ii);
//	printPalVector(angularvel);
	
	palVector3 v;
	GetAngularVelocity(v);
	palVector3 sum;
	vec_add(&sum,&angularvel,&v);
	SetAngularVelocity(sum);
}

void palBody::ApplyImpulseAtPosition(Float px, Float py, Float pz, Float ix, Float iy, Float iz) {
	ApplyImpulse(ix,iy,iz);

	palVector3 f,q,p,bpos,tadd;
	f.x=ix; f.y=iy; f.z=iz;
	p.x=px; p.y=py; p.z=pz;
	GetPosition(bpos);
	vec_sub(&q,&p,&bpos);
	vec_cross(&tadd,&q,&f);
	ApplyAngularImpulse(tadd.x,tadd.y,tadd.z);
}

void palBody::ApplyForceAtPosition(Float px, Float py, Float pz, Float fx, Float fy, Float fz) {
	//based off code from ODE
	ApplyForce(fx,fy,fz);
	palVector3 f,q,p,bpos,tadd;
	f.x=fx; f.y=fy; f.z=fz;
	p.x=px; p.y=py; p.z=pz;
	GetPosition(bpos);
	vec_sub(&q,&p,&bpos);
	vec_cross(&tadd,&q,&f);
	ApplyTorque(tadd.x,tadd.y,tadd.z);
}


void palBody::ApplyForce(Float fx, Float fy, Float fz) {
	Float ts=PF->GetActivePhysics()->GetLastTimestep();
	ApplyImpulse(fx*ts,fy*ts,fz*ts);
}

void palBody::ApplyTorque(Float tx, Float ty, Float tz) {
	Float ts=PF->GetActivePhysics()->GetLastTimestep();
	ApplyAngularImpulse(tx*ts,ty*ts,tz*ts);
}

#if 0
void palBody::SetForce(Float fx, Float fy, Float fz) {
	m_fForceX = fx;
	m_fForceY = fy;
	m_fForceZ = fz;
}


void palBody::AddForce(Float fx, Float fy, Float fz) {
	palVector3 force;
	GetForce(force);
	SetForce(force.x+fx,force.y+fy,force.z+fz);
}


void palBody::SetTorque(Float tx, Float ty, Float tz) {
	m_fTorqueX = tx;
	m_fTorqueY = ty;
	m_fTorqueZ = tz;
}

void palBody::AddTorque(Float tx, Float ty, Float tz) {
	palVector3 torque;
	GetTorque(torque);
	SetTorque(torque.x+tx,torque.y+ty,torque.z+tz);
}
#endif

void palBody::SetGeometryBody(palGeometry *pgeom) {
	if (pgeom)
		pgeom->m_pBody=this;
}
/////////////////////////