//-----------------------------------------------------------------------
	int cPhysicsMaterialNewton::BeginContactCallback(const NewtonMaterial* material,
									const NewtonBody* apBody1, const NewtonBody* apBody2,
									int alThreadIndex)
	{
		iPhysicsBody* pContactBody1 = (cPhysicsBodyNewton*) NewtonBodyGetUserData(apBody1);
		iPhysicsBody* pContactBody2 = (cPhysicsBodyNewton*) NewtonBodyGetUserData(apBody2);

        if(pContactBody1->GetCollide()==false) return 0;
		if(pContactBody2->GetCollide()==false) return 0;

		if(pContactBody1->IsActive()==false) return 0;
		if(pContactBody2->IsActive()==false) return 0;

		if(pContactBody1->IsRagDoll() && pContactBody2->GetCollideRagDoll()==false) return 0;
		if(pContactBody2->IsRagDoll() && pContactBody1->GetCollideRagDoll()==false) return 0;

		if(pContactBody1->IsCharacter() && pContactBody2->GetCollideCharacter()==false) return 0;
		if(pContactBody2->IsCharacter() && pContactBody1->GetCollideCharacter()==false) return 0;

		//Log("----- Begin contact between body '%s' and '%s'.\n",mpContactBody1->GetName().c_str(),
		//													mpContactBody2->GetName().c_str());

		if(pContactBody1->OnBeginCollision(pContactBody2) == false) return 0;
		if(pContactBody2->OnBeginCollision(pContactBody1) == false) return 0;

		return 1;
	}

    void GenericContactProcessCompatible(const void* const newtonContactJoint, float64 timestep, int threadIndex)
    {
        con_assert(newtonContactJoint != nullptr, "zero pointer")

            NewtonBody* body0 = NewtonJointGetBody0(static_cast<const NewtonJoint*>(newtonContactJoint));
        NewtonBody* body1 = NewtonJointGetBody1(static_cast<const NewtonJoint*>(newtonContactJoint));

        con_assert(body0 != nullptr && body1 != nullptr, "zero pointers")

            if (body0 != nullptr && body1 != nullptr)
            {
                iPhysicsBody* physicsBody0 = static_cast<iPhysicsBody*>(NewtonBodyGetUserData(static_cast<const NewtonBody*>(body0)));
                iPhysicsBody* physicsBody1 = static_cast<iPhysicsBody*>(NewtonBodyGetUserData(static_cast<const NewtonBody*>(body1)));

                con_assert(physicsBody0 != nullptr && physicsBody1 != nullptr, "zero pointers");

                void* contact = NewtonContactJointGetFirstContact(static_cast<const NewtonJoint*>(newtonContactJoint));
                NewtonMaterial* materialCombo = NewtonContactGetMaterial(contact);
                iPhysicsMaterialCombo* physicsMaterialCombo = static_cast<iPhysicsMaterialCombo*>(NewtonMaterialGetMaterialPairUserData(materialCombo));

                if (physicsMaterialCombo != nullptr && physicsBody0 != nullptr && physicsBody1 != nullptr)
                {
                    physicsMaterialCombo->contact(physicsBody0, physicsBody1);
                }
            }
    }

int dNewton::OnBodiesAABBOverlap (const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonBody* const body1, int threadIndex)
{
	dAssert (NewtonBodyGetWorld (body0) == NewtonBodyGetWorld (body1));
	dNewton* const world = (dNewton*) NewtonWorldGetUserData(NewtonBodyGetWorld (body0));
	dNewtonBody* const dBody0 = (dNewtonBody*) NewtonBodyGetUserData (body0);
	dNewtonBody* const dBody1 = (dNewtonBody*) NewtonBodyGetUserData (body1);

	return world->OnBodiesAABBOverlap(dBody0, dBody1, threadIndex);
}

