//********************************************************************
// This source file is part of Ninfa3D Engine
// 
// Copyright (C) 2008-2009 Viz_fuerte.
// All rights reserved.
// 
// LIMITED WARRANTY
// This software is provided "as-is", without any express or implied warranty.
// In no event shall the author be held liable for any damages arising from the use of this software.
// This is a contract. By accepting the Ninfa3D Engine (the SOFTWARE), 
// you (either an individual or a single entity, the LICENSEE) accept the terms and conditions of this licensing agreement.
// 
// If the LICENSEE does not accept the terms and conditions, then LICENSEE must not accept the SOFTWARE.
// If at a later time the LICENSEE decides not to continue licensing the SOFTWARE under these terms and 
// conditions, then the LICENSEE must delete permanently all files constituting the SOFTWARE.
//
// File: CBodySceneNode.cpp
//********************************************************************

/*--------------------------------------------------------------------
Includos*/

	#include "CBodySceneNode.h"

/*--------------------------------------------------------------------
Variables externas*/

	extern NewtonWorld* nWorld;
	extern int defaultMaterialID;
	extern ISceneManager* SceneManager;
	extern IrrlichtDevice* device;

/*--------------------------------------------------------------------
Variables globales*/

	extern float GravityX;
	extern float GravityY;
	extern float GravityZ;

	float fDensity;
	float lViscosity;
	float aViscosity;

	array<NewtonBody*> gBody;

