/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
* 
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* 
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "dgPhysicsStdafx.h"
#include "dgBody.h"
#include "dgWorld.h"
#include "dgContact.h"


#define REST_RELATIVE_VELOCITY		dgFloat32 (1.0e-3f)
#define MAX_DYNAMIC_FRICTION_SPEED	dgFloat32 (0.3f)


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

dgContactMaterial::dgContactMaterial()
{
	//	_ASSERTE ((sizeof (dgContactMaterial) & 15) == 0);
	_ASSERTE ((((dgUnsigned64) this) & 15) == 0);
	m_point.m_x = dgFloat32 (0.0f);
	m_point.m_y = dgFloat32 (0.0f);
	m_point.m_z = dgFloat32 (0.0f);
	m_softness = dgFloat32 (0.1f);
	m_restitution = dgFloat32 (0.4f);

	m_staticFriction0 = dgFloat32 (0.9f);
	m_staticFriction1 = dgFloat32 (0.9f);
	m_dynamicFriction0 = dgFloat32 (0.5f);
	m_dynamicFriction1 = dgFloat32 (0.5f);
	m_dir0_Force = dgFloat32 (0.0f);
	m_dir1_Force = dgFloat32 (0.0f);
	m_normal_Force = dgFloat32 (0.0f);
	m_penetrationPadding = dgFloat32 (0.0f);
//m_penetrationPadding = 0.5f;

//	m_collisionEnable = true;
//	m_friction0Enable = true;
//	m_friction1Enable = true;
//	m_collisionContinueCollisionEnable = true;
	m_flags = m_collisionEnable__ | m_friction0Enable__ | m_friction1Enable__ | m_collisionContinueCollisionEnable__;
}



dgContact::dgContact(dgWorld* world)
	:dgConstraint(), dgList<dgContactMaterial>(world->GetAllocator())
{
	_ASSERTE ((((dgUnsigned64) this) & 15) == 0);

	dgActiveContacts &activeContacts = *world;

	m_contactNode = activeContacts.Append (this);

	m_maxDOF = 3;
	m_broadphaseLru = 0;
	m_enableCollision = true;
	m_constId = dgContactConstraintId;
	m_world = world;
	m_myCacheMaterial = NULL;
}

inline dgContact::~dgContact()
{
	dgActiveContacts &activeContacts = *m_world;
	dgList<dgContactMaterial>::RemoveAll();
	activeContacts.Remove (m_contactNode);
}

void dgContact::GetInfo (dgConstraintInfo* const info) const
{
	memset (info, 0, sizeof (dgConstraintInfo));
	InitInfo (info);
	info->m_collideCollisionOn = GetCount();
	strcpy (info->m_discriptionType, "contact");
}

void dgContact::CalculatePointDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& dir, const dgPointParam& param) const
{
	_ASSERTE (m_body0);
	_ASSERTE (m_body1);

	dgVector r0CrossDir (param.m_r0 * dir);
	dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobian_IM0; 
	jacobian0.m_linear[0] = dir.m_x;
	jacobian0.m_linear[1] = dir.m_y;
	jacobian0.m_linear[2] = dir.m_z;
	jacobian0.m_linear[3] = dgFloat32 (0.0f);
	jacobian0.m_angular[0] = r0CrossDir.m_x;
	jacobian0.m_angular[1] = r0CrossDir.m_y;
	jacobian0.m_angular[2] = r0CrossDir.m_z;
	jacobian0.m_angular[3] = dgFloat32 (0.0f);


	dgVector r1CrossDir (dir * param.m_r1);
	dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobian_IM1; 
	jacobian1.m_linear[0] = -dir.m_x;
	jacobian1.m_linear[1] = -dir.m_y;
	jacobian1.m_linear[2] = -dir.m_z;
	jacobian1.m_linear[3] =  dgFloat32 (0.0f);
	jacobian1.m_angular[0] = r1CrossDir.m_x;
	jacobian1.m_angular[1] = r1CrossDir.m_y;
	jacobian1.m_angular[2] = r1CrossDir.m_z;
	jacobian1.m_angular[3] = dgFloat32 (0.0f);
}