int _CDECL MaterialPair::collisionCallback_onAABBOverlap( const NewtonMaterial* material, const NewtonBody* newtonBody0, const NewtonBody* newtonBody1, int threadIndex )
{
    MaterialPair* me;
    me = (MaterialPair*)NewtonMaterialGetMaterialPairUserData( material );

    Body* body0 = (OgreNewt::Body*)NewtonBodyGetUserData( newtonBody0 );
    Body* body1 = (OgreNewt::Body*)NewtonBodyGetUserData( newtonBody1 );

    return me->m_contactcallback->onAABBOverlap( body0, body1, threadIndex );
}

	int PhysWorld3D::OnAABBOverlap(const NewtonMaterial* const material, const NewtonBody* const body0, const NewtonBody* const body1, int threadIndex)
	{
		RigidBody3D* bodyA = static_cast<RigidBody3D*>(NewtonBodyGetUserData(body0));
		RigidBody3D* bodyB = static_cast<RigidBody3D*>(NewtonBodyGetUserData(body1));
		assert(bodyA && bodyB);

		Callback* callbackData = static_cast<Callback*>(NewtonMaterialGetMaterialPairUserData(material));
		assert(callbackData);
		assert(callbackData->aabbOverlapCallback);

		return callbackData->aabbOverlapCallback(*bodyA, *bodyB);
	}

int _CDECL ContactCallback::contactBegin( const NewtonMaterial* material, const NewtonBody* body0, const NewtonBody* body1 )
{
	ContactCallback* me;

	me = (ContactCallback*)NewtonMaterialGetMaterialPairUserData( material );

	me->m_material = (NewtonMaterial*)material;

	//save the bodies...
	me->m_body0 = (OgreNewt::Body*)NewtonBodyGetUserData( body0 );
	me->m_body1 = (OgreNewt::Body*)NewtonBodyGetUserData( body1 );

	return me->userBegin();
}

	void PostUpdate(dFloat timestep, int threadIndex)
	{
		int count = 1;
		void* nodes[32];
		dMatrix parentMatrix[32];
		nodes[0] = NewtonInverseDynamicsGetRoot(m_kinematicSolver);

		parentMatrix[0] = dGetIdentityMatrix();

		NewtonWorld* const world = GetManager()->GetWorld();
		DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(world);

		while (count) {
			dMatrix matrix;

			count --;
			void* const rootNode = nodes[count];
			NewtonBody* const body = NewtonInverseDynamicsGetBody(m_kinematicSolver, rootNode);
			NewtonBodyGetMatrix (body, &matrix[0][0]);
			dMatrix localMatrix (matrix * parentMatrix[count]);

			DemoEntity* const ent = (DemoEntity*)NewtonBodyGetUserData(body);

			dQuaternion rot(localMatrix);
			ent->SetMatrix(*scene, rot, localMatrix.m_posit);

			matrix = matrix.Inverse();
			for (void* node = NewtonInverseDynamicsGetFirstChildNode(m_kinematicSolver, rootNode); node; node = NewtonInverseDynamicsGetNextChildNode(m_kinematicSolver, node)) {
				nodes[count] = node;
				parentMatrix[count] = matrix;
				count ++;
			}
		}
	}

void Body::__setTransformCallback(const NewtonBody* body, const dFloat* matrix, int threadIndex)
{
	//std::cout << "\ttransform " << threadIndex << " " << body << std::endl;
	Body* _body = (Body*)NewtonBodyGetUserData(body);
	_body->m_matrix = Mat4f(matrix);
	//std::cout << "\ttransform end " << threadIndex << " " << body << std::endl;
}

	virtual void EventCallback (const CustomTriggerController* const me, TriggerEventType event, NewtonBody* const visitor) const
	{
		// send this message to the entity
		DemoEntity* const entity = (DemoEntity*) NewtonBodyGetUserData(visitor);
		if (entity) {
			// map the event massage 
			int message = -1;
			switch (event) 
			{
				case m_enterTrigger:
					message = ENTER_TRIGGER;
					break;

				case m_exitTrigger:
					message = EXIT_TRIGGER;
					break;

				case m_inTrigger:
					message = INSIDE_TRIGGER;
					break;
			}
				
			// pass the controller pointer as the user data of this massage
			entity->MessageHandler(me->GetBody(), message, (void*)me);
		}
	}

