
#include "MPhysicsCallbacks.h"

static int getBuoyancyPlane (const int collisionID, void *context, const dFloat* globalSpaceMatrix, dFloat* globalSpacePlane)
{
   matrix4 mat = *(matrix4*)globalSpaceMatrix;
   globalSpacePlane[0] = 0.0f;
   globalSpacePlane[1] = 1.0f;
   globalSpacePlane[2] = 0.0f;
   globalSpacePlane[3] = - rand() * 10;

   return 1;   
}

void applyBouyancyForce (const NewtonBody* body)
{
	CPhysicsEntity *tmp;
	
	// make sure the body is active is autoactive state
	NewtonBodySetAutoFreeze  (body, 1);
	tmp = (CPhysicsEntity *)NewtonBodyGetUserData(body);
	if (tmp->waterBody) {
		// the body is is water. we want to keepit active while is in it
		// set autoactive off
		NewtonBodySetAutoFreeze (body, 0);
		// make the body active
		NewtonWorldUnfreezeBody (tmp->getWorld(), body);

		// apply the bouyancy force 
		vector3df gravity (0.0f, 9.8f * 10, 0.0f);
		NewtonBodyAddBuoyancyForce (body, 10.0f, 0.3f, 0.3f, &gravity.X, NULL, NULL);

		// reset the pointer to water volume
		tmp->waterBody = NULL;
	}
 }



// callback for applying gravity - formally PhysicsApplyForceAndTorque
void _cdecl physicsApplyGravity(const NewtonBody* body)
{
	f32 mass;
	f32 Ixx;
	f32 Iyy;
	f32 Izz;

    // get the body's mass, apply gravity
	NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
	
	vector3df force(0.0f, -9.8f * mass, 0.0f);

	NewtonBodySetForce (body, &force.X);
}


// set the transformation of a rigid body (when a physics body moves, move the visual representation too)
void _cdecl physicsSetTransform (const NewtonBody* body, const float* matrix)
{
    matrix4 mat;
    memcpy(mat.pointer(), matrix, sizeof(float)*16);

	// get the graphic object form the rigid body
	CPhysicsEntity *entity = (CPhysicsEntity *) NewtonBodyGetUserData(body);

	if (entity->getNode())
	{
		entity->getNode()->setPosition(mat.getTranslation() * NewtonToIrr);

		vector3df euler;
		NewtonGetEulerAngle (mat.pointer(), &euler.X); // Irrlicht 1.3
		entity->getNode()->setRotation(euler * EulerMultiple);
	}
}


void applyForceAndTorqueEvent (const NewtonBody* body) 
{ 
	float mass; 
	float Ixx; 
	float Iyy; 
	float Izz; 

	NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz); 

	CPhysicsEntity *entity = (CPhysicsEntity *) NewtonBodyGetUserData(body);

	vector3df force (0.0f, -9.8f * mass, 0.0f); 
	vector3df torque (0.0f, 0.0f, 0.0f);  

	force += entity->forceAdd;
	torque += entity->forceAdd;
   
	NewtonBodyAddForce (body, &force.X); 
	NewtonBodyAddTorque (body, &torque.X); 

	if (entity->forceSet!=vector3df(0,0,0))
	   NewtonBodySetForce(body, &entity->forceSet.X);

	if (entity->torqueSet!=vector3df(0,0,0))
	   NewtonBodySetTorque(body, &entity->torqueSet.X);
	
	entity->forceAdd = vector3df(0,0,0);
	entity->forceSet = vector3df(0,0,0);

	entity->torqueAdd = vector3df(0,0,0);
	entity->torqueSet = vector3df(0,0,0);
	
	applyBouyancyForce(body);
}