dgUnsigned32 dgContact::JacobianDerivative (dgContraintDescritor& params)
{
	dgInt32 frictionIndex = 0;
	if (m_maxDOF) {
		dgInt32 i = 0;
		frictionIndex = GetCount();
		for (dgList<dgContactMaterial>::dgListNode* node = GetFirst(); node; node = node->GetNext()) {
			const dgContactMaterial& contact = node->GetInfo(); 
			JacobianContactDerivative (params, contact, i, frictionIndex);
			i ++;
		}
	}

	return dgUnsigned32 (frictionIndex);
}



void dgContact::JacobianContactDerivative (dgContraintDescritor& params, const dgContactMaterial& contact, dgInt32 normalIndex, dgInt32& frictionIndex) 
{
	dgPointParam pointData;

	InitPointParam (pointData, dgFloat32 (1.0f), contact.m_point, contact.m_point);
	CalculatePointDerivative (normalIndex, params, contact.m_normal, pointData); 

	dgVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
	dgFloat32 restitution	= contact.m_restitution;

	dgFloat32 relVelocErr = velocError % contact.m_normal;

	dgFloat32 penetration = GetMin (contact.m_penetration, dgFloat32(0.5f));
	dgFloat32 penetrationStiffness = dgFloat32 (50.0f) * contact.m_softness;
	dgFloat32 penetrationVeloc = penetration * penetrationStiffness;
	_ASSERTE (dgAbsf (penetrationVeloc - dgFloat32 (50.0f) * contact.m_softness * GetMin (contact.m_penetration, dgFloat32(0.5f))) < dgFloat32 (1.0e-6f));
	if (relVelocErr > REST_RELATIVE_VELOCITY) {
		relVelocErr *= (restitution + dgFloat32 (1.0f));
	}

	params.m_restitution[normalIndex] = restitution;
	params.m_penetration[normalIndex] = penetration;
	params.m_penetrationStiffness[normalIndex] = penetrationStiffness;
	params.m_forceBounds[normalIndex].m_low = dgFloat32 (0.0f);
	params.m_forceBounds[normalIndex].m_normalIndex = DG_NORMAL_CONSTRAINT;
	params.m_forceBounds[normalIndex].m_jointForce = (dgFloat32*)&contact.m_normal_Force;
	params.m_jointStiffness[normalIndex] = dgFloat32 (1.0f);
	params.m_isMotor[normalIndex] = 0;


	params.m_jointAccel[normalIndex] = GetMax (dgFloat32 (-4.0f), relVelocErr + penetrationVeloc) * params.m_invTimestep;
//	params.m_jointAccel[normalIndex] = (penetrationVeloc + relVelocErr) * params.m_invTimestep;
	if (contact.m_flags & dgContactMaterial::m_overrideNormalAccel__) {
		params.m_jointAccel[normalIndex] += contact.m_normal_Force;
	}

	// first dir friction force
	if (contact.m_flags & dgContactMaterial::m_friction0Enable__) {
		dgInt32 jacobIndex = frictionIndex;
		frictionIndex += 1;
		CalculatePointDerivative (jacobIndex, params, contact.m_dir0, pointData); 
		relVelocErr = velocError % contact.m_dir0;
		params.m_forceBounds[jacobIndex].m_normalIndex = normalIndex;
		params.m_jointStiffness[jacobIndex] = dgFloat32 (1.0f);

		params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
		params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
		params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
//		if (contact.m_override0Accel) {
		if (contact.m_flags & dgContactMaterial::m_override0Accel__) {
			params.m_jointAccel[jacobIndex] = contact.m_dir0_Force;
			params.m_isMotor[jacobIndex] = 1;
		} else {
			params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
			params.m_isMotor[jacobIndex] = 0;
		}
		if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
			params.m_forceBounds[jacobIndex].m_low = -contact.m_dynamicFriction0;
			params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction0;
		} else {
			params.m_forceBounds[jacobIndex].m_low = -contact.m_staticFriction0;
			params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction0;
		}
		params.m_forceBounds[jacobIndex].m_jointForce = (dgFloat32*)&contact.m_dir0_Force;
	}