/*--------------------------------------------------------------------
Declaracion global de funciones*/

	void _cdecl SetMeshTransformEvent(const NewtonBody*, const float* matrix,int threadIndex);
	void _cdecl ApplyForceAndTorque(const NewtonBody* body,float timestep,int threadIndex);
	int PhysicsBouyancyPlane(const int collisionID,void *context,const dFloat* matrix,dFloat* globalSpacePlane);
	int ApplyBuoyancyAABBLevel (const NewtonMaterial* material, const NewtonBody* body0, const NewtonBody* body1, int threadIndex);
	
	//Constructor
	CBodySceneNode::CBodySceneNode(ISceneNode* parent,ISceneManager* mgr,s32 id):ISceneNode(parent,mgr,id)
	{
	}

	//Destructor
	CBodySceneNode::~CBodySceneNode()
	{
	}

	//Crea un cuerpo vacio
	void CBodySceneNode::addBodyNull()
	{
		NewtonCollision* collision = NewtonCreateNull(nWorld);
		body = NewtonCreateBody(nWorld,collision);
		NewtonReleaseCollision(nWorld,collision);
	}

	//Crea un cuerpo con forma de cubo
	void CBodySceneNode::addBodyCube(float dx,float dy,float dz,float mass)
	{
		float Ixx,Iyy,Izz;
		dVector origin,inertia;

		NewtonCollision* collision = NewtonCreateBox(nWorld,dx,dy,dz,0,NULL);
		NewtonConvexCollisionCalculateInertialMatrix(collision,&inertia[0],&origin[0]);	

		Ixx = mass * inertia[0];
		Iyy = mass * inertia[1];
		Izz = mass * inertia[2];

		body = NewtonCreateBody(nWorld,collision);

		NewtonBodySetCentreOfMass(body,&origin[0]);
		NewtonBodySetMassMatrix(body,mass,Ixx,Iyy,Izz);

		NewtonBodySetTransformCallback(body,SetMeshTransformEvent);
		NewtonBodySetForceAndTorqueCallback(body,ApplyForceAndTorque);

		NewtonBodySetAutoSleep(body,1);
		NewtonBodySetFreezeState(body,0);

		NewtonBodySetMaterialGroupID(body,defaultMaterialID);

		NewtonReleaseCollision(nWorld,collision);
		gBody.push_back(body);
	}

	//Crea un cuerpo con forma de esfera
	void CBodySceneNode::addBodySphere(float rx,float ry,float rz,float mass)
	{
		float Ixx,Iyy,Izz;
		dVector origin,inertia;

		NewtonCollision* collision = NewtonCreateSphere(nWorld,rx,ry,rz,2,NULL);
		NewtonConvexCollisionCalculateInertialMatrix(collision,&inertia[0],&origin[0]);	

		Ixx = mass * inertia[0];
		Iyy = mass * inertia[1];
		Izz = mass * inertia[2];

		body = NewtonCreateBody(nWorld,collision);

		NewtonBodySetCentreOfMass(body,&origin[0]);
		NewtonBodySetMassMatrix(body,mass,Ixx,Iyy,Izz);

		NewtonBodySetTransformCallback(body,SetMeshTransformEvent);
		NewtonBodySetForceAndTorqueCallback(body,ApplyForceAndTorque);

		NewtonBodySetAutoSleep(body,1);
		NewtonBodySetFreezeState(body,0);

		NewtonBodySetMaterialGroupID(body,defaultMaterialID);

		NewtonReleaseCollision(nWorld,collision);
		gBody.push_back(body);
	}

	//Crea un cuerpo con forma de cono
	void CBodySceneNode::addBodyCone(float radius,float height,float mass,bool offset)
	{
		float Ixx,Iyy,Izz;
		dVector origin,inertia;
		NewtonCollision* collision;

		if(offset)
		{
			matrix4 mat;
			mat.setTranslation(vector3df(0.0f,height/2.0f,0.0f));
			mat.setRotationDegrees(vector3df(0.0f,0.0f,90.0f));

			collision = NewtonCreateCone(nWorld,radius,height,1,&mat.pointer()[0]);
		}
		else
			collision = NewtonCreateCone(nWorld,radius,height,1,NULL);

		NewtonConvexCollisionCalculateInertialMatrix(collision,&inertia[0],&origin[0]);	

		Ixx = mass * inertia[0];
		Iyy = mass * inertia[1];
		Izz = mass * inertia[2];

		body = NewtonCreateBody(nWorld,collision);

		NewtonBodySetCentreOfMass(body,&origin[0]);
		NewtonBodySetMassMatrix(body,mass,Ixx,Iyy,Izz);

		NewtonBodySetTransformCallback(body,SetMeshTransformEvent);
		NewtonBodySetForceAndTorqueCallback(body,ApplyForceAndTorque);

		NewtonBodySetAutoSleep(body,1);
		NewtonBodySetFreezeState(body,0);
		NewtonBodySetMaterialGroupID(body,defaultMaterialID);

		NewtonReleaseCollision(nWorld,collision);
		gBody.push_back(body);
	}

	//Crea un cuerpo con forma de cilindro
	void CBodySceneNode::addBodyCylinder(float radius,float height,float mass,bool offset)
	{
		float Ixx,Iyy,Izz;
		dVector origin,inertia;
		NewtonCollision* collision;

		if(offset)
		{
			matrix4 mat;
			mat.setTranslation(vector3df(0.0f,height/2.0f,0.0f));
			mat.setRotationDegrees(vector3df(0.0f,0.0f,90.0f));

			collision = NewtonCreateCylinder(nWorld,radius,height,4,&mat.pointer()[0]);
		}
		else
			collision = NewtonCreateCylinder(nWorld,radius,height,4,NULL);

		NewtonConvexCollisionCalculateInertialMatrix(collision,&inertia[0],&origin[0]);	

		Ixx = mass * inertia[0];
		Iyy = mass * inertia[1];
		Izz = mass * inertia[2];

		body = NewtonCreateBody(nWorld,collision);

		NewtonBodySetCentreOfMass(body,&origin[0]);
		NewtonBodySetMassMatrix(body,mass,Ixx,Iyy,Izz);

		NewtonBodySetTransformCallback(body, SetMeshTransformEvent);
		NewtonBodySetForceAndTorqueCallback(body,ApplyForceAndTorque);

		NewtonBodySetAutoSleep(body,1);
		NewtonBodySetFreezeState(body,0);
		NewtonBodySetMaterialGroupID(body,defaultMaterialID);

		NewtonReleaseCollision(nWorld,collision);
		gBody.push_back(body);
	}

	//Crea un cuerpo con forma de cilindro
	void CBodySceneNode::addBodyCapsule(float radius,float height,float mass,bool offset)
	{
		float Ixx,Iyy,Izz;
		dVector origin,inertia;
		NewtonCollision* collision;

		if(offset)
		{
			matrix4 mat;
			mat.setTranslation(vector3df(0.0f,height/2.0f,0.0f));
			mat.setRotationDegrees(vector3df(0.0f,0.0f,90.0f));

			collision = NewtonCreateCapsule(nWorld,radius,height,3,&mat.pointer()[0]);
		}
		else
			collision = NewtonCreateCapsule(nWorld,radius,height,3,NULL);

		NewtonConvexCollisionCalculateInertialMatrix(collision,&inertia[0],&origin[0]);	

		Ixx = mass * inertia[0];
		Iyy = mass * inertia[1];
		Izz = mass * inertia[2];

		body = NewtonCreateBody(nWorld,collision);

		NewtonBodySetCentreOfMass(body,&origin[0]);
		NewtonBodySetMassMatrix(body,mass,Ixx,Iyy,Izz);

		NewtonBodySetTransformCallback(body, SetMeshTransformEvent);
		NewtonBodySetForceAndTorqueCallback(body,ApplyForceAndTorque);

		NewtonBodySetAutoSleep(body,1);
		NewtonBodySetFreezeState(body,0);

		NewtonBodySetMaterialGroupID(body,defaultMaterialID);

		NewtonReleaseCollision(nWorld,collision);
		gBody.push_back(body);
	}

	void CBodySceneNode::addBodyHull(IAnimatedMeshSceneNode* mesh,float mass)
	{
		dVector origin;
		dVector inertia;

		float Ixx,Iyy,Izz;
		IAnimatedMesh *irr_mesh = mesh->getMesh();
		unsigned int nMeshBuffer = 0;
		IMeshBuffer *mesh_buffer = NULL;
		float* vertices;
		vector3df Scale = mesh->getScale();

		unsigned int nVertices = 0;

		for(nMeshBuffer=0;nMeshBuffer<irr_mesh->getMeshBufferCount();nMeshBuffer++ )
		{
			nVertices += irr_mesh->getMesh(0)->getMeshBuffer(nMeshBuffer)->getVertexCount();
		}

		vertices = new float[nVertices * 3];
		unsigned int tmpCounter = 0;

		for(nMeshBuffer=0;nMeshBuffer<irr_mesh->getMeshBufferCount();nMeshBuffer++ )
		{
			mesh_buffer = irr_mesh->getMeshBuffer(nMeshBuffer);
			S3DVertex *S3vertices = (S3DVertex*)mesh_buffer->getVertices();

			for(unsigned int i=0;i<mesh_buffer->getVertexCount();i++)
			{
				vertices[tmpCounter++] = S3vertices[i].Pos.X * Scale.X;
				vertices[tmpCounter++] = S3vertices[i].Pos.Y * Scale.Y;
				vertices[tmpCounter++] = S3vertices[i].Pos.Z * Scale.Z; 
			}
		}

		NewtonCollision* collision = NewtonCreateConvexHull(nWorld,nVertices,vertices,sizeof(float)*3,0.01,6,NULL);
		delete [] vertices;

		NewtonConvexCollisionCalculateInertialMatrix(collision,&inertia[0],&origin[0]);	

		Ixx = mass * inertia[0];
		Iyy = mass * inertia[1];
		Izz = mass * inertia[2];

		body = NewtonCreateBody(nWorld,collision);

		NewtonBodySetCentreOfMass(body,&origin[0]);
		NewtonBodySetMassMatrix(body,mass,Ixx,Iyy,Izz);

		NewtonBodySetTransformCallback(body,SetMeshTransformEvent);
		NewtonBodySetForceAndTorqueCallback(body,ApplyForceAndTorque);

		NewtonBodySetAutoSleep(body,1);
		NewtonBodySetFreezeState(body,0);
		NewtonBodySetMaterialGroupID(body,defaultMaterialID);

		NewtonReleaseCollision(nWorld,collision);
		gBody.push_back(body);
	}

	void CBodySceneNode::addBodyTree(IAnimatedMeshSceneNode* mesh,float mass)
	{
		dVector origin;
		dVector inertia;
		float Ixx,Iyy,Izz;
		IAnimatedMesh* irr_mesh = mesh->getMesh();

		//NewtonCollision* collision = NewtonCreateTreeCollision(nWorld,9);
		NewtonCollision* collision = NewtonCreateTreeCollision(nWorld,NULL);
		NewtonTreeCollisionBeginBuild(collision);

		unsigned int nMeshBuffer;
		IMeshBuffer* mesh_buffer;

		vector3df vArray[3];
		vector3df Scale = mesh->getScale();

		for(nMeshBuffer = 0;nMeshBuffer<irr_mesh->getMeshBufferCount();++nMeshBuffer)
		{
			mesh_buffer = irr_mesh->getMesh(0)->getMeshBuffer(nMeshBuffer);

			unsigned short* indices = mesh_buffer->getIndices();

			switch(mesh_buffer->getVertexType())
			{
			case EVT_STANDARD:
				{
					S3DVertex* vertices = (S3DVertex*)mesh_buffer->getVertices();

					for(unsigned int i = 0;i<mesh_buffer->getIndexCount();i+=3)
					{
						vArray[0] = vertices[indices[i]].Pos * Scale.X;
						vArray[1] = vertices[indices[i+1]].Pos * Scale.Y;
						vArray[2] = vertices[indices[i+2]].Pos * Scale.Z;

						NewtonTreeCollisionAddFace(collision,3,&vArray[0].X,sizeof(float)*3,1);
					}
				}
				break;
			case EVT_2TCOORDS:
				{
					S3DVertex2TCoords* vertices = (S3DVertex2TCoords*)mesh_buffer->getVertices();

					for(unsigned int i = 0;i<mesh_buffer->getIndexCount();i+=3)
					{
						vArray[0] = vertices[indices[i]].Pos * Scale.X;
						vArray[1] = vertices[indices[i+1]].Pos * Scale.Y;
						vArray[2] = vertices[indices[i+2]].Pos * Scale.Z;

						NewtonTreeCollisionAddFace(collision,3,&vArray[0].X,sizeof(vector3df),1);
					}
				}
				break;
			case EVT_TANGENTS:
				{
					S3DVertexTangents* vertices = (S3DVertexTangents*)mesh_buffer->getVertices();

					for(unsigned int i = 0;i<mesh_buffer->getIndexCount();i+=3)
					{
						vArray[0] = vertices[indices[i]].Pos * Scale.X;
						vArray[1] = vertices[indices[i+1]].Pos * Scale.Y;
						vArray[2] = vertices[indices[i+2]].Pos * Scale.Z;

						NewtonTreeCollisionAddFace(collision,3,&vArray[0].X,sizeof(vector3df),1);
					}
				}
				break;
			}
		}
		NewtonTreeCollisionEndBuild(collision,1);
		NewtonConvexCollisionCalculateInertialMatrix(collision,&inertia[0],&origin[0]);	

		Ixx = mass * inertia[0];
		Iyy = mass * inertia[1];
		Izz = mass * inertia[2];

		body = NewtonCreateBody(nWorld,collision);

		NewtonBodySetCentreOfMass(body,&origin[0]);
		NewtonBodySetMassMatrix(body,mass,Ixx,Iyy,Izz);

		NewtonBodySetTransformCallback(body,SetMeshTransformEvent);
		NewtonBodySetForceAndTorqueCallback(body,ApplyForceAndTorque);

		NewtonBodySetAutoSleep(body,1);
		NewtonBodySetFreezeState(body,0);
		NewtonBodySetMaterialGroupID(body,defaultMaterialID);

		NewtonReleaseCollision(nWorld,collision);
		gBody.push_back(body);
	}

	

	void CBodySceneNode::addBodyTreeBSP(IAnimatedMesh* q3levelmesh,ISceneNode* q3node)
	{
	
	// added by nespa 

	//LevelNewtonCollision
	NewtonCollision* nmapcollision = NewtonCreateTreeCollision(nWorld, NULL);
	NewtonTreeCollisionBeginBuild(nmapcollision);
	u32 cMeshBuffer, j;
	int v1i, v2i, v3i;
	IMeshBuffer *mb;
	float vArray[9]; // vertex array (3*3 floats)
	int tmpCount = 0;
	core::aabbox3df		box;

	scene::IMesh *mesh = q3levelmesh->getMesh(0);
	for (cMeshBuffer=0; cMeshBuffer<mesh->getMeshBufferCount(); cMeshBuffer++)
	{
		mb = q3levelmesh->getMesh(0)->getMeshBuffer(cMeshBuffer);
		S3DVertex2TCoords* mb_vertices = (S3DVertex2TCoords*)mb->getVertices();
		u16* mb_indices  = mb->getIndices();
		// add each triangle from the mesh
		for (j=0; j<mb->getIndexCount(); j+=3)
		{
			v1i = mb_indices[j];
			v2i = mb_indices[j+1];
			v3i = mb_indices[j+2];
			vArray[0] = mb_vertices[v1i].Pos.X;
			vArray[1] = mb_vertices[v1i].Pos.Y;
			vArray[2] = mb_vertices[v1i].Pos.Z;
			vArray[3] = mb_vertices[v2i].Pos.X;
			vArray[4] = mb_vertices[v2i].Pos.Y;
			vArray[5] = mb_vertices[v2i].Pos.Z;
			vArray[6] = mb_vertices[v3i].Pos.X;
			vArray[7] = mb_vertices[v3i].Pos.Y;
			vArray[8] = mb_vertices[v3i].Pos.Z;
			box.addInternalPoint(mb_vertices[v1i].Pos.X, mb_vertices[v1i].Pos.Y, mb_vertices[v1i].Pos.Z);
			box.addInternalPoint(mb_vertices[v2i].Pos.X, mb_vertices[v2i].Pos.Y, mb_vertices[v2i].Pos.Z);
			box.addInternalPoint(mb_vertices[v3i].Pos.X, mb_vertices[v3i].Pos.Y, mb_vertices[v3i].Pos.Z);
			NewtonTreeCollisionAddFace(nmapcollision, 3, &vArray[0], 12, 1);
		}
	}

	NewtonTreeCollisionEndBuild(nmapcollision, 0);
	NewtonBody* nmapbody = NewtonCreateBody(nWorld, nmapcollision);
	box = q3node->getTransformedBoundingBox();
	//NewtonSetWorldSize (nWorld, (float*)&(box.MinEdge.X), (float*)&(box.MaxEdge.X));
	NewtonReleaseCollision(nWorld, nmapcollision);
	}

	
	


	void CBodySceneNode::addBodyTerrain(ITerrainSceneNode* mesh,int LOD)
	{
		float vArray[9];

		CDynamicMeshBuffer meshBuffer(EVT_2TCOORDS,EIT_32BIT); 
		mesh->getMeshBufferForLOD(meshBuffer,LOD);

		//NewtonCollision* collision = NewtonCreateTreeCollision(nWorld,9);
		NewtonCollision* collision = NewtonCreateTreeCollision(nWorld,NULL);
		NewtonTreeCollisionBeginBuild(collision);

		int v1i,v2i,v3i;

		S3DVertex2TCoords* vertices = (S3DVertex2TCoords*)meshBuffer.getVertexBuffer().getData();
		unsigned int* indices = (unsigned int*)meshBuffer.getIndexBuffer().getData();

		vector3df Scale = mesh->getScale();

		for(unsigned int i = 0;i<meshBuffer.getIndexCount();i += 3)
		{
			v1i = indices[i];
			v2i = indices[i+1];
			v3i = indices[i+2];

			// build face data
			vArray[0] = vertices[v1i].Pos.X * Scale.X;
			vArray[1] = vertices[v1i].Pos.Y * Scale.Y;
			vArray[2] = vertices[v1i].Pos.Z * Scale.Z;

			vArray[3] = vertices[v2i].Pos.X * Scale.X;
			vArray[4] = vertices[v2i].Pos.Y * Scale.Y;
			vArray[5] = vertices[v2i].Pos.Z * Scale.Z;

			vArray[6] = vertices[v3i].Pos.X * Scale.X;
			vArray[7] = vertices[v3i].Pos.Y * Scale.Y;
			vArray[8] = vertices[v3i].Pos.Z * Scale.Z; 

			// make sure we do not add degenerated polygons to the tree
			vector3df irrvArray[3] = { vector3df(vArray[0],vArray[1],vArray[2]),
									   vector3df(vArray[3],vArray[4],vArray[5]),
									   vector3df(vArray[6],vArray[7],vArray[8]) };

			vector3df e0(irrvArray[1] - irrvArray[0]);
			vector3df e1(irrvArray[2] - irrvArray[0]);

			// the area of the face is the cross product
			vector3df area(e0.crossProduct(e1));

			// skip faces with very small area
			float mag = area.dotProduct(area);
			if (mag > 1.0e-6f)
				NewtonTreeCollisionAddFace(collision,3,(float*)vArray,sizeof(vector3df),11);
		}

		NewtonTreeCollisionEndBuild(collision,0);

		body = NewtonCreateBody(nWorld,collision);

		NewtonBodySetMaterialGroupID(body,defaultMaterialID);
		NewtonReleaseCollision(nWorld,collision);
		gBody.push_back(body);
	}

	//Crea un cuerpo con forma de cubo
	void CBodySceneNode::addBodyWater(float SX,float SY,float SZ,float FluidDensity,float LinearViscosity,float AngularViscosity)
	{
		fDensity = FluidDensity;
		lViscosity = LinearViscosity;
		aViscosity = AngularViscosity;

		NewtonCollision* collision = NewtonCreateBox(nWorld,SX,SY,SZ,0,NULL);
		NewtonCollisionSetAsTriggerVolume(collision,1);

		body = NewtonCreateBody(nWorld,collision);

		int bouyancyForceMaterialID;
		bouyancyForceMaterialID = NewtonMaterialCreateGroupID (nWorld);

		NewtonMaterialSetCollisionCallback(nWorld, bouyancyForceMaterialID, defaultMaterialID, NULL, ApplyBuoyancyAABBLevel, NULL); 

		NewtonBodySetMaterialGroupID(body,bouyancyForceMaterialID);
		//NewtonReleaseCollision(nWorld,collision);
	}

	void CBodySceneNode::setParent(ISceneNode* newParent)
	{
		NewtonBodySetUserData(body,newParent);
	}

	void CBodySceneNode::setPosition(const vector3df& position)
	{
		matrix4 mat;
		NewtonBodyGetMatrix(body,&mat.pointer()[0]);
		mat.getRotationDegrees();
		mat.setTranslation(position);
		NewtonBodySetMatrix(body,&mat.pointer()[0]);
	}

	void CBodySceneNode::setRotation(const vector3df& rot)
	{
		matrix4 mat;
		NewtonBodyGetMatrix(body,&mat.pointer()[0]);
		mat.getTranslation();
		mat.setRotationDegrees(rot);
		NewtonBodySetMatrix(body,&mat.pointer()[0]);
	}

	vector3df CBodySceneNode::getAbsolutePosition() const
	{
		matrix4 mat;
		NewtonBodyGetMatrix(body,&mat.pointer()[0]);
		return mat.getTranslation();
	}

	const vector3df& CBodySceneNode::getRotation() const
	{
		matrix4 mat;
		NewtonBodyGetMatrix(body,&mat.pointer()[0]);
		return mat.getRotationDegrees().normalize();
	}

	NewtonBody* CBodySceneNode::getBody()
	{
		return body;
	}

	vector3df CBodySceneNode::getColl(NewtonBody* body2,int mode)
	{
		dMatrix t1,t2;
		vector3df result;
		float contacts[3];
		float normals[3];
		float penetration[1];
		int col;

		NewtonBodyGetMatrix(body,&t1[0][0]);
		NewtonBodyGetMatrix(body2,&t2[0][0]);

		col = NewtonCollisionCollide(nWorld,1,
			NewtonBodyGetCollision(body),&t1[0][0],
			NewtonBodyGetCollision(body2),&t2[0][0],contacts,normals,penetration,0);

		if(mode==0)
			result = vector3df(contacts[0],contacts[1],contacts[2]);
		else
			result = vector3df(normals[0],normals[1],normals[2]);

		if(col)
			return result;
		else
			return vector3df(0.0);
	}

	bool CBodySceneNode::BodiesCollide(NewtonBody* body2)
	{
		dMatrix t1,t2;

		float contacts[3];
		float normals[3];
		float penetration[1];
		int col;

		NewtonBodyGetMatrix(body,&t1[0][0]);
		NewtonBodyGetMatrix(body2,&t2[0][0]);

		col = NewtonCollisionCollide(nWorld,1,NewtonBodyGetCollision(body),&t1[0][0],
					NewtonBodyGetCollision(body2),&t2[0][0],contacts,normals,penetration,0);

		if(col>0)
			return true;
		else
			return false;
	}

	void CBodySceneNode::remove()
	{
		NewtonDestroyBody(nWorld,body);
	}

	void CBodySceneNode::OnPreRender()
	{
	};

	void CBodySceneNode::render()
	{
	};

	SMaterial& CBodySceneNode::getMaterial(s32 i)
	{
		return material;
	}

	const aabbox3d<f32>& CBodySceneNode::getBoundingBox() const
	{
		return BBox;
	}

	void _cdecl SetMeshTransformEvent(const NewtonBody* body,const float* matrix,int threadIndex)
	{
		matrix4 mat;
		memcpy(mat.pointer(),matrix,sizeof(float)*16);

		ISceneNode* tmp = (ISceneNode*)NewtonBodyGetUserData(body);

		if(tmp)
		{
			tmp->setPosition(mat.getTranslation());
			tmp->setRotation(mat.getRotationDegrees());
			tmp->updateAbsolutePosition();
		}
	}

	void _cdecl ApplyForceAndTorque(const NewtonBody* body,float timestep,int threadIndex) 
	{ 
		float mass,Ixx,Iyy,Izz; 

		NewtonBodyGetMassMatrix(body,&mass,&Ixx,&Iyy,&Izz); 
		dVector force(GravityX * mass,GravityY * mass,GravityZ * mass);
		NewtonBodyAddForce(body,&force.m_x);
	}

	int PhysicsBouyancyPlane(const int collisionID,void *context,const dFloat* matrix,dFloat* globalSpacePlane)
	{
		const NewtonBody* trigger;
		const dMatrix& globalSpaceMatrix =  *(dMatrix*)matrix;

		trigger = (const NewtonBody*)context;

		dVector p0(globalSpaceMatrix.m_posit);
		dVector p1(globalSpaceMatrix.m_posit);

		p0.m_y += 100.0;
		p1.m_y -= 100.0;

		dMatrix volumeMatrix;
		NewtonBodyGetMatrix(trigger,&volumeMatrix[0][0]);

		dVector q0(volumeMatrix.UntransformVector(p0));
		dVector q1(volumeMatrix.UntransformVector(p1));

		int attribute;
		dFloat param;
		dVector normal;
		param = NewtonCollisionRayCast(NewtonBodyGetCollision(trigger),&q0[0],&q1[0],&normal[0],&attribute);

		normal = volumeMatrix.RotateVector(normal);
		dVector point (p0 + (p1 - p0).Scale(param));

		globalSpacePlane[0] = normal.m_x;
		globalSpacePlane[1] = normal.m_y;
		globalSpacePlane[2] = normal.m_z;
		globalSpacePlane[3] = -(normal % point);
		return 1;
	}

	int ApplyBuoyancyAABBLevel (const NewtonMaterial* material, const NewtonBody* body0, const NewtonBody* body1, int threadIndex)
	{
		const NewtonBody* newbody;
		const NewtonBody* trigger;

		if (NewtonCollisionIsTriggerVolume(NewtonBodyGetCollision(body0)))
		{
			newbody = body1;
			trigger = body0;
		}
		else
		{
			newbody = body0;
			trigger = body1;
		}

		dVector gravity(GravityX,GravityY,GravityZ);
		NewtonBodyAddBuoyancyForce(newbody,fDensity,lViscosity,aViscosity,&gravity[0],PhysicsBouyancyPlane,(void*)trigger);
		return 1;
	}