static void MagneticField (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
	{
		dFloat magnetStregnth;
		const NewtonBody* body0;
		const NewtonBody* body1; 
		const NewtonBody* magneticField;
		const NewtonBody* magneticPiece;

		body0 = NewtonJointGetBody0 (contactJoint);
		body1 = NewtonJointGetBody1 (contactJoint);

		// get the magnetic field body
		magneticPiece = body0;
		magneticField = body1;
		if (NewtonCollisionIsTriggerVolume (NewtonBodyGetCollision(body0))) {
			magneticPiece = body1;
			magneticField = body0;
		}
		_ASSERTE (NewtonCollisionIsTriggerVolume (NewtonBodyGetCollision(magneticField)));

		// calculate the magnetic force field

		dMatrix center;
		dMatrix location;

		NewtonBodyGetMatrix (magneticField, &center[0][0]);
		NewtonBodyGetMatrix (magneticPiece, &location[0][0]);

		Magnet* magnet;
		magnet = (Magnet*)NewtonBodyGetUserData(magneticField);

		magnetStregnth = magnet->m_magnetStregnth;

		// calculate the magnetic force;

		dFloat den;
		dVector force (center.m_posit - location.m_posit);

		den = force % force;
		den = magnetStregnth / (den * dSqrt (den) + 0.1f);
		force = force.Scale (den);

		// because we are modifiing one of the bodies membber in the call back, there uis a chace that 
		// another materail can be operations on the same object at the same time of aother thread
		// therfore we need to make the assigmnet in a critical section.
		NewtonWorldCriticalSectionLock (NewtonBodyGetWorld (magneticPiece));
		
		// add the magner force
		NewtonBodyAddForce (magneticPiece, &force[0]);

		force = force.Scale (-1.0f);
		NewtonBodyAddForce (magnet->m_magneticCore, &force[0]);

		// also if the body is sleeping fore it to wake up for this frame
		NewtonBodySetFreezeState (magneticPiece, 0);
		NewtonBodySetFreezeState (magnet->m_magneticCore, 0);

		// unlock the critical section
		NewtonWorldCriticalSectionUnlock (NewtonBodyGetWorld (magneticPiece));
	}

void GenericContactProcess (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
	int isHightField;
	NewtonBody* body;
	NewtonCollision* collision;
	NewtonCollisionInfoRecord info;

	isHightField = 1;
	body = NewtonJointGetBody0 (contactJoint);
	collision = NewtonBodyGetCollision(body);
	NewtonCollisionGetInfo(collision, &info);
	if (info.m_collisionType != SERIALIZE_ID_HEIGHTFIELD) {
		body = NewtonJointGetBody1 (contactJoint);
		collision = NewtonBodyGetCollision(body);
		NewtonCollisionGetInfo(collision, &info);
		isHightField  = (info.m_collisionType == SERIALIZE_ID_HEIGHTFIELD); 
	}

	#define HOLE_IN_TERRAIN 10
	if (isHightField) {
		void* nextContact;
		for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = nextContact) {
			int faceID;
			NewtonMaterial* material;

			nextContact = NewtonContactJointGetNextContact (contactJoint, contact);

			material = NewtonContactGetMaterial (contact);
			faceID = NewtonMaterialGetContactFaceAttribute (material);
			if (faceID == HOLE_INTERRAIN) {
				NewtonContactJointRemoveContact (contactJoint, contact); 
			}
		}
	}
}

int NewtonBodyCollide(NewtonWorld* world, int maxsize, NewtonBody* body0, NewtonBody* body1, float* contacts, float* normals, float* penetration)
{
      NewtonCollision* collision[2];
      float mat0[16];
      float mat1[16];
      collision[0]=NewtonBodyGetCollision(body0);
      collision[1]=NewtonBodyGetCollision(body1);
      NewtonBodyGetMatrix(body0,mat0);
      NewtonBodyGetMatrix(body1,mat1);
      return NewtonCollisionCollide(world,maxsize,collision[0],mat0,collision[1],mat1,contacts,normals,penetration,0);
}