//	if (contact.m_friction1Enable) {
	if (contact.m_flags & dgContactMaterial::m_friction1Enable__) {
		dgInt32 jacobIndex = frictionIndex;
		frictionIndex += 1;
		CalculatePointDerivative (jacobIndex, params, contact.m_dir1, pointData); 
		relVelocErr = velocError % contact.m_dir1;
		params.m_forceBounds[jacobIndex].m_normalIndex = normalIndex;
		params.m_jointStiffness[jacobIndex] = dgFloat32 (1.0f);

		params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
		params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
		params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
//		if (contact.m_override1Accel) {
		if (contact.m_flags & dgContactMaterial::m_override1Accel__) {
			_ASSERTE (0);
			params.m_jointAccel[jacobIndex] = contact.m_dir1_Force;
			params.m_isMotor[jacobIndex] = 1;
		} else {
			params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
			params.m_isMotor[jacobIndex] = 0;
		}
		if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
			params.m_forceBounds[jacobIndex].m_low = - contact.m_dynamicFriction1;
			params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction1;
		} else {
			params.m_forceBounds[jacobIndex].m_low = - contact.m_staticFriction1;
			params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction1;
		}
		params.m_forceBounds[jacobIndex].m_jointForce = (dgFloat32*)&contact.m_dir1_Force;
	}

//dgTrace (("p(%f %f %f)\n", params.m_jointAccel[normalIndex], params.m_jointAccel[normalIndex + 1], params.m_jointAccel[normalIndex + 2]));

}



void dgContact::JointAccelerationsSimd(const dgJointAccelerationDecriptor& params)
{
#ifdef DG_BUILD_SIMD_CODE
//	simd_type bodyVeloc0;
//	simd_type bodyOmega0;
//	simd_type bodyVeloc1;
//	simd_type bodyOmega1;
//	simd_type zero;
//	simd_type four;
//	simd_type negOne;
//	simd_type tol002;
//	simd_type timeStep;
//	simd_type invTimeStep;

	const dgJacobianPair* const Jt = params.m_Jt;

	simd_type zero = simd_set1 (dgFloat32 (0.0f));
	simd_type four = simd_set1 (dgFloat32 (4.0f));
	simd_type negOne = simd_set1 (dgFloat32 (-1.0f));
	simd_type tol002 = simd_set1 (dgFloat32 (1.0e-2f));
	simd_type timeStep = simd_set1 (params.m_timeStep);
	simd_type invTimeStep = simd_set1 (params.m_invTimeStep);

	simd_type bodyVeloc0 = (simd_type&) m_body0->m_veloc;
	simd_type bodyOmega0 = (simd_type&) m_body0->m_omega;
	simd_type bodyVeloc1 = (simd_type&) m_body1->m_veloc;
	simd_type bodyOmega1 = (simd_type&) m_body1->m_omega;

	for (dgInt32 k = 0; k < params.m_rowsCount; k ++) {
		if (!params.m_accelIsMotor[k]) {
			//dgVector relVeloc (Jt[k].m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0));
			//relVeloc += Jt[k].m_jacobian_IM0.m_angular.CompProduct(bodyOmega0);
			//relVeloc += Jt[k].m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1);
			//relVeloc += Jt[k].m_jacobian_IM1.m_angular.CompProduct(bodyOmega1);
			simd_type relVeloc = simd_mul_v (              (simd_type&)Jt[k].m_jacobian_IM0.m_linear, bodyVeloc0); 
			relVeloc = simd_mul_add_v (relVeloc, (simd_type&)Jt[k].m_jacobian_IM0.m_angular, bodyOmega0); 
			relVeloc = simd_mul_add_v (relVeloc, (simd_type&)Jt[k].m_jacobian_IM1.m_linear, bodyVeloc1); 
			relVeloc = simd_mul_add_v (relVeloc, (simd_type&)Jt[k].m_jacobian_IM1.m_angular, bodyOmega1); 

			//		vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
			//		aRel = relAccel.m_x + relAccel.m_y + relAccel.m_z;
			relVeloc = simd_add_v (relVeloc, simd_move_hl_v(relVeloc, relVeloc));
			relVeloc = simd_add_s (relVeloc, simd_permut_v (relVeloc, relVeloc, PURMUT_MASK(3, 3, 3, 1)));
			simd_type relAccel = simd_load_s (params.m_externAccelaration[k]);
			if (params.m_normalForceIndex[k] < 0) {
				//			dgFloat32 restitution;
				//simd_type penetration; 
				//simd_type restitution; 
				//simd_type velocMask;
				//simd_type penetrationVeloc;
				//simd_type penetrationMask; 

				//			restitution = dgFloat32 (1.0f);
				//			if (vRel <= dgFloat32 (0.0f)) {
				//				restitution += params.m_restitution[k];
				//			}
				simd_type restitution = simd_sub_s (simd_and_v (simd_set1 (params.m_restitution[k]), simd_cmplt_s (relVeloc, zero)), negOne);

				simd_type penetration = simd_load_s (params.m_penetration[k]);
				//			dgFloat32 penetrationVeloc;
				//			penetrationVeloc = 0.0f;
				//			if (params.m_penetration[k] > dgFloat32 (1.0e-2f)) {
				//				if (vRel > dgFloat32 (0.0f)) {
				//					_ASSERTE (penetrationCorrection >= dgFloat32 (0.0f));
				//					params.m_penetration[k] = GetMax (dgFloat32 (0.0f), params.m_penetration[k] - vRel * params.m_timeStep);
				//				}
				//				penetrationVeloc = -(params.m_penetration[k] * params.m_penetrationStiffness[k]);
				//			}
				simd_type penetrationMask = simd_cmpgt_s (penetration, tol002);
				simd_type velocMask = simd_and_v (penetrationMask, simd_cmpgt_s (relVeloc, zero));
				penetration = simd_max_s (zero, simd_sub_s (penetration, simd_and_v (simd_mul_s (relVeloc, timeStep), velocMask)));
				simd_type penetrationVeloc = simd_and_v (simd_mul_s (penetration, simd_load_s (params.m_penetrationStiffness[k])), penetrationMask);
				simd_store_s (penetration, &params.m_penetration[k]);

				//			vRel *= restitution;
				//			vRel = GetMin (dgFloat32 (4.0f), vRel + penetrationVeloc);
				//			relVeloc = simd_mul_s (relVeloc, restitution);
				relVeloc = simd_min_s (four, simd_sub_s (simd_mul_s (relVeloc, restitution), penetrationVeloc));
			}

			//params.m_coordenateAccel[k] = -aRel;
			//relAccel = simd_mul_add_s (relAccel, relVeloc, invTimeStep);
			//simd_store_s (simd_mul_s (relAccel, negOne), &params.m_coordenateAccel[k]);

			//params.m_coordenateAccel[k] = (aRel - vRel * params.m_invTimeStep);
			simd_store_s (simd_mul_sub_s (relAccel, relVeloc, invTimeStep), &params.m_coordenateAccel[k]);
		}
	}
#endif
}


void dgContact::JointAccelerations(const dgJointAccelerationDecriptor& params)
{
	const dgJacobianPair* const Jt = params.m_Jt;
	const dgVector& bodyVeloc0 = m_body0->m_veloc;
	const dgVector& bodyOmega0 = m_body0->m_omega;
	const dgVector& bodyVeloc1 = m_body1->m_veloc;
	const dgVector& bodyOmega1 = m_body1->m_omega;

	for (dgInt32 k = 0; k < params.m_rowsCount; k ++) {
		if (!params.m_accelIsMotor[k]) {
			dgFloat32 vRel;
			dgFloat32 aRel;

			dgVector relVeloc (Jt[k].m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0));
			relVeloc += Jt[k].m_jacobian_IM0.m_angular.CompProduct(bodyOmega0);
			relVeloc += Jt[k].m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1);
			relVeloc += Jt[k].m_jacobian_IM1.m_angular.CompProduct(bodyOmega1);
			vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
			aRel = params.m_externAccelaration[k];

			if (params.m_normalForceIndex[k] < 0) {
				dgFloat32 restitution;
				dgFloat32 penetrationVeloc;
				

				restitution = dgFloat32 (1.0f);
				if (vRel <= dgFloat32 (0.0f)) {
					restitution += params.m_restitution[k];
				}

				penetrationVeloc = 0.0f;
				if (params.m_penetration[k] > dgFloat32 (1.0e-2f)) {
					dgFloat32 penetrationCorrection;
					if (vRel > dgFloat32 (0.0f)) {
						penetrationCorrection = vRel * params.m_timeStep;
						_ASSERTE (penetrationCorrection >= dgFloat32 (0.0f));
						params.m_penetration[k] = GetMax (dgFloat32 (0.0f), params.m_penetration[k] - penetrationCorrection);
					}
					penetrationVeloc = -(params.m_penetration[k] * params.m_penetrationStiffness[k]);
				}

				vRel *= restitution;
				vRel = GetMin (dgFloat32 (4.0f), vRel + penetrationVeloc);
			}
			params.m_coordenateAccel[k] =  (aRel - vRel * params.m_invTimeStep);
		}
	}
}