void CProjectile::OnHit( const NewtonBody* body, vector3df vImpactVel, f32 fImpactMass )
{
    dFloat mass;
    dFloat Ixx;
    dFloat Iyy;
    dFloat Izz;

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

    if ( mass != 0.0f )
    {
      CNewtonNode* newtonNode = 0;
      newtonNode = ( CNewtonNode * )NewtonBodyGetUserData( body );
      if ( newtonNode )
      {
        OnHitNewtonNode( newtonNode, vImpactVel, fImpactMass );
      }

      vector3df vImpulse = vImpactVel* fImpactMass* fImpactMass* fImpactMass * 0.00001f; //TEMP: 0.00

      NewtonAddBodyImpulse( body, &vImpulse.X, &Pos.X );
    }
    else
    {
      OnHitLevel( vImpactVel, fImpactMass );
    }
}

static void OnReconstructMainMeshCallBack (NewtonBody* const body, NewtonFracturedCompoundMeshPart* const mainMesh, const NewtonCollision* const fracturedCompoundCollision)
{
	DemoEntity* const entity = (DemoEntity*)NewtonBodyGetUserData(body);
	DemoMesh* const visualMesh = (DemoMesh*)entity->GetMesh();
	dAssert (visualMesh->IsType(DemoMesh::GetRttiType()));
	
	dAssert (NewtonCollisionGetType(fracturedCompoundCollision) == SERIALIZE_ID_FRACTURED_COMPOUND);

	DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(NewtonBodyGetWorld(body));
	dAssert (scene);

	visualMesh->RemoveAll();
	for (void* segment = NewtonFracturedCompoundMeshPartGetFirstSegment(mainMesh); segment; segment = NewtonFracturedCompoundMeshPartGetNextSegment (segment)) {
		DemoSubMesh* const subMesh = visualMesh->AddSubMesh();

		int material = NewtonFracturedCompoundMeshPartGetMaterial (segment); 
		int indexCount = NewtonFracturedCompoundMeshPartGetIndexCount (segment); 

		subMesh->m_textureHandle = AddTextureRef ((GLuint)material);
		subMesh->m_shader = scene->GetShaderCache().m_diffuseEffect;

		subMesh->AllocIndexData (indexCount);
		subMesh->m_indexCount = NewtonFracturedCompoundMeshPartGetIndexStream (fracturedCompoundCollision, mainMesh, segment, (int*)subMesh->m_indexes); 
	}

	visualMesh->OptimizeForRender();
}

    unsigned _CDECL Raycast::newtonRaycastPreFilter(const NewtonBody *body, const NewtonCollision *collision, void* userData)
    {
        // get our object!
        Raycast* me = (Raycast*)userData;

        Body* bod = (Body*)NewtonBodyGetUserData( body );
        const World* world = bod->getWorld();


        me->m_treecollisioncallback_bodyalreadyadded = false;
        me->m_treecollisioncallback_lastbody = bod;

        if (me->userPreFilterCallback( bod ))
            return 1;
        else
        {

            if( world->getDebugger().isRaycastRecording() && world->getDebugger().isRaycastRecordingHitBodies() )
            {
                world->getDebugger().addDiscardedBody(bod);
            }

            return 0;
        }
    }

    float _CDECL Raycast::newtonRaycastFilter(const NewtonBody* body, const float* hitNormal, int collisionID, void* userData, float intersectParam)
    {
        // get our object!
        Raycast* me = (Raycast*)userData;

        Body* bod = (Body*)NewtonBodyGetUserData( body );
        const World* world = bod->getWorld();
        Ogre::Vector3 normal = Ogre::Vector3( hitNormal[0], hitNormal[1], hitNormal[2] );


        if( me->m_treecollisioncallback_bodyalreadyadded )
            return intersectParam;


        if( world->getDebugger().isRaycastRecording() && world->getDebugger().isRaycastRecordingHitBodies() )
        {
            world->getDebugger().addHitBody(bod);
        }


        if (me->userCallback( bod, intersectParam, normal, collisionID ))
            return intersectParam;
        else
            return 1.1;

    }

void dNewtonTriggerManager::EventCallback (const dCustomTriggerController* const trigger, dTriggerEventType event, NewtonBody* const guess) const
{
	dNewtonTrigger* const callback = (dNewtonTrigger*) trigger->GetUserData();
	dNewtonBody* const guessBody = (dNewtonBody*) NewtonBodyGetUserData(guess);
	switch (event) 
	{
		case m_enterTrigger:
		{
			callback->OnEnter(guessBody);
			break;
		}

		case m_exitTrigger:
		{
			callback->OnExit(guessBody);
			break;
		}

		case m_inTrigger:
		{
			callback->OnInside(guessBody);
			break;
		}
	}
}

	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));
	}

		unsigned _convexCastBodyFilter(const NewtonBody* body, const NewtonCollision* collision, void* userData)
		{
			tPlayerController * ctrl = (tPlayerController *)userData;
			tBody * tbody = (tBody *)NewtonBodyGetUserData(body);

			return 1;
		}

void _cdecl Body::newtonForceTorqueCallback( const NewtonBody* body )
{
	OgreNewt::Body* me = (OgreNewt::Body*)NewtonBodyGetUserData( body );

	if (me->m_forcecallback)
		(*me->m_forcecallback)( me );
}

void RigidBodyWorld::UpdatePhysics ()
{
	RigidBodyWorldDesc* const desc = (RigidBodyWorldDesc*) RigidBodyWorldDesc::GetDescriptor();

	float timestep = 1.0f / desc->m_minFps;
	if (timestep > 1.0f / 60.0f) {
		timestep = 1.0f / 60.0f;
	}


	desc->m_updateRigidBodyMatrix = false;
	NewtonUpdate(desc->m_newton, timestep);
	TimeValue t (GetCOREInterface()->GetTime());

	float scale = 1.0f / float (GetMasterScale(UNITS_METERS));
	for (NewtonBody* body = NewtonWorldGetFirstBody(desc->m_newton); body; body = NewtonWorldGetNextBody(desc->m_newton, body))	{
		dMatrix matrix;
		INode* const node = (INode*)NewtonBodyGetUserData(body);
		NewtonBodyGetMatrix(body, &matrix[0][0]);

		matrix = desc->m_systemMatrix * matrix * desc->m_systemMatrixInv;
		matrix.m_posit = matrix.m_posit.Scale (scale);

		Matrix3 maxMatrix (GetMatrixFromdMatrix (matrix));
		node->SetNodeTM(t, maxMatrix);
	}
	
	UpdateViewPorts ();

	desc->m_updateRigidBodyMatrix = true;
}

unsigned dNewtonRayCast::PrefilterCallback(const NewtonBody* const body, const NewtonCollision* const collision, void* const userData)
{
	dNewtonRayCast* const me = (dNewtonRayCast*) userData;
	const dNewtonBody* const myBody = (dNewtonBody*) NewtonBodyGetUserData(body);
	const dNewtonCollision* const myCollision = (dNewtonCollision*) NewtonCollisionGetUserData(collision);
	return me->OnPrefilter (myBody, myCollision);
}

// Transform callback to set the matrix of a the visual entity
void SetTransformCallback (const NewtonBody* body, const float* matrix, int threadIndex)
{
	NewtonEntity* ent;

	// Get the position from the matrix
	glm::vec4 position (matrix[12], matrix[13], matrix[14], 1.0f);
	glm::quat rotation;

	// we will ignore the Rotation part of matrix and use the quaternion rotation stored in the body
	NewtonBodyGetRotation(body, &rotation[0]);

	// get the entity associated with this rigid body
	ent = (NewtonEntity*) NewtonBodyGetUserData(body);

	// since this tutorial run the physics and a different fps than the Graphics
	// we need to save the entity current transformation state before updating the new state.
	ent->prevPosition = ent->curPosition;
	ent->prevRotation = ent->curRotation;

	if (glm::dot(ent->curRotation,rotation) < 0.0f) {
		ent->prevRotation *= -1.0f;
	}

	// set the new position and orientation for this entity
	ent->curPosition = position;
	ent->curRotation = rotation;
}