VALUE MSNewton::Bodies::touching(VALUE self, VALUE v_body1, VALUE v_body2) {
	const NewtonBody* body1 = Util::value_to_body(v_body1);
	const NewtonBody* body2 = Util::value_to_body(v_body2);
	Util::validate_two_bodies(body1, body2);
	const NewtonWorld* world = NewtonBodyGetWorld(body1);
	NewtonCollision* colA = NewtonBodyGetCollision(body1);
	NewtonCollision* colB = NewtonBodyGetCollision(body2);
	dMatrix matrixA;
	dMatrix matrixB;
	NewtonBodyGetMatrix(body1, &matrixA[0][0]);
	NewtonBodyGetMatrix(body2, &matrixB[0][0]);
	return NewtonCollisionIntersectionTest(world, colA, &matrixA[0][0], colB, &matrixB[0][0], 0) == 1 ? Qtrue : Qfalse;
}

	void cPhysicsBodyNewton::RenderDebugGeometry(iLowLevelGraphics *a_pLowLevel, const cColor &a_Color)
	{
		//cPhysicsWorldNewton *pPhysicsWorld = static_cast<cPhysicsWorldNewton*>(NewtonWorldGetUserData(m_pNewtonWorld));
		//pPhysicsWorld->GetWorld3D()->gets

		NewtonCollision *pCollision = NewtonBodyGetCollision(m_pNewtonBody);
		float matrix[4][4];
		NewtonBodyGetMatrix(m_pNewtonBody, &matrix[0][0]);
		g_pLowLevelGraphics = a_pLowLevel;
		g_DebugColor = a_Color;
		NewtonCollision *pNewtonCollision = NewtonBodyGetCollision(m_pNewtonBody);
		NewtonCollisionForEachPolygonDo(pNewtonCollision,
			m_mtxLocalTransform.GetTranspose().m[0], RenderDebugPolygon, this);
	}

static void UserContactFriction (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	// call  the basic call back
	GenericContactProcess (contactJoint, timestep, threadIndex);

	const NewtonBody* const body0 = NewtonJointGetBody0(contactJoint);
	const NewtonBody* const body1 = NewtonJointGetBody1(contactJoint);
	const NewtonBody* body = body0;
	NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
	if (mass == 0.0f) {
		body = body1;
	}

	//now core 300 can have per collision user data 		
	NewtonCollision* const collision = NewtonBodyGetCollision(body);
	void* userData = NewtonCollisionGetUserData (collision);
	dFloat frictionValue = *((dFloat*)&userData);
	for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
		NewtonMaterial* const material = NewtonContactGetMaterial (contact);
		NewtonMaterialSetContactFrictionCoef (material, frictionValue + 0.1f, frictionValue, 0);
		NewtonMaterialSetContactFrictionCoef (material, frictionValue + 0.1f, frictionValue, 1);
	}
}

	NewtonBody* CreateBodyPart(DemoEntity* const bodyPart, const dArmRobotConfig& definition)
	{
		NewtonCollision* const shape = MakeConvexHull(bodyPart);

		// calculate the bone matrix
		dMatrix matrix(bodyPart->CalculateGlobalMatrix());

		NewtonWorld* const world = GetWorld();

		// create the rigid body that will make this bone
		NewtonBody* const body = NewtonCreateDynamicBody(world, shape, &matrix[0][0]);

		// destroy the collision helper shape 
		NewtonDestroyCollision(shape);

		// get the collision from body
		NewtonCollision* const collision = NewtonBodyGetCollision(body);

		// calculate the moment of inertia and the relative center of mass of the solid
		NewtonBodySetMassProperties(body, definition.m_mass, collision);
//NewtonBodySetMassProperties(body, 0.0f, collision);

		// save the user lifterData with the bone body (usually the visual geometry)
		NewtonBodySetUserData(body, bodyPart);

		// assign a body part id
		//NewtonCollisionSetUserID(collision, definition.m_bodyPartID);

		// set the bod part force and torque call back to the gravity force, skip the transform callback
		NewtonBodySetForceAndTorqueCallback(body, PhysicsApplyGravityForce);
		return body;
	}

void RenderAABB (NewtonWorld* const world)
{
	glDisable (GL_LIGHTING);
	glDisable(GL_TEXTURE_2D);

	glColor3f(0.0f, 0.0f, 1.0f);

	glBegin(GL_LINES);
//glVertex3f (-20.3125000f, 3.54991579f, 34.3441200f);
//glVertex3f (-19.6875000f, 3.54257250f, 35.2211456f);

	for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
		dVector p0; 
		dVector p1; 
		dMatrix matrix;
		NewtonCollision* const collision = NewtonBodyGetCollision(body);
		NewtonBodyGetMatrix (body, &matrix[0][0]);
		NewtonCollisionCalculateAABB (collision, &matrix[0][0], &p0[0], &p1[0]);
//		CalculateAABB (collision, matrix, p0, p1);

		glVertex3f (p0.m_x, p0.m_y, p0.m_z);
		glVertex3f (p1.m_x, p0.m_y, p0.m_z);

		glVertex3f (p0.m_x, p1.m_y, p0.m_z);
		glVertex3f (p1.m_x, p1.m_y, p0.m_z);

		glVertex3f (p0.m_x, p1.m_y, p1.m_z);
		glVertex3f (p1.m_x, p1.m_y, p1.m_z);

		glVertex3f (p0.m_x, p0.m_y, p1.m_z);
		glVertex3f (p1.m_x, p0.m_y, p1.m_z);


		glVertex3f (p0.m_x, p0.m_y, p0.m_z);
		glVertex3f (p0.m_x, p1.m_y, p0.m_z);

		glVertex3f (p1.m_x, p0.m_y, p0.m_z);
		glVertex3f (p1.m_x, p1.m_y, p0.m_z);

		glVertex3f (p0.m_x, p0.m_y, p1.m_z);
		glVertex3f (p0.m_x, p1.m_y, p1.m_z);

		glVertex3f (p1.m_x, p0.m_y, p1.m_z);
		glVertex3f (p1.m_x, p1.m_y, p1.m_z);


		glVertex3f (p0.m_x, p0.m_y, p0.m_z);
		glVertex3f (p0.m_x, p0.m_y, p1.m_z);

		glVertex3f (p1.m_x, p0.m_y, p0.m_z);
		glVertex3f (p1.m_x, p0.m_y, p1.m_z);

		glVertex3f (p0.m_x, p1.m_y, p0.m_z);
		glVertex3f (p0.m_x, p1.m_y, p1.m_z);

		glVertex3f (p1.m_x, p1.m_y, p0.m_z);
		glVertex3f (p1.m_x, p1.m_y, p1.m_z);
	}
	glEnd();
}

static void GetCollisionSubShape(const NewtonJoint* const contactJoint, NewtonBody* const body)
{
	NewtonCollisionInfoRecord collisionInfo;

	NewtonCollision* const collision = NewtonBodyGetCollision(body);
	NewtonCollisionGetInfo (collision, &collisionInfo);


	int count = 0;
	NewtonCollision* collidingSubShapeArrar[32];

	// see if this is a compound collision or any other collision with sub collision shapes  
	if (collisionInfo.m_collisionType == SERIALIZE_ID_COMPOUND) {

		// to get the torque we need the center of gravity in global space
		dVector origin;
		dMatrix bodyMatrix;
		NewtonBodyGetMatrix(body, &bodyMatrix[0][0]);
		NewtonBodyGetCentreOfMass(body, &origin[0]);
		origin = bodyMatrix.TransformVector(origin);

		for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
			// get the material of this contact, 
			// this part contain all contact information, the sub colliding shape, 
			NewtonMaterial* const material = NewtonContactGetMaterial (contact);
			NewtonCollision* const subShape = NewtonMaterialGetBodyCollidingShape (material, body);
			int i = count - 1;
			for (; i >= 0; i --) {
				if (collidingSubShapeArrar[i] == subShape) {
					break;
				}
			}
			if (i < 0) {
				collidingSubShapeArrar[count] = subShape;
				count ++;
				dAssert (count < int (sizeof (collidingSubShapeArrar) / sizeof (collidingSubShapeArrar[0])));

				// you can also get the forces here, however when tho function is call form a contact material
				// we can only get resting forces, impulsive forces can not be read here since they has no being calculated yet.
				// whoever if this function function is call after the NetwonUpdate they the application can read the contact force, that was applied to each contact point
				dVector force;
				dVector posit;
				dVector normal;
				NewtonMaterialGetContactForce (material, body, &force[0]);
				NewtonMaterialGetContactPositionAndNormal (material, body, &posit[0], &normal[0]);
				// the torque on this contact is
				dVector torque ((origin - posit) * force);

				// do what ever you want wit this  


			}
		}
	}

	// here we should have an array of all colling sub shapes
	if (count) {
		// do what you need with this sub shape list
	}
}

//////////////////////////////////////////////////////////////////////////
// Position the body precisely
//////////////////////////////////////////////////////////////////////////
void DropDownBox(NewtonBody *body)
{
	float matrix[16];
	NewtonBodyGetMatrix(body, matrix);
	Matrix4 m = Matrix4(matrix);

	Vector3 pos = m.GetPosition();
	pos.y += 1.0f;
	m.SetPosition(pos);
	Vector3 p = pos;
	p.y -= 20;

	// cast collision shape within +20 -20 y range
	NewtonWorld *world = NewtonBodyGetWorld(body);
	NewtonCollision *collision = NewtonBodyGetCollision(body);

	float param;
	NewtonWorldConvexCastReturnInfo info[16];
	m.FlattenToArray(matrix);
	NewtonWorldConvexCast(world, matrix, &p[0], collision, &param, body, DropDownConvexCastCallback, info, 16, 0);

	m = Matrix4(matrix);
	pos = m.GetPosition();
	m.SetPosition(pos + Vector3(0, (p.y - pos.y) * param, 0) );

	m.FlattenToArray(matrix);
	NewtonBodySetMatrix(body, matrix);
}

	void RayCastCompoundsAllSubShapes(const dVector& origin, const dVector& end)
	{
		m_param = 1.0f;
		m_body = NULL;
		NewtonWorld* const world = GetWorld();
		NewtonWorldRayCast(world, &origin[0], &end[0], PickCompound, this, NULL, 0);
		if (m_body) {
			// we found a compound, find all sub shape on teh path of the ray
			dMatrix matrix;
			NewtonBodyGetMatrix(m_body, &matrix[0][0]);
			dVector localP0(matrix.UntransformVector(origin));
			dVector localP1(matrix.UntransformVector(end));

			NewtonCollision* const compoundCollision = NewtonBodyGetCollision(m_body);
			dAssert(NewtonCollisionGetType(compoundCollision) == SERIALIZE_ID_COMPOUND);
			for (void* node = NewtonCompoundCollisionGetFirstNode(compoundCollision); node; node = NewtonCompoundCollisionGetNextNode(compoundCollision, node)) {
				dVector normal;
				dLong attribute;
				NewtonCollision* const subShape = NewtonCompoundCollisionGetCollisionFromNode(compoundCollision, node);
				dFloat xxx = NewtonCollisionRayCast(subShape, &localP0[0], &localP1[0], &normal[0], &attribute);
				if (xxx < 1.0f) {
					dTrace (("sub shape hit\n"))
				}
			}
		}
	}

static void DebugShowBodyCollision (const NewtonBody* const body, DEBUG_DRAW_MODE mode)
{
	switch (NewtonBodyGetType(body)) 
	{
		case NEWTON_DYNAMIC_BODY:
		{
			int sleepState = NewtonBodyGetSleepState(body);
			if (sleepState == 1) {
				// indicate when body is sleeping 
				glColor3f(0.42f, 0.73f, 0.98f);
			} else {
				// body is active
				glColor3f(1.0f, 1.0f, 1.0f);
			}
			break;
		}

		case NEWTON_KINEMATIC_BODY:
			glColor3f(1.0f, 1.0f, 0.0f);
			break;

		case NEWTON_DEFORMABLE_BODY:
			glColor3f (0.0f, 1.0f, 1.0f);
			break;
	}
	dMatrix matrix;
	NewtonBodyGetMatrix(body, &matrix[0][0]);



	NewtonCollisionForEachPolygonDo (NewtonBodyGetCollision(body), &matrix[0][0], DebugShowGeometryCollision, (void*) mode);
}

static void UserContactRestitution (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;
	NewtonBody* body;
	NewtonBody* body0;
	NewtonBody* body1;

	// call  the basic call back
	GenericContactProcess (contactJoint, timestep, threadIndex);

	body0 = NewtonJointGetBody0(contactJoint);
	body1 = NewtonJointGetBody1(contactJoint);

	body = body0;
	NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
	if (mass == 0.0f) {
		body = body1;
	}

	NewtonCollision* const collision = NewtonBodyGetCollision(body);
	void* userData = NewtonCollisionGetUserData (collision);
	dFloat restitution = *((float*)&userData);
	for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
		NewtonMaterial* const material = NewtonContactGetMaterial (contact);
		NewtonMaterialSetContactElasticity (material, restitution);
	}
}

int PhysicsIslandUpdate (const NewtonWorld* world, const void* islandHandle, int bodyCount)
{
	if (showIslans) {
		dVector minAABB ( 1.0e10f,  1.0e10f,  1.0e10f, 0.0f);
		dVector maxAABB (-1.0e10f, -1.0e10f, -1.0e10f, 0.0f);
		for (int i = 0; i < bodyCount; i ++) {
			dVector p0;
			dVector p1;

#if 0
			// show the engine loose aabb
			NewtonIslandGetBodyAABB (islandHandle, i, &p0[0], &p1[0]);
#else
			// calculate the shape aabb

			dMatrix matrix;
			NewtonBody* body;
			NewtonCollision *collision;

			body = NewtonIslandGetBody (islandHandle, i);
			collision = NewtonBodyGetCollision (body);
			NewtonBodyGetMatrix (body, &matrix[0][0]);
			CalculateAABB (collision, matrix, p0, p1);
#endif
			for (int j = 0; j < 3; j ++ ) {
				minAABB[j] = p0[j] < minAABB[j] ? p0[j] : minAABB[j];
				maxAABB[j] = p1[j] > maxAABB[j] ? p1[j] : maxAABB[j];
			}
		}
		DebugDrawAABB (minAABB, maxAABB);
	}

	//	g_activeBodies += bodyCount;
	return 1;
}

static void CreateVisualEntity (DemoEntityManager* const scene, NewtonBody* const body)
{
	dMatrix matrix;
	NewtonBodyGetMatrix(body, &matrix[0][0]);

	DemoEntity* const visualEntity = new DemoEntity(matrix, NULL);
	scene->Append(visualEntity);

	// set the entiry as the user data;
	NewtonBodySetUserData (body, visualEntity);

	// create the mesh geometry and attach it to the entity
	DemoMesh* const visualMesh = new DemoMesh ("fraturedMainMesh");
	visualEntity->SetMesh (visualMesh, dGetIdentityMatrix());
	visualMesh->Release();

	// create the mesh and set the vertex array, but not the sub meshes
	NewtonCollision* const fracturedCompoundCollision = NewtonBodyGetCollision(body);
	dAssert (NewtonCollisionGetType(fracturedCompoundCollision) == SERIALIZE_ID_FRACTURED_COMPOUND);

	// add the vertex data
	NewtonFracturedCompoundMeshPart* const mainMesh = NewtonFracturedCompoundGetMainMesh (fracturedCompoundCollision);
	AddMeshVertexwData (visualMesh, mainMesh, fracturedCompoundCollision);

	// now add the sub mesh by calling the call back
	OnReconstructMainMeshCallBack (body, mainMesh, fracturedCompoundCollision);
}

dNewtonDynamicBody::dNewtonDynamicBody(dNewtonWorld* const world, dNewtonCollision* const collision, dMatrix matrix, dFloat mass)
	:dNewtonBody(matrix)
{
	NewtonWorld* const newton = world->m_world;

	NewtonWaitForUpdateToFinish(newton);
	m_body = NewtonCreateDynamicBody(newton, collision->m_shape, &matrix[0][0]);
	collision->DeleteShape();
	collision->SetShape(NewtonBodyGetCollision(m_body));

	NewtonBodySetMassProperties(m_body, mass, NewtonBodyGetCollision(m_body));

	NewtonBodySetUserData(m_body, this);
	NewtonBodySetTransformCallback(m_body, OnBodyTransformCallback);
	NewtonBodySetForceAndTorqueCallback(m_body, OnForceAndTorqueCallback);
}

void SetShowMeshCollision (SceneManager& me, int mode)
{
	NewtonTreeCollisionCallback showFaceCallback;

	showFaceCallback = NULL;
	if (mode) {
		showFaceCallback = ShowMeshCollidingFaces;
	}

	// iterate the world
	for (const NewtonBody* body = NewtonWorldGetFirstBody (me.m_world); body; body = NewtonWorldGetNextBody (me.m_world, body)) {
		NewtonCollision* collision;
		NewtonCollisionInfoRecord info;

		collision = NewtonBodyGetCollision (body);
		NewtonCollisionGetInfo (collision, &info);

		switch (info.m_collisionType) 
		{
			case SERIALIZE_ID_TREE:
			case SERIALIZE_ID_SCENE:
			case SERIALIZE_ID_USERMESH:
			case SERIALIZE_ID_HEIGHTFIELD:
			{
				NewtonStaticCollisionSetDebugCallback (collision, showFaceCallback);
				break;
			}

			default: 
				break;
		}
	}
}

		virtual const void InitRigiBody(const NewtonBody* const body, const char* const bodyName) const
		{
			dMatrix matrix;
			DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(NewtonBodyGetWorld(body));

			NewtonCollision* const collision = NewtonBodyGetCollision(body);
			DemoMesh* const mesh = new DemoMesh("ragdoll", collision, "smilli.tga", "smilli.tga", "smilli.tga");

			NewtonBodyGetMatrix(body, &matrix[0][0]);
			DemoEntity* const entity = new DemoEntity(matrix, NULL);
			entity->SetNameID (bodyName);
			entity->SetMesh(mesh, dGetIdentityMatrix());
			scene->Append(entity);
			mesh->Release();

			// save the pointer to the graphic object with the body.
			NewtonBodySetUserData(body, entity);

			// assign the wood id
			NewtonBodySetMaterialGroupID(body, m_material);

			//set continuous collision mode
			//NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode);

			// set a destructor for this rigid body
			NewtonBodySetDestructorCallback(body, PhysicsBodyDestructor);

			// set the transform call back function
			NewtonBodySetTransformCallback(body, DemoEntity::TransformCallback);

			// set the force and torque call back function
			NewtonBodySetForceAndTorqueCallback(body, PhysicsApplyGravityForce);
		}

void CustomPlayerController::SetPlayerOrigin (dFloat originHigh)
{
	dAssert (0);
	NewtonCollision* const playerShape = NewtonBodyGetCollision(m_body);
	NewtonCompoundCollisionBeginAddRemove(playerShape);	

		dMatrix supportShapeMatrix (dGetIdentityMatrix());
		supportShapeMatrix[0] = m_upVector;
		supportShapeMatrix[1] = m_frontVector;
		supportShapeMatrix[2] = supportShapeMatrix[0] * supportShapeMatrix[1];
		supportShapeMatrix.m_posit = supportShapeMatrix[0].Scale(m_height * 0.5f - originHigh);
		supportShapeMatrix.m_posit.m_w = 1.0f;
		NewtonCollisionSetMatrix (m_supportShape, &supportShapeMatrix[0][0]);

		dMatrix collisionShapeMatrix (supportShapeMatrix);
		dFloat cylinderHeight = m_height - m_stairStep;
		dAssert (cylinderHeight > 0.0f);
		collisionShapeMatrix.m_posit = collisionShapeMatrix[0].Scale(cylinderHeight * 0.5f + m_stairStep - originHigh);
		collisionShapeMatrix.m_posit.m_w = 1.0f;
		NewtonCollisionSetMatrix (m_upperBodyShape, &collisionShapeMatrix[0][0]);

	NewtonCompoundCollisionEndAddRemove (playerShape);	

	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;
	NewtonBodyGetMassMatrix(m_body, &mass, &Ixx, &Iyy, &Izz);
	NewtonBodySetMassProperties(m_body, mass, playerShape);
}

void RigidBodyData::Save(ISave* const isave)
{
	ULONG nwrit;
	int revision = D_FILE_REVISION;
	dVector mass;
	dVector com;
	dVector veloc;
	dVector omega;
	dMatrix matrix;

	RigidBodyWorldDesc& me = *(RigidBodyWorldDesc*) RigidBodyWorldDesc::GetDescriptor();

	NewtonBodyGetMatrix(m_body, &matrix[0][0]);
	NewtonBodyGetVelocity(m_body, &veloc[0]);
	NewtonBodyGetOmega(m_body, &omega[0]);

	NewtonCollision* const collision = NewtonBodyGetCollision(m_body);

	isave->Write((const char*)&revision, sizeof (revision), &nwrit);
	isave->Write((const char*)&m_oldControlerID, sizeof (m_oldControlerID), &nwrit);
	isave->Write((const char*)&m_collisionShape, sizeof (m_collisionShape), &nwrit);
	isave->Write((const char*)&m_hideGizmos, sizeof (m_hideGizmos), &nwrit);
	isave->Write((const char*)&m_mass, sizeof (m_mass), &nwrit);
	isave->Write((const char*)&m_inertia, sizeof (m_inertia), &nwrit);
	isave->Write((const char*)&m_origin, sizeof (m_origin), &nwrit);

	isave->Write((const char*)&matrix, sizeof (matrix), &nwrit);
	isave->Write((const char*)&veloc, sizeof (veloc), &nwrit);
	isave->Write((const char*)&omega, sizeof (omega), &nwrit);
	NewtonCollisionSerialize (me.m_newton, collision, SaveCollision, isave);
}