void dgContact::JointVelocityCorrection(const dgJointAccelerationDecriptor& params)
{
	_ASSERTE (0);
}


dgCollidingPairCollector::dgCollidingPairCollector ()
{
	m_count = 0;
	m_maxSize = 0;
//	m_world = NULL;
	m_pairs = NULL;
	m_sentinel = NULL;
}

dgCollidingPairCollector::~dgCollidingPairCollector ()
{
}

void dgCollidingPairCollector::Init ()
{
//	m_world = me;
	dgWorld* const world = (dgWorld*) this;
	m_count = 0;
	m_maxSize = dgInt32 (world->m_pairMemoryBufferSizeInBytes / sizeof (dgPair));
	m_pairs = (dgPair*) world->m_pairMemoryBuffer ;
}

void dgCollidingPairCollector::SetCaches (dgThreadPairCache* const chaches)
{
	for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i ++) {
		m_chacheBuffers[i] = &chaches[i];
		m_chacheBuffers[i]->m_count = 0;
	}
}

void dgCollidingPairCollector::FlushChache (dgThreadPairCache* const pairChache)
{
	dgWorld* const world = (dgWorld*) this;
	while ((m_count + pairChache->m_count) > m_maxSize) {
		void* newBuffer;
		newBuffer = world->m_allocator->Malloc (2 * world->m_pairMemoryBufferSizeInBytes) ;

		world->m_pairMemoryBufferSizeInBytes *= 2;
		memcpy (newBuffer, world->m_pairMemoryBuffer, m_maxSize * sizeof (dgPair));
		world->m_allocator->Free (world->m_pairMemoryBuffer);

		world->m_pairMemoryBuffer = newBuffer;
		m_maxSize = dgInt32 (world->m_pairMemoryBufferSizeInBytes / sizeof (dgPair));
		m_pairs = (dgPair*) world->m_pairMemoryBuffer ;
	}

	memcpy (&m_pairs[m_count], pairChache->m_chacheBuffer, sizeof (dgPair) * pairChache->m_count);
	m_count += pairChache->m_count;
	pairChache->m_count = 0;
}

void dgCollidingPairCollector::AddPair (dgBody* const bodyPtr0, dgBody* const bodyPtr1, dgInt32 threadIndex)
{
	if ((bodyPtr0 != m_sentinel) && (bodyPtr1 != m_sentinel)) {
		dgWorld* const world = (dgWorld*) this;
		if (bodyPtr0->GetSleepState() & bodyPtr1->GetSleepState()) {
			dgContact* contact = NULL;
			if (bodyPtr0->m_invMass.m_w != dgFloat32 (0.0f)) {
				for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (bodyPtr0, bodyPtr1); link; link = world->FindConstraintLinkNext (link, bodyPtr1)) {
					dgConstraint* const constraint = link->GetInfo().m_joint;
					if (constraint->GetId() == dgContactConstraintId) {
						contact = (dgContact*)constraint;
						break;
					}
				}
			} else if (bodyPtr1->m_invMass.m_w != dgFloat32 (0.0f)) {
				_ASSERTE (bodyPtr1->m_invMass.m_w != dgFloat32 (0.0f));
				for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (bodyPtr1, bodyPtr0); link; link = world->FindConstraintLinkNext (link, bodyPtr0)) {
					dgConstraint* const constraint = link->GetInfo().m_joint;
					if (constraint->GetId() == dgContactConstraintId) {
						contact = (dgContact*)constraint;
						break;
					} 
				}
			} else {
				return;
			}

			if (contact) {
				_ASSERTE (contact->GetId() == dgContactConstraintId);
				contact->m_broadphaseLru = dgInt32 (world->m_broadPhaseLru);
			}
			
		} else {
			dgBody* tmpbody0 (bodyPtr0);
			dgBody* tmpbody1 (bodyPtr1);
			if (tmpbody0->m_uniqueID > tmpbody1->m_uniqueID) {
				Swap(tmpbody0, tmpbody1);
			}
			dgBody* const body0 (tmpbody0);
			dgBody* const body1 (tmpbody1);

			_ASSERTE (body0->GetWorld());
			_ASSERTE (body1->GetWorld());
			_ASSERTE (body0->GetWorld() == world);
			_ASSERTE (body1->GetWorld() == world);

			dgContact* contact = NULL;
			if (body0->m_invMass.m_w != dgFloat32 (0.0f)) {
				for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (body0, body1); link; link = world->FindConstraintLinkNext (link, body1)) {
					dgConstraint* const constraint = link->GetInfo().m_joint;
					if (constraint->GetId() == dgContactConstraintId) {
						contact = (dgContact*)constraint;
					} else {
						if (!constraint->IsCollidable()) {
							return;
						} 
					}
				}
			} else if (body1->m_invMass.m_w != dgFloat32 (0.0f)) {
				_ASSERTE (body1->m_invMass.m_w != dgFloat32 (0.0f));
				for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (body1, body0); link; link = world->FindConstraintLinkNext (link, body0)) {
					dgConstraint* const constraint = link->GetInfo().m_joint;
					if (constraint->GetId() == dgContactConstraintId) {
						contact = (dgContact*)constraint;
					} else {
						if (!constraint->IsCollidable()) {
							return;
						}
					}
				}
			} else {
				return;
			}

			if (!(body0->m_collideWithLinkedBodies & body1->m_collideWithLinkedBodies)) {
				if (world->AreBodyConnectedByJoints (body0, body1)) {
					return;
				}
			}

		
			_ASSERTE (!contact || contact->GetId() == dgContactConstraintId);

			dgUnsigned32 group0_ID = dgUnsigned32 (body0->m_bodyGroupId);
			dgUnsigned32 group1_ID = dgUnsigned32 (body1->m_bodyGroupId);
			if (group1_ID < group0_ID) {
				Swap (group0_ID, group1_ID);
			}

			dgUnsigned32 key = (group1_ID << 16) + group0_ID;
			const dgBodyMaterialList& materialList = *world;  

			const dgContactMaterial* const material = &materialList.Find (key)->GetInfo();
//			if (material->m_collisionEnable) {
			if (material->m_flags & dgContactMaterial::m_collisionEnable__) {
				dgInt32 processContacts;

				processContacts = 1;
				if (material->m_aabbOverlap) {
					processContacts = material->m_aabbOverlap (*material, *body0, *body1, threadIndex);
				}
				if (processContacts) {
					_ASSERTE (!body0->m_collision->IsType (dgCollision::dgCollisionNull_RTTI));
					_ASSERTE (!body1->m_collision->IsType (dgCollision::dgCollisionNull_RTTI));

					dgThreadPairCache& pairChache = *m_chacheBuffers[threadIndex];
					
					if (pairChache.m_count >= DG_CACHE_PAIR_BUFFER) {
						world->dgGetUserLock();
						FlushChache (&pairChache);
						world->dgReleasedUserLock();
					}

					dgInt32 count = pairChache.m_count;
					pairChache.m_chacheBuffer[count].m_body0 = body0;
					pairChache.m_chacheBuffer[count].m_body1 = body1;
					pairChache.m_chacheBuffer[count].m_material = material; 
					pairChache.m_chacheBuffer[count].m_contact = contact;
					pairChache.m_count = count + 1;
				}
			}
		}
	}
}