/* 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 "hpl1/engine/libraries/newton/core/dg.h"

#include "dgBodyMasterList.h"
#include "dgCollision.h"
#include "dgCollisionBVH.h"
#include "dgCollisionBox.h"
#include "dgCollisionCapsule.h"
#include "dgCollisionChamferCylinder.h"
#include "dgCollisionCompound.h"
#include "dgCollisionCompoundBreakable.h"
#include "dgCollisionCone.h"
#include "dgCollisionConvexHull.h"
#include "dgCollisionConvexModifier.h"
#include "dgCollisionCylinder.h"
#include "dgCollisionEllipse.h"
#include "dgCollisionHeightField.h"
#include "dgCollisionNull.h"
#include "dgCollisionScene.h"
#include "dgCollisionSphere.h"
#include "dgCollisionUserMesh.h"
#include "dgWorld.h"
#include "dgWorldDynamicUpdate.h"


#define DG_USE_CACHE_CONTACTS

#define DG_PRUNE_PADDING_BYTES 128

//#define DG_CONTACT_CACHE_TOLERANCE dgFloat32 (1.0e-6f)
//#define DG_REDUCE_CONTACT_TOLERANCE dgFloat32 (1.0e-2f)
//#define DG_PRUNE_CONTACT_TOLERANCE dgFloat32 (1.0e-2f)

#define DG_COMPOUND_MAX_SORT_ARRAY (1024 * 2)

dgCollision *dgWorld::CreateNull() {
	dgUnsigned32 crc;
	dgCollision *collision;
	dgBodyCollisionList::dgTreeNode *node;

	crc = dgCollision::dgCollisionNull_RTTI;

	node = dgBodyCollisionList::Find(crc);
	if (!node) {
		collision = new (m_allocator) dgCollisionNull(m_allocator, crc);
		node = dgBodyCollisionList::Insert(collision, crc);
	}

	node->GetInfo()->AddRef();
	return node->GetInfo();
}

dgCollision *dgWorld::CreateBox(dgFloat32 dx, dgFloat32 dy, dgFloat32 dz,
                                dgInt32 shapeID, const dgMatrix &offsetMatrix) {
	//  dgUnsigned32 crc;
	//  dgCollision* collision;
	//  dgBodyCollisionList::dgTreeNode* node;
	dgUnsigned32 buffer[2 * sizeof(dgMatrix) / sizeof(dgInt32)];

	dx = dgAbsf(dx);
	dy = dgAbsf(dy);
	dz = dgAbsf(dz);

	memset(buffer, 0, sizeof(buffer));
	buffer[0] = m_boxCollision;
	buffer[1] = dgCollision::Quantize(dx * dgFloat32(0.5f));
	buffer[2] = dgCollision::Quantize(dy * dgFloat32(0.5f));
	buffer[3] = dgCollision::Quantize(dz * dgFloat32(0.5f));
	buffer[4] = dgUnsigned32(shapeID);
	memcpy(&buffer[5], &offsetMatrix, sizeof(dgMatrix));
	dgUnsigned32 crc = dgCollision::MakeCRC(buffer, sizeof(buffer));

	dgBodyCollisionList::dgTreeNode *node = dgBodyCollisionList::Find(crc);
	if (!node) {
		dgCollision *const collision = new (m_allocator) dgCollisionBox(m_allocator,
		        crc, dx, dy, dz, offsetMatrix);
		collision->SetUserDataID(dgUnsigned32(shapeID));
		node = dgBodyCollisionList::Insert(collision, crc);
	}

	node->GetInfo()->AddRef();
	return node->GetInfo();
}

dgCollision *dgWorld::CreateSphere(dgFloat32 radii, dgInt32 shapeID,
                                   const dgMatrix &offsetMatrix) {
	dgUnsigned32 buffer[2 * sizeof(dgMatrix) / sizeof(dgInt32)];
	radii = dgAbsf(radii);

	memset(buffer, 0, sizeof(buffer));
	buffer[0] = m_sphereCollision;
	buffer[1] = dgCollision::Quantize(radii);
	buffer[2] = dgUnsigned32(shapeID);
	memcpy(&buffer[3], &offsetMatrix, sizeof(dgMatrix));
	dgUnsigned32 crc = dgCollision::MakeCRC(buffer, sizeof(buffer));

	dgBodyCollisionList::dgTreeNode *node = dgBodyCollisionList::Find(crc);
	if (!node) {
		dgCollision *const collision = new (m_allocator) dgCollisionSphere(
		    m_allocator, crc, radii, offsetMatrix);
		collision->SetUserDataID(dgUnsigned32(shapeID));
		node = dgBodyCollisionList::Insert(collision, crc);
	}

	node->GetInfo()->AddRef();
	return node->GetInfo();
}

dgCollision *dgWorld::CreateEllipse(dgFloat32 rx, dgFloat32 ry, dgFloat32 rz,
                                    dgInt32 shapeID, const dgMatrix &offsetMatrix) {
	dgUnsigned32 buffer[2 * sizeof(dgMatrix) / sizeof(dgInt32)];

	rx = dgAbsf(rx);
	ry = dgAbsf(ry);
	rz = dgAbsf(rz);

	memset(buffer, 0, sizeof(buffer));
	buffer[0] = m_ellipseCollision;
	buffer[1] = dgCollision::Quantize(rx);
	buffer[2] = dgCollision::Quantize(ry);
	buffer[3] = dgCollision::Quantize(rz);
	buffer[4] = dgUnsigned32(shapeID);
	memcpy(&buffer[5], &offsetMatrix, sizeof(dgMatrix));
	dgUnsigned32 crc = dgCollision::MakeCRC(buffer, sizeof(buffer));

	dgBodyCollisionList::dgTreeNode *node = dgBodyCollisionList::Find(crc);
	if (!node) {
		dgCollision *const collision = new (m_allocator) dgCollisionEllipse(
		    m_allocator, crc, rx, ry, rz, offsetMatrix);
		collision->SetUserDataID(dgUnsigned32(shapeID));
		node = dgBodyCollisionList::Insert(collision, crc);
	}

	node->GetInfo()->AddRef();
	return node->GetInfo();
}

dgCollision *dgWorld::CreateCapsule(dgFloat32 radius, dgFloat32 height,
                                    dgInt32 shapeID, const dgMatrix &offsetMatrix) {
	dgUnsigned32 crc;
	dgCollision *collision;
	dgBodyCollisionList::dgTreeNode *node;
	dgUnsigned32 buffer[2 * sizeof(dgMatrix) / sizeof(dgInt32)];

	radius = dgAbsf(radius);
	height = dgAbsf(height);

	memset(buffer, 0, sizeof(buffer));
	buffer[0] = m_capsuleCollision;
	buffer[1] = dgCollision::Quantize(radius);
	buffer[2] = dgCollision::Quantize(height * dgFloat32(0.5f));
	buffer[3] = dgUnsigned32(shapeID);
	memcpy(&buffer[4], &offsetMatrix, sizeof(dgMatrix));
	crc = dgCollision::MakeCRC(buffer, sizeof(buffer));

	node = dgBodyCollisionList::Find(crc);
	if (!node) {
		collision = new (m_allocator) dgCollisionCapsule(m_allocator, crc, radius,
		        height, offsetMatrix);
		collision->SetUserDataID(dgUnsigned32(shapeID));
		node = dgBodyCollisionList::Insert(collision, crc);
	}

	node->GetInfo()->AddRef();
	return node->GetInfo();
}

dgCollision *dgWorld::CreateCone(dgFloat32 radius, dgFloat32 height,
                                 dgInt32 shapeID, const dgMatrix &offsetMatrix) {
	dgUnsigned32 crc;
	dgCollision *collision;
	dgBodyCollisionList::dgTreeNode *node;
	dgUnsigned32 buffer[2 * sizeof(dgMatrix) / sizeof(dgInt32)];

	radius = dgAbsf(radius);
	height = dgAbsf(height);

	memset(buffer, 0, sizeof(buffer));
	buffer[0] = m_coneCollision;
	buffer[1] = dgCollision::Quantize(radius);
	buffer[2] = dgCollision::Quantize(height * dgFloat32(0.5f));
	buffer[3] = dgUnsigned32(shapeID);
	memcpy(&buffer[4], &offsetMatrix, sizeof(dgMatrix));
	crc = dgCollision::MakeCRC(buffer, sizeof(buffer));

	node = dgBodyCollisionList::Find(crc);
	if (!node) {
		collision = new (m_allocator) dgCollisionCone(m_allocator, crc, radius,
		        height, offsetMatrix);
		collision->SetUserDataID(dgUnsigned32(shapeID));
		node = dgBodyCollisionList::Insert(collision, crc);
	}

	node->GetInfo()->AddRef();
	return node->GetInfo();
}

dgCollision *dgWorld::CreateCylinder(dgFloat32 radius, dgFloat32 height,
                                     dgInt32 shapeID, const dgMatrix &offsetMatrix) {
	dgUnsigned32 crc;
	dgCollision *collision;
	dgBodyCollisionList::dgTreeNode *node;
	dgUnsigned32 buffer[2 * sizeof(dgMatrix) / sizeof(dgInt32)];

	radius = dgAbsf(radius);
	height = dgAbsf(height);

	memset(buffer, 0, sizeof(buffer));
	buffer[0] = m_cylinderCollision;
	buffer[1] = dgCollision::Quantize(radius);
	buffer[2] = dgCollision::Quantize(height * dgFloat32(0.5f));
	buffer[3] = dgUnsigned32(shapeID);
	memcpy(&buffer[4], &offsetMatrix, sizeof(dgMatrix));
	crc = dgCollision::MakeCRC(buffer, sizeof(buffer));

	node = dgBodyCollisionList::Find(crc);
	if (!node) {
		collision = new (m_allocator) dgCollisionCylinder(m_allocator, crc, radius,
		        height, offsetMatrix);
		collision->SetUserDataID(dgUnsigned32(shapeID));
		node = dgBodyCollisionList::Insert(collision, crc);
	}

	node->GetInfo()->AddRef();
	return node->GetInfo();
}

dgCollision *dgWorld::CreateChamferCylinder(dgFloat32 radius, dgFloat32 height,
        dgInt32 shapeID, const dgMatrix &offsetMatrix) {
	dgUnsigned32 crc;
	dgCollision *collision;
	dgBodyCollisionList::dgTreeNode *node;
	dgUnsigned32 buffer[2 * sizeof(dgMatrix) / sizeof(dgInt32)];

	radius = dgAbsf(radius);
	height = dgAbsf(height);

	memset(buffer, 0, sizeof(buffer));
	buffer[0] = m_chamferCylinderCollision;
	buffer[1] = dgCollision::Quantize(radius);
	buffer[2] = dgCollision::Quantize(height * dgFloat32(0.5f));
	buffer[3] = dgUnsigned32(shapeID);
	memcpy(&buffer[4], &offsetMatrix, sizeof(dgMatrix));
	crc = dgCollision::MakeCRC(buffer, sizeof(buffer));

	node = dgBodyCollisionList::Find(crc);
	if (!node) {
		collision = new (m_allocator) dgCollisionChamferCylinder(m_allocator, crc,
		        radius, height, offsetMatrix);
		collision->SetUserDataID(dgUnsigned32(shapeID));
		node = dgBodyCollisionList::Insert(collision, crc);
	}

	node->GetInfo()->AddRef();
	return node->GetInfo();
}

dgCollision *dgWorld::CreateConvexHull(dgInt32 count,
                                       const dgFloat32 *vertexArray, dgInt32 strideInBytes, dgFloat32 tolerance,
                                       dgInt32 shapeID, const dgMatrix &offsetMatrix) {
	dgStack<dgUnsigned32> buffer(
	    2 + 3 * count + dgInt32(sizeof(dgMatrix) / sizeof(dgInt32)));

	// create crc signature for cache lookup
	memset(&buffer[0], 0, size_t(buffer.GetSizeInBytes()));
	buffer[0] = m_convexHullCollision;
	buffer[1] = dgUnsigned32(shapeID);
	dgInt32 stride = dgInt32(strideInBytes / sizeof(dgFloat32));
	for (dgInt32 i = 0; i < count; i++) {
		buffer[2 + i * 3 + 0] = dgCollision::Quantize(vertexArray[i * stride + 0]);
		buffer[2 + i * 3 + 1] = dgCollision::Quantize(vertexArray[i * stride + 1]);
		buffer[2 + i * 3 + 2] = dgCollision::Quantize(vertexArray[i * stride + 2]);
	}
	memcpy(&buffer[2 + count * 3], &offsetMatrix, sizeof(dgMatrix));
	dgUnsigned32 crc = dgCollision::MakeCRC(&buffer[0], buffer.GetSizeInBytes());

	// find the shape in cache
	dgBodyCollisionList::dgTreeNode *node = dgBodyCollisionList::Find(crc);
	if (!node) {
		// chape not found crate a new one and add to the cache
		dgCollisionConvexHull *const collision =
		    new (m_allocator) dgCollisionConvexHull(m_allocator, crc, count,
		            strideInBytes, tolerance, vertexArray, offsetMatrix);
		if (collision->GetVertexCount()) {
			collision->SetUserDataID(dgUnsigned32(shapeID));
			node = dgBodyCollisionList::Insert(collision, crc);
		} else {
			/*
			 // hack to save the data that make convex hull fail
			 char xxx[32];
			 static int aaa  ;
			 snprintf (xxx, 32, "file1__%d.txt", aaa);
			 aaa ++;
			 FILE * file = fopen (xxx, "wb");
			 fprintf (file, "float data[][3] = {\n");
			 for (dgInt32 i = 0; i < count; i ++) {
			 fprintf (file, "{%f, %f, %f},\n", vertexArray[i * stride + 0], vertexArray[i * stride + 1], vertexArray[i * stride + 2]);
			 }
			 fprintf (file, "};\n");
			 fclose (file);
			 */
			NEWTON_ASSERT(0);
			// could not make the shape destroy the shell and return NULL
			// note this is the only newton shape that can return NULL;
			collision->Release();
			return NULL;
		}
	}

	// add reference to the shape and return the collision pointer
	node->GetInfo()->AddRef();
	return node->GetInfo();
}

dgCollision *dgWorld::CreateConvexModifier(dgCollision *convexCollision) {
	dgCollision *collision;

	collision = NULL;
	if (convexCollision->IsType(dgCollision::dgConvexCollision_RTTI)) {
		//      if (!convexCollision->IsType (dgCollision::dgCollisionCompound_RTTI) && !convexCollision->IsType (m_nullType)) {
		if (!convexCollision->IsType(dgCollision::dgCollisionNull_RTTI)) {
			collision = new (m_allocator) dgCollisionConvexModifier(
			    (dgCollisionConvex *)convexCollision, this);
		}
	}

	return collision;
}

dgCollision *dgWorld::CreateCollisionCompound(dgInt32 count,
        dgCollision *const array[]) {
	// compound collision are not cached
	return new (m_allocator) dgCollisionCompound(count,
	        (dgCollisionConvex * const*)array, this);
}

// dgCollision* dgWorld::CreateCollisionCompoundBreakable (
//	dgInt32 count,
//	dgMeshEffect* const solidArray[],
//	dgMeshEffect* const splitePlanes[],
//  dgMatrix* const matrixArray,
//	dgInt32* const idArray,
//	dgFloat32* const densities,
//	dgInt32 debriID,
//	dgCollisionCompoundBreakableCallback callback,
//	void* buildUsedData)
dgCollision *dgWorld::CreateCollisionCompoundBreakable(dgInt32 count,
        dgMeshEffect *const solidArray[], const dgInt32 *const idArray,
        const dgFloat32 *const densities, const dgInt32 *const internalFaceMaterial,
        dgInt32 debriID, dgFloat32 gap) {
	//  return new dgCollisionCompoundBreakable (count, solidArray, splitePlanes, matrixArray, idArray, densities, debriID, callback, buildUsedData, this);
	return new (m_allocator) dgCollisionCompoundBreakable(count, solidArray,
	        idArray, densities, internalFaceMaterial, debriID, gap, this);
}

dgCollision *dgWorld::CreateBVH() {
	// collision tree are not cached
	return new (m_allocator) dgCollisionBVH(m_allocator);
}

dgCollision *dgWorld::CreateStaticUserMesh(const dgVector &boxP0,
        const dgVector &boxP1, const dgUserMeshCreation &data) {
	return new (m_allocator) dgCollisionUserMesh(m_allocator, boxP0, boxP1, data);
}

dgCollision *dgWorld::CreateBVHFieldCollision(dgInt32 width, dgInt32 height,
        dgInt32 contructionMode, const dgUnsigned16 *const elevationMap,
        const dgInt8 *const atributeMap, dgFloat32 horizontalScale,
        dgFloat32 vertcalScale) {
	return new (m_allocator) dgCollisionHeightField(this, width, height,
	        contructionMode, elevationMap, atributeMap, horizontalScale, vertcalScale);
}

dgCollision *dgWorld::CreateScene() {
	return new (m_allocator) dgCollisionScene(this);
}

void dgWorld::Serialize(const dgCollision *shape, dgSerialize serialization,
                        void *const userData) const {
	dgInt32 signature[4];

	signature[0] = shape->GetCollisionPrimityType();
	signature[1] = dgInt32(shape->GetSignature());
	signature[2] = 0;
	signature[3] = 0;
	serialization(userData, signature, sizeof(signature));

	shape->Serialize(serialization, userData);

	dgInt32 end = SERIALIZE_END;
	serialization(userData, &end, sizeof(dgInt32));
}

// dgCollision* dgWorld::CreateFromSerialization (dgInt32 signature, dgCollisionID type, dgDeserialize deserialization, void* const userData)
dgCollision *dgWorld::CreateFromSerialization(dgDeserialize deserialization,
        void *const userData) {
	dgInt32 signature[4];

	deserialization(userData, signature, sizeof(signature));

	dgBodyCollisionList::dgTreeNode *node = dgBodyCollisionList::Find(
	        dgUnsigned32(signature[1]));
	dgCollision *collision = NULL;
	if (node) {
		collision = node->GetInfo();
		collision->AddRef();

	} else {
		switch (signature[0]) {
		case m_sceneCollision: {
			collision = new (m_allocator) dgCollisionScene(this, deserialization,
			        userData);
			break;
		}

		case m_heightField: {
			collision = new (m_allocator) dgCollisionHeightField(this,
			        deserialization, userData);
			break;
		}
		case m_boundingBoxHierachy: {
			collision = new (m_allocator) dgCollisionBVH(this, deserialization,
			        userData);
			break;
		}

		// if (!stricmp (type, "box"))
		case m_boxCollision: {
			collision = new (m_allocator) dgCollisionBox(this, deserialization,
			        userData);
			node = dgBodyCollisionList::Insert(collision, collision->GetSignature());
			collision->AddRef();
			break;
		}

		//} else if (!stricmp (type, "cone")) {
		case m_coneCollision: {
			collision = new (m_allocator) dgCollisionCone(this, deserialization,
			        userData);
			node = dgBodyCollisionList::Insert(collision, collision->GetSignature());
			collision->AddRef();
			break;
		}

		//} else if (!stricmp (type, "sphere_1")) {
		case m_ellipseCollision: {
			collision = new (m_allocator) dgCollisionEllipse(this, deserialization,
			        userData);
			node = dgBodyCollisionList::Insert(collision, collision->GetSignature());
			collision->AddRef();
			break;
		}

		//} else if (!stricmp (type, "sphere")) {
		case m_sphereCollision: {
			collision = new (m_allocator) dgCollisionSphere(this, deserialization,
			        userData);
			node = dgBodyCollisionList::Insert(collision, collision->GetSignature());
			collision->AddRef();
			break;
		}

		//} else if (!stricmp (type, "cylinder")) {
		case m_cylinderCollision: {
			collision = new (m_allocator) dgCollisionCylinder(this, deserialization,
			        userData);
			node = dgBodyCollisionList::Insert(collision, collision->GetSignature());
			collision->AddRef();
			break;
		}

		//} else if (!stricmp (type, "capsule")) {
		case m_capsuleCollision: {
			collision = new (m_allocator) dgCollisionCapsule(this, deserialization,
			        userData);
			node = dgBodyCollisionList::Insert(collision, collision->GetSignature());
			collision->AddRef();
			break;
		}

		//} else if (!stricmp (type, "compound")) {
		case m_compoundCollision: {
			collision = new (m_allocator) dgCollisionCompound(this, deserialization,
			        userData);
			break;
		}

		case m_compoundBreakable: {
			collision = new (m_allocator) dgCollisionCompoundBreakable(this,
			        deserialization, userData);
			break;
		}

		//} else if (!stricmp (type, "convexHull")) {
		case m_convexHullCollision: {
			collision = new (m_allocator) dgCollisionConvexHull(this, deserialization,
			        userData);
			node = dgBodyCollisionList::Insert(collision, collision->GetSignature());
			collision->AddRef();
			break;
		}

		case m_convexConvexIntance: {
			collision =
			    new (m_allocator) dgCollisionCompoundBreakable::dgCollisionConvexIntance(
			    this, deserialization, userData);
			break;
		}

		//} else if (!stricmp (type, "chamferCylinder")) {
		case m_chamferCylinderCollision: {
			collision = new (m_allocator) dgCollisionChamferCylinder(this,
			        deserialization, userData);
			node = dgBodyCollisionList::Insert(collision, collision->GetSignature());
			collision->AddRef();
			break;
		}

		case m_nullCollision: {
			collision = new (m_allocator) dgCollisionNull(this, deserialization,
			        userData);
			node = dgBodyCollisionList::Insert(collision, collision->GetSignature());
			collision->AddRef();
			break;
		}

		case m_convexCollisionModifier: {
			collision = new (m_allocator) dgCollisionConvexModifier(this,
			        deserialization, userData);
			break;
		}

		default:
			NEWTON_ASSERT(0);
		}
	}

	dgInt32 endMarker;
	do {
		deserialization(userData, &endMarker, sizeof(dgInt32));
	} while (endMarker != SERIALIZE_END);
	return collision;
}

dgContactMaterial *dgWorld::GetMaterial(dgUnsigned32 bodyGroupId0,
                                        dgUnsigned32 bodyGroupId1) const {
	if (bodyGroupId0 > bodyGroupId1) {
		Swap(bodyGroupId0, bodyGroupId1);
	}

	dgUnsigned32 key = (bodyGroupId1 << 16) + bodyGroupId0;
	dgBodyMaterialList::dgTreeNode *const node = dgBodyMaterialList::Find(key);

	return node ? &node->GetInfo() : NULL;
}

dgContactMaterial *dgWorld::GetFirstMaterial() const {
	dgBodyMaterialList::dgTreeNode *const node = dgBodyMaterialList::Minimum();
	NEWTON_ASSERT(node);
	return &node->GetInfo();
}

dgContactMaterial *dgWorld::GetNextMaterial(dgContactMaterial *material) const {
	dgBodyMaterialList::dgTreeNode *const thisNode =
	    dgBodyMaterialList::GetNodeFromInfo(*material);
	NEWTON_ASSERT(thisNode);
	dgBodyMaterialList::dgTreeNode *const node =
	    (dgBodyMaterialList::dgTreeNode *)thisNode->Next();
	if (node) {
		return &node->GetInfo();
	}

	return NULL;
}

dgUnsigned32 dgWorld::GetDefualtBodyGroupID() const {
	return m_defualtBodyGroupID;
}

dgUnsigned32 dgWorld::CreateBodyGroupID() {
	dgContactMaterial pairMaterial;

	pairMaterial.m_aabbOverlap = NULL;
	pairMaterial.m_contactPoint = NULL;
	pairMaterial.m_compoundAABBOverlap = NULL;

	dgUnsigned32 newId = m_bodyGroupID;
	m_bodyGroupID += 1;
	for (dgUnsigned32 i = 0; i < m_bodyGroupID; i++) {
		dgUnsigned32 key = (newId << 16) + i;

		dgBodyMaterialList::Insert(pairMaterial, key);
	}

	return newId;
}

void dgWorld::RemoveFromCache(dgCollision *const collision) {
	dgBodyCollisionList::dgTreeNode *const node = dgBodyCollisionList::Find(
	            collision->m_signature);
	if (node) {
		collision->m_signature = 0xffffffff;
		NEWTON_ASSERT(node->GetInfo() == collision);
		collision->Release();
		dgBodyCollisionList::Remove(node);
	}
}

void dgWorld::ReleaseCollision(dgCollision *const collision) {
	if (m_destroyCollision) {
		if (collision->GetRefCount() == 1) {
			m_destroyCollision(this, collision);
		}
	}

	dgInt32 ref = collision->Release();
	if (ref == 1) {
		dgBodyCollisionList::dgTreeNode *const node = dgBodyCollisionList::Find(
		            collision->m_signature);
		if (node) {
			NEWTON_ASSERT(node->GetInfo() == collision);
			if (m_destroyCollision) {
				m_destroyCollision(this, collision);
			}
			collision->Release();
			dgBodyCollisionList::Remove(node);
		}
	}
}

// ********************************************************************************
//
// separate collision system
//
// ********************************************************************************
dgInt32 dgWorld::ClosestPoint(const dgTriplex &point, dgCollision *const collision,
                              const dgMatrix &matrix, dgTriplex &contact, dgTriplex &normal,
                              dgInt32 threadIndex) const {
	dgTriplex contactA;
	dgMatrix pointMatrix(dgGetIdentityMatrix());

	contact = point;
	pointMatrix.m_posit.m_x = point.m_x;
	pointMatrix.m_posit.m_y = point.m_y;
	pointMatrix.m_posit.m_z = point.m_z;
	return ClosestPoint(collision, matrix, m_pointCollision, pointMatrix, contact,
	                    contactA, normal, threadIndex);
}

dgInt32 dgWorld::ClosestPoint(dgCollision *const collisionA,
                              const dgMatrix &matrixA, dgCollision *const collisionB,
                              const dgMatrix &matrixB, dgTriplex &contactA, dgTriplex &contactB,
                              dgTriplex &normalAB, dgInt32 threadIndex) const {
	dgBody collideBodyA;
	dgBody collideBodyB;

	collideBodyA.m_matrix = matrixA;
	collideBodyA.m_collision = collisionA;
	collideBodyA.m_collisionWorldMatrix = collisionA->m_offset * matrixA;

	collideBodyB.m_matrix = matrixB;
	collideBodyB.m_collision = collisionB;
	collideBodyB.m_collisionWorldMatrix = collisionB->m_offset * matrixB;

	if (collisionA->IsType(dgCollision::dgCollisionCompound_RTTI)) {
		return ClosestCompoundPoint(&collideBodyA, &collideBodyB, contactA,
		                            contactB, normalAB, threadIndex);
	} else if (collisionB->IsType(dgCollision::dgCollisionCompound_RTTI)) {
		dgInt32 flag;
		flag = ClosestCompoundPoint(&collideBodyB, &collideBodyA, contactB,
		                            contactA, normalAB, threadIndex);
		normalAB.m_x *= dgFloat32(-1.0f);
		normalAB.m_y *= dgFloat32(-1.0f);
		normalAB.m_z *= dgFloat32(-1.0f);
		return flag;

	} else if (collisionA->IsType(dgCollision::dgConvexCollision_RTTI) && collisionB->IsType(dgCollision::dgConvexCollision_RTTI)) {
		dgInt32 flag;
		dgCollisionParamProxy proxy(threadIndex);
		dgContactPoint contacts[16];

		proxy.m_referenceBody = &collideBodyA;
		proxy.m_referenceCollision = collideBodyA.m_collision;
		proxy.m_referenceMatrix = collideBodyA.m_collisionWorldMatrix;

		proxy.m_floatingBody = &collideBodyB;
		proxy.m_floatingCollision = collideBodyB.m_collision;
		proxy.m_floatingMatrix = collideBodyB.m_collisionWorldMatrix;

		proxy.m_timestep = dgFloat32(0.0f);
		proxy.m_penetrationPadding = dgFloat32(0.0f);
		proxy.m_unconditionalCast = 1;
		proxy.m_continueCollision = 0;
		proxy.m_maxContacts = 16;
		proxy.m_contacts = &contacts[0];
		flag = ClosestPoint(proxy);

		if (flag) {
			contactA.m_x = contacts[0].m_point.m_x;
			contactA.m_y = contacts[0].m_point.m_y;
			contactA.m_z = contacts[0].m_point.m_z;

			contactB.m_x = contacts[1].m_point.m_x;
			contactB.m_y = contacts[1].m_point.m_y;
			contactB.m_z = contacts[1].m_point.m_z;

			normalAB.m_x = contacts[0].m_normal.m_x;
			normalAB.m_y = contacts[0].m_normal.m_y;
			normalAB.m_z = contacts[0].m_normal.m_z;
		}
		return flag;
	}

	return 0;
}

dgInt32 dgWorld::ClosestCompoundPoint(dgBody *const compoundConvexA,
                                      dgBody *const collisionB, dgTriplex &contactA, dgTriplex &contactB,
                                      dgTriplex &normalAB, dgInt32 threadIndex) const {
	dgCollisionCompound *const collision =
	    (dgCollisionCompound *)compoundConvexA->m_collision;
	NEWTON_ASSERT(collision->IsType(dgCollision::dgCollisionCompound_RTTI));
	return collision->ClosestDitance(compoundConvexA, contactA, collisionB,
	                                 contactB, normalAB);

	/*
	 dgInt32 retFlag;
	 dgInt32 count;
	 dgMatrix* collisionMatrixArray;
	 dgCollisionConvex** collisionArray;
	 dgCollisionCompound *compoundCollision;

	 dgContactPoint contact0;
	 dgContactPoint contact1;

	 compoundCollision = (dgCollisionCompound *) compoundConvexA->m_collision;
	 count = compoundCollision->m_count;
	 collisionArray = compoundCollision->m_array;
	 collisionMatrixArray = compoundCollision->m_collisionMatrix;

	 retFlag = 0;
	 if (collisionB->m_collision->IsType (dgCollision::dgConvexCollision_RTTI)) {
	 dgFloat32 minDist2;
	 dgCollisionParamProxy proxy(threadIndex);
	 dgContactPoint contacts[16];

	 proxy.m_referenceBody = compoundConvexA;
	 proxy.m_floatingBody = collisionB;
	 proxy.m_floatingCollision = collisionB->m_collision;
	 proxy.m_floatingMatrix = collisionB->m_collisionWorldMatrix ;

	 proxy.m_timestep = dgFloat32 (0.0f);
	 proxy.m_penetrationPadding = dgFloat32 (0.0f);
	 proxy.m_unconditionalCast = 1;
	 proxy.m_continueCollision = 0;
	 proxy.m_maxContacts = 16;
	 proxy.m_contacts = &contacts[0];

	 dgMatrix saveCollMatrix (compoundConvexA->m_collisionWorldMatrix);
	 minDist2 = dgFloat32 (1.0e10f);

	 for (dgInt32 i = 0; i < count; i ++) {
	 dgInt32 flag;

	 compoundConvexA->m_collision = collisionArray[i];
	 compoundConvexA->m_collisionWorldMatrix = collisionArray[i]->m_offset * saveCollMatrix;
	 proxy.m_referenceCollision = compoundConvexA->m_collision;
	 proxy.m_referenceMatrix = compoundConvexA->m_collisionWorldMatrix;
	 flag = ClosestPoint (proxy);
	 if (flag) {
	 dgFloat32 dist2;
	 retFlag = 1;
	 dgVector err (contacts[0].m_point - contacts[1].m_point);
	 dist2 = err % err;
	 if (dist2 < minDist2) {
	 minDist2 = dist2;
	 contact0 = contacts[0];
	 contact1 = contacts[1];
	 }
	 }
	 }

	 } else {
	 dgInt32 count1;
	 dgFloat32 minDist2;
	 dgCollisionParamProxy proxy(threadIndex);
	 dgContactPoint contacts[16];
	 dgMatrix* collisionMatrixArray1;
	 dgCollisionConvex** collisionArray1;
	 dgCollisionCompound *compoundCollision1;

	 NEWTON_ASSERT (collisionB->m_collision->IsType (dgCollision::dgCollisionCompound_RTTI));

	 dgMatrix saveCollMatrix (compoundConvexA->m_collisionWorldMatrix);
	 dgMatrix saveCollMatrix1 (collisionB->m_collisionWorldMatrix);

	 compoundCollision1 = (dgCollisionCompound *) collisionB->m_collision;
	 count1 = compoundCollision1->m_count;
	 collisionArray1 = compoundCollision1->m_array;
	 collisionMatrixArray1 = compoundCollision1->m_collisionMatrix;

	 proxy.m_referenceBody = compoundConvexA;
	 proxy.m_floatingBody = collisionB;
	 proxy.m_timestep = dgFloat32 (0.0f);
	 proxy.m_penetrationPadding = dgFloat32 (0.0f);
	 proxy.m_unconditionalCast = 1;
	 proxy.m_continueCollision = 0;
	 proxy.m_maxContacts = 16;
	 proxy.m_contacts = &contacts[0];

	 minDist2 = dgFloat32 (1.0e10f);
	 for (dgInt32 i = 0; i < count; i ++) {
	 compoundConvexA->m_collision = collisionArray[i];
	 compoundConvexA->m_collisionWorldMatrix = collisionArray[i]->m_offset * saveCollMatrix;

	 proxy.m_referenceCollision = compoundConvexA->m_collision;
	 proxy.m_referenceMatrix = compoundConvexA->m_collisionWorldMatrix;
	 for (dgInt32 j = 0; j < count1; j ++) {
	 dgInt32 flag;
	 collisionB->m_collision = collisionArray1[j];
	 collisionB->m_collisionWorldMatrix = collisionArray1[j]->m_offset * saveCollMatrix1;

	 proxy.m_floatingCollision = collisionB->m_collision;
	 proxy.m_floatingMatrix = collisionB->m_collisionWorldMatrix ;
	 flag = ClosestPoint (proxy);
	 if (flag) {
	 dgFloat32 dist2;
	 retFlag = 1;
	 dgVector err (contacts[0].m_point - contacts[1].m_point);
	 dist2 = err % err;
	 if (dist2 < minDist2) {
	 minDist2 = dist2;
	 contact0 = contacts[0];
	 contact1 = contacts[1];
	 }
	 }
	 }
	 }
	 }

	 if (retFlag) {
	 contactA.m_x = contact0.m_point.m_x;
	 contactA.m_y = contact0.m_point.m_y;
	 contactA.m_z = contact0.m_point.m_z;

	 contactB.m_x = contact1.m_point.m_x;
	 contactB.m_y = contact1.m_point.m_y;
	 contactB.m_z = contact1.m_point.m_z;

	 normalAB.m_x = contact0.m_normal.m_x;
	 normalAB.m_y = contact0.m_normal.m_y;
	 normalAB.m_z = contact0.m_normal.m_z;
	 }

	 return retFlag;
	 */
}

// **********************************************************************************
//
// dynamics collision system
//
// **********************************************************************************
static inline dgInt32 CompareContact(const dgContactPoint *const contactA,
                                     const dgContactPoint *const contactB, void *dommy) {
	if (contactA->m_point[0] < contactB->m_point[0]) {
		return -1;
	} else if (contactA->m_point[0] > contactB->m_point[0]) {
		return 1;
	} else {
		return 0;
	}
}

void dgWorld::SortContacts(dgContactPoint *const contact, dgInt32 count) const {
	dgSort(contact, count, CompareContact, NULL);
}

dgInt32 dgWorld::ReduceContacts(dgInt32 count, dgContactPoint *const contact,
                                dgInt32 maxCount, dgFloat32 tol, dgInt32 arrayIsSorted) const {
	if ((count > maxCount) && (maxCount > 1)) {
		//      dgInt32 index;
		//      dgInt32 countOver;
		//      dgFloat32 window;
		//      dgFloat32 window2;
		dgUnsigned8 mask[DG_MAX_CONTATCS];

		if (!arrayIsSorted) {
			SortContacts(contact, count);
		}

		dgInt32 index = 0;
		dgFloat32 window = tol;
		dgFloat32 window2 = window * window;
		dgInt32 countOver = count - maxCount;

		NEWTON_ASSERT(countOver >= 0);
		memset(mask, 0, size_t(count));
		do {
			for (dgInt32 i = 0; (i < count) && countOver; i++) {
				if (!mask[i]) {
					dgFloat32 val = contact[i].m_point[index] + window;
					for (dgInt32 j = i + 1;
					        (j < count) && countOver && (contact[j].m_point[index] < val);
					        j++) {
						if (!mask[j]) {
							dgVector dp(contact[j].m_point - contact[i].m_point);
							dgFloat32 dist2 = dp % dp;
							if (dist2 < window2) {
								mask[j] = 1;
								countOver--;
							}
						}
					}
				}
			}
			window *= dgFloat32(2.0f);
			window2 = window * window;

		} while (countOver);

		dgInt32 j = 0;
		for (dgInt32 i = 0; i < count; i++) {
			if (!mask[i]) {
				contact[j] = contact[i];
				j++;
			}
		}
		NEWTON_ASSERT(j == maxCount);

	} else {
		maxCount = count;
	}

	return maxCount;
}

dgInt32 dgWorld::PruneContacts(dgInt32 count, dgContactPoint *const contact,
                               dgInt32 maxCount) const {
	//  dgInt32 index;
	//  dgInt32 packContacts;
	//  dgFloat32 window;
	//  dgFloat32 window2;
	dgUnsigned8 mask[DG_MAX_CONTATCS];

	dgInt32 index = 0;
	dgInt32 packContacts = 0;
	dgFloat32 window = DG_PRUNE_CONTACT_TOLERANCE;
	dgFloat32 window2 = window * window;

	memset(mask, 0, size_t(count));
	SortContacts(contact, count);

	for (dgInt32 i = 0; i < count; i++) {
		if (!mask[i]) {
			dgFloat32 val = contact[i].m_point[index] + window;
			for (dgInt32 j = i + 1; (j < count) && (contact[j].m_point[index] < val);
			        j++) {
				if (!mask[j]) {
					dgVector dp(contact[j].m_point - contact[i].m_point);
					dgFloat32 dist2 = dp % dp;
					if (dist2 < window2) {
						mask[j] = 1;
						packContacts = 1;
					}
				}
			}
		}
	}

	if (packContacts) {
		dgInt32 j = 0;
		for (dgInt32 i = 0; i < count; i++) {
			if (!mask[i]) {
				contact[j] = contact[i];
				j++;
			}
		}
		count = j;
	}

	if (count > maxCount) {
		count = ReduceContacts(count, contact, maxCount, window * dgFloat32(2.0f),
		                       1);
	}
	return count;
}

void dgWorld::ProcessCachedContacts(dgContact *const contact,
                                    const dgContactMaterial *const material, dgFloat32 timestep,
                                    dgInt32 threadIndex) const {
	NEWTON_ASSERT(contact);
	NEWTON_ASSERT(contact->m_body0);
	NEWTON_ASSERT(contact->m_body1);
	NEWTON_ASSERT(contact->m_myCacheMaterial);
	NEWTON_ASSERT(contact->m_myCacheMaterial == material);

	NEWTON_ASSERT(contact->m_body0 != contact->m_body1);
	dgList<dgContactMaterial> &list = *contact;
	contact->m_broadphaseLru = dgInt32(m_broadPhaseLru);
	contact->m_myCacheMaterial = material;

	dgList<dgContactMaterial>::dgListNode *nextContactNode;
	for (dgList<dgContactMaterial>::dgListNode *contactNode = list.GetFirst();
	        contactNode; contactNode = nextContactNode) {
		nextContactNode = contactNode->GetNext();
		dgContactMaterial &contactMaterial = contactNode->GetInfo();

		NEWTON_ASSERT(dgCheckFloat(contactMaterial.m_point.m_x));
		NEWTON_ASSERT(dgCheckFloat(contactMaterial.m_point.m_y));
		NEWTON_ASSERT(dgCheckFloat(contactMaterial.m_point.m_z));
		NEWTON_ASSERT(contactMaterial.m_body0);
		NEWTON_ASSERT(contactMaterial.m_body1);
		NEWTON_ASSERT(contactMaterial.m_collision0);
		NEWTON_ASSERT(contactMaterial.m_collision1);
		NEWTON_ASSERT(contactMaterial.m_body0 == contact->m_body0);
		NEWTON_ASSERT(contactMaterial.m_body1 == contact->m_body1);
		//      NEWTON_ASSERT (contactMaterial.m_userId != 0xffffffff);

		contactMaterial.m_softness = material->m_softness;
		contactMaterial.m_restitution = material->m_restitution;
		contactMaterial.m_staticFriction0 = material->m_staticFriction0;
		contactMaterial.m_staticFriction1 = material->m_staticFriction1;
		contactMaterial.m_dynamicFriction0 = material->m_dynamicFriction0;
		contactMaterial.m_dynamicFriction1 = material->m_dynamicFriction1;

		//      contactMaterial.m_collisionEnable = true;
		//      contactMaterial.m_friction0Enable = material->m_friction0Enable;
		//      contactMaterial.m_friction1Enable = material->m_friction1Enable;
		//      contactMaterial.m_override0Accel = false;
		//      contactMaterial.m_override1Accel = false;
		//      contactMaterial.m_overrideNormalAccel = false;
		contactMaterial.m_flags = dgContactMaterial::m_collisionEnable__ | (material->m_flags & (dgContactMaterial::m_friction0Enable__ | dgContactMaterial::m_friction1Enable__));
		contactMaterial.m_userData = material->m_userData;
	}

	if (material->m_contactPoint) {
		material->m_contactPoint(reinterpret_cast<const NewtonJoint *>(contact), timestep, threadIndex);
	}

	contact->m_maxDOF = dgUnsigned32(3 * contact->GetCount());
}

void dgWorld::ProcessTriggers(dgCollidingPairCollector::dgPair *const pair,
                              dgFloat32 timestep, dgInt32 threadIndex) {
	dgBody *const body0 = pair->m_body0;
	dgBody *const body1 = pair->m_body1;
	dgContact *contact1 = pair->m_contact;
	const dgContactMaterial *const material = pair->m_material;
	NEWTON_ASSERT(body0 != body1);

	if (!contact1) {
		dgGetUserLock();
		contact1 = new (m_allocator) dgContact(this);
		pair->m_contact = contact1;
		AttachConstraint(contact1, body0, body1);
		dgReleasedUserLock();
	} else if (contact1->GetBody0() != body0) {
		NEWTON_ASSERT(0);
		NEWTON_ASSERT(contact1->GetBody1() == body0);
		NEWTON_ASSERT(contact1->GetBody0() == body1);
		Swap(contact1->m_body0, contact1->m_body1);
		Swap(contact1->m_link0, contact1->m_link1);
	}

	dgContact *const contact = contact1;
	contact->m_myCacheMaterial = material;
	contact->m_broadphaseLru = dgInt32(m_broadPhaseLru);

	NEWTON_ASSERT(body0);
	NEWTON_ASSERT(body1);
	NEWTON_ASSERT(contact->m_body0 == body0);
	NEWTON_ASSERT(contact->m_body1 == body1);

	if (material->m_contactPoint) {
		material->m_contactPoint(reinterpret_cast<const NewtonJoint *>(contact), timestep, threadIndex);
	}
	contact->m_maxDOF = 0;
}

void dgWorld::ProcessContacts(dgCollidingPairCollector::dgPair *const pair,
                              dgFloat32 timestep, dgInt32 threadIndex) {
	dgBody *const body0 = pair->m_body0;
	dgBody *const body1 = pair->m_body1;
	dgContact *contact1 = pair->m_contact;
	const dgContactMaterial *const material = pair->m_material;
	dgContactPoint *const contactArray = pair->m_contactBuffer;
	NEWTON_ASSERT(body0 != body1);

	if (!contact1) {
		dgGetUserLock();
		contact1 = new (m_allocator) dgContact(this);
		pair->m_contact = contact1;
		AttachConstraint(contact1, body0, body1);
		dgReleasedUserLock();
	} else if (contact1->GetBody0() != body0) {
		NEWTON_ASSERT(0);
		NEWTON_ASSERT(contact1->GetBody1() == body0);
		NEWTON_ASSERT(contact1->GetBody0() == body1);
		Swap(contact1->m_body0, contact1->m_body1);
		Swap(contact1->m_link0, contact1->m_link1);
	}

	dgContact *const contact = contact1;
	dgInt32 contactCount = pair->m_contactCount;
	contact->m_myCacheMaterial = material;
	dgList<dgContactMaterial> &list = *contact;

	contact->m_broadphaseLru = dgInt32(m_broadPhaseLru);

	NEWTON_ASSERT(body0);
	NEWTON_ASSERT(body1);
	NEWTON_ASSERT(contact->m_body0 == body0);
	NEWTON_ASSERT(contact->m_body1 == body1);

	contact->m_prevPosit0 = body0->m_matrix.m_posit;
	contact->m_prevPosit1 = body1->m_matrix.m_posit;
	contact->m_prevRotation0 = body0->m_rotation;
	contact->m_prevRotation1 = body1->m_rotation;

	dgInt32 count = 0;
	dgVector cachePosition[DG_MAX_CONTATCS];
	dgList<dgContactMaterial>::dgListNode *nodes[DG_MAX_CONTATCS];
	for (dgList<dgContactMaterial>::dgListNode *contactNode = list.GetFirst();
	        contactNode; contactNode = contactNode->GetNext()) {
		nodes[count] = contactNode;
		cachePosition[count] = contactNode->GetInfo().m_point;
		count++;
	}

	const dgVector &v0 = body0->m_veloc;
	const dgVector &w0 = body0->m_omega;
	const dgMatrix &matrix0 = body0->m_matrix;

	const dgVector &v1 = body1->m_veloc;
	const dgVector &w1 = body1->m_omega;
	const dgMatrix &matrix1 = body1->m_matrix;

	dgVector controlDir0(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
	                     dgFloat32(0.0f));
	dgVector controlDir1(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
	                     dgFloat32(0.0f));
	dgVector controlNormal(contactArray[0].m_normal);
	dgVector vel0(v0 + w0 * (contactArray[0].m_point - matrix0.m_posit));
	dgVector vel1(v1 + w1 * (contactArray[0].m_point - matrix1.m_posit));
	dgVector vRel(vel1 - vel0);
	dgVector tangDir(vRel - controlNormal.Scale(vRel % controlNormal));
	dgFloat32 diff = tangDir % tangDir;

	dgInt32 staticMotion = 0;
	//  if (diff > dgFloat32 (1.0e-2f)) {
	//      staticMotion = 0;
	//  } else {
	if (diff <= dgFloat32(1.0e-2f)) {
		staticMotion = 1;
		if (dgAbsf(controlNormal.m_z) > dgFloat32(0.577f)) {
			tangDir = dgVector(-controlNormal.m_y, controlNormal.m_z, dgFloat32(0.0f),
			                   dgFloat32(0.0f));
		} else {
			tangDir = dgVector(-controlNormal.m_y, controlNormal.m_x, dgFloat32(0.0f),
			                   dgFloat32(0.0f));
		}
		controlDir0 = controlNormal * tangDir;
		NEWTON_ASSERT(controlDir0 % controlDir0 > dgFloat32(1.0e-8f));
		controlDir0 = controlDir0.Scale(dgRsqrt(controlDir0 % controlDir0));
		controlDir1 = controlNormal * controlDir0;
		NEWTON_ASSERT(
		    dgAbsf((controlDir0 * controlDir1) % controlNormal - dgFloat32(1.0f)) < dgFloat32(1.0e-3f));
	}

	// dgTrace (("contact pair %d %d\n", body0->m_uniqueID, body1->m_uniqueID));

	dgFloat32 maxImpulse = dgFloat32(-1.0f);
	dgFloat32 breakImpulse0 = dgFloat32(0.0f);
	dgFloat32 breakImpulse1 = dgFloat32(0.0f);
	for (dgInt32 i = 0; i < contactCount; i++) {
		dgList<dgContactMaterial>::dgListNode *contactNode;
		contactNode = NULL;
		dgFloat32 min = dgFloat32(1.0e20f);
		dgInt32 index = -1;
		for (dgInt32 j = 0; j < count; j++) {
			dgVector v(cachePosition[j] - contactArray[i].m_point);
			diff = v % v;
			if (diff < min) {
				min = diff;
				index = j;
				contactNode = nodes[j];
			}
		}

		if (contactNode) {
			count--;
			NEWTON_ASSERT(index != -1);
			nodes[index] = nodes[count];
			cachePosition[index] = cachePosition[count];
		} else {
			dgGetUserLock();
			contactNode = list.Append();
			dgReleasedUserLock();
		}

		dgContactMaterial *const contactMaterial = &contactNode->GetInfo();

		NEWTON_ASSERT(dgCheckFloat(contactArray[i].m_point.m_x));
		NEWTON_ASSERT(dgCheckFloat(contactArray[i].m_point.m_y));
		NEWTON_ASSERT(dgCheckFloat(contactArray[i].m_point.m_z));
		NEWTON_ASSERT(contactArray[i].m_body0);
		NEWTON_ASSERT(contactArray[i].m_body1);
		NEWTON_ASSERT(contactArray[i].m_collision0);
		NEWTON_ASSERT(contactArray[i].m_collision1);
		NEWTON_ASSERT(contactArray[i].m_body0 == body0);
		NEWTON_ASSERT(contactArray[i].m_body1 == body1);
		//      NEWTON_ASSERT (contactArray[i].m_userId != 0xffffffff);

		contactMaterial->m_point = contactArray[i].m_point;
		contactMaterial->m_normal = contactArray[i].m_normal;
		contactMaterial->m_userId = contactArray[i].m_userId;
		contactMaterial->m_penetration = contactArray[i].m_penetration;
		contactMaterial->m_body0 = contactArray[i].m_body0;
		contactMaterial->m_body1 = contactArray[i].m_body1;
		contactMaterial->m_collision0 = contactArray[i].m_collision0;
		contactMaterial->m_collision1 = contactArray[i].m_collision1;
		contactMaterial->m_softness = material->m_softness;
		contactMaterial->m_restitution = material->m_restitution;
		contactMaterial->m_staticFriction0 = material->m_staticFriction0;
		contactMaterial->m_staticFriction1 = material->m_staticFriction1;
		contactMaterial->m_dynamicFriction0 = material->m_dynamicFriction0;
		contactMaterial->m_dynamicFriction1 = material->m_dynamicFriction1;

		NEWTON_ASSERT(
		    (dgAbsf(contactMaterial->m_normal % contactMaterial->m_normal) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));

		// dgTrace (("%f\n", contactMaterial.m_penetration));
		// dgTrace (("p(%f %f %f) n(%f %f %f)\n", contactMaterial.m_point.m_x, contactMaterial.m_point.m_y, contactMaterial.m_point.m_z,
		//                                     contactMaterial.m_normal.m_x, contactMaterial.m_normal.m_y, contactMaterial.m_normal.m_z));

		//      contactMaterial.m_collisionEnable = true;
		//      contactMaterial.m_friction0Enable = material->m_friction0Enable;
		//      contactMaterial.m_friction1Enable = material->m_friction1Enable;
		//      contactMaterial.m_override0Accel = false;
		//      contactMaterial.m_override1Accel = false;
		//      contactMaterial.m_overrideNormalAccel = false;
		contactMaterial->m_flags = dgContactMaterial::m_collisionEnable__ | (material->m_flags & (dgContactMaterial::m_friction0Enable__ | dgContactMaterial::m_friction1Enable__));
		contactMaterial->m_userData = material->m_userData;

		if (staticMotion) {
			if ((contactMaterial->m_normal % controlNormal) > dgFloat32(0.9995f)) {
				contactMaterial->m_dir0 = controlDir0;
				contactMaterial->m_dir1 = controlDir1;
			} else {
				if (dgAbsf(contactMaterial->m_normal.m_z) > dgFloat32(0.577f)) {
					tangDir = dgVector(-contactMaterial->m_normal.m_y,
					                   contactMaterial->m_normal.m_z, dgFloat32(0.0f), dgFloat32(0.0f));
				} else {
					tangDir = dgVector(-contactMaterial->m_normal.m_y,
					                   contactMaterial->m_normal.m_x, dgFloat32(0.0f), dgFloat32(0.0f));
				}
				contactMaterial->m_dir0 = contactMaterial->m_normal * tangDir;
				NEWTON_ASSERT(
				    contactMaterial->m_dir0 % contactMaterial->m_dir0 > dgFloat32(1.0e-8f));
				contactMaterial->m_dir0 = contactMaterial->m_dir0.Scale(
				                              dgRsqrt(contactMaterial->m_dir0 % contactMaterial->m_dir0));
				contactMaterial->m_dir1 = contactMaterial->m_normal * contactMaterial->m_dir0;
				NEWTON_ASSERT(
				    dgAbsf((contactMaterial->m_dir0 * contactMaterial->m_dir1) % contactMaterial->m_normal - dgFloat32(1.0f)) < dgFloat32(1.0e-3f));
			}

		} else {
			dgVector velocity0(v0 + w0 * (contactMaterial->m_point - matrix0.m_posit));
			dgVector velocity1(v1 + w1 * (contactMaterial->m_point - matrix1.m_posit));
			dgVector vRelative(velocity1 - velocity0);

			dgFloat32 impulse = vRelative % contactMaterial->m_normal;
			if (dgAbsf(impulse) > maxImpulse) {
				maxImpulse = dgAbsf(impulse);
				breakImpulse0 = contactMaterial->m_collision0->GetBreakImpulse();
				breakImpulse1 = contactMaterial->m_collision1->GetBreakImpulse();
			}

			dgVector tangDirection(vRelative - contactMaterial->m_normal.Scale(impulse));
			diff = tangDirection % tangDirection;

			if (diff > dgFloat32(1.0e-2f)) {
				contactMaterial->m_dir0 = tangDirection.Scale(dgRsqrt(diff));
			} else {
				if (dgAbsf(contactMaterial->m_normal.m_z) > dgFloat32(0.577f)) {
					tangDirection = dgVector(-contactMaterial->m_normal.m_y,
					                         contactMaterial->m_normal.m_z, dgFloat32(0.0f), dgFloat32(0.0f));
				} else {
					tangDirection = dgVector(-contactMaterial->m_normal.m_y,
					                         contactMaterial->m_normal.m_x, dgFloat32(0.0f), dgFloat32(0.0f));
				}
				contactMaterial->m_dir0 = contactMaterial->m_normal * tangDirection;
				NEWTON_ASSERT(
				    contactMaterial->m_dir0 % contactMaterial->m_dir0 > dgFloat32(1.0e-8f));
				contactMaterial->m_dir0 = contactMaterial->m_dir0.Scale(
				                              dgRsqrt(contactMaterial->m_dir0 % contactMaterial->m_dir0));
			}
			contactMaterial->m_dir1 = contactMaterial->m_normal * contactMaterial->m_dir0;
			NEWTON_ASSERT(
			    dgAbsf((contactMaterial->m_dir0 * contactMaterial->m_dir1) % contactMaterial->m_normal - dgFloat32(1.0f)) < dgFloat32(1.0e-3f));
		}
		contactMaterial->m_normal.m_w = dgFloat32(0.0f);
		contactMaterial->m_dir0.m_w = dgFloat32(0.0f);
		contactMaterial->m_dir1.m_w = dgFloat32(0.0f);
	}

	for (dgInt32 i = 0; i < count; i++) {
		dgGetUserLock();
		list.Remove(nodes[i]);
		dgReleasedUserLock();
	}

	if (material->m_contactPoint) {
		material->m_contactPoint(reinterpret_cast<const NewtonJoint *>(contact), timestep, threadIndex);
	}

	if (maxImpulse > dgFloat32(1.0f)) {
		maxImpulse /= (body0->m_invMass.m_w + body1->m_invMass.m_w);
		if ((maxImpulse > breakImpulse0) || (maxImpulse > breakImpulse1)) {
			dgGetUserLock();
			if (maxImpulse > breakImpulse0) {
				AddToBreakQueue(contact, body0, maxImpulse);
			}
			if (maxImpulse > breakImpulse1) {
				AddToBreakQueue(contact, body1, maxImpulse);
			}
			dgReleasedUserLock();
		}
	}

	contact->m_maxDOF = dgUnsigned32(3 * contact->GetCount());
}

dgInt32 dgWorld::ValidateContactCache(dgBody *const convexBody,
                                      dgBody *const otherBody, dgContact *const contact) const {
	NEWTON_ASSERT(contact && (contact->GetId() == dgContactConstraintId));
	NEWTON_ASSERT(
	    (contact->GetBody0() == otherBody) || (contact->GetBody1() == otherBody));
	NEWTON_ASSERT(
	    (contact->GetBody0() == convexBody) || (contact->GetBody1() == convexBody));

	dgInt32 contactCount = 0;

#ifdef DG_USE_CACHE_CONTACTS
#define DG_CACHE_DIST_TOL dgFloat32(1.0e-3f)

	dgBody *const body0 = contact->GetBody0();
	dgVector error0(contact->m_prevPosit0 - body0->m_matrix.m_posit);
	dgFloat32 err2 = error0 % error0;
	if (err2 < (DG_CACHE_DIST_TOL * DG_CACHE_DIST_TOL)) {
		dgBody *const body1 = contact->GetBody1();
		dgVector error1(contact->m_prevPosit1 - body1->m_matrix.m_posit);
		err2 = error1 % error1;
		if (err2 < (DG_CACHE_DIST_TOL * DG_CACHE_DIST_TOL)) {
			dgQuaternion errorRot0(contact->m_prevRotation0 - body0->m_rotation);
			err2 = errorRot0.DotProduct(errorRot0);
			if (err2 < (DG_CACHE_DIST_TOL * DG_CACHE_DIST_TOL)) {
				dgQuaternion errorRot1(contact->m_prevRotation1 - body1->m_rotation);
				err2 = errorRot1.DotProduct(errorRot1);
				if (err2 < (DG_CACHE_DIST_TOL * DG_CACHE_DIST_TOL)) {
					dgMatrix matrix0(
					    dgMatrix(contact->m_prevRotation0, contact->m_prevPosit0).Inverse() * body0->m_matrix);
					dgMatrix matrix1(
					    dgMatrix(contact->m_prevRotation1, contact->m_prevPosit1).Inverse() * body1->m_matrix);

					dgList<dgContactMaterial> &list = *contact;
					for (dgList<dgContactMaterial>::dgListNode *ptr = list.GetFirst();
					        ptr; ptr = ptr->GetNext()) {
						dgContactMaterial &contactMaterial = ptr->GetInfo();
						dgVector p0(matrix0.TransformVector(contactMaterial.m_point));
						dgVector p1(matrix1.TransformVector(contactMaterial.m_point));
						dgVector error(p1 - p0);

						err2 = error % error;
						if (err2 > (DG_CACHE_DIST_TOL * DG_CACHE_DIST_TOL)) {
							contactCount = 0;
							break;
						}
						contactCount++;
					}
				}
			}
		}
	}
#endif

	return contactCount;
}

void dgWorld::CompoundContactsSimd(dgCollidingPairCollector::dgPair *const pair,
                                   dgCollisionParamProxy &proxy) const {
	dgInt32 contactCount;
	dgBody *otherBody;
	dgBody *compoundBody;
	dgContact *constraint;
	dgContactPoint *const contacts = pair->m_contactBuffer;

	constraint = pair->m_contact;
	compoundBody = pair->m_body0;
	otherBody = pair->m_body1;

	pair->m_contactCount = 0;
	proxy.m_contacts = contacts;

	pair->m_isTrigger = 0;
	proxy.m_isTriggerVolume = 0;
	proxy.m_inTriggerVolume = 0;

	if (constraint) {
		contactCount = ValidateContactCache(compoundBody, otherBody, constraint);
		if (contactCount) {
			pair->m_contactCount = 0;
			pair->m_contactBuffer = NULL;
			return;
		}
	}

	contactCount =
	    ((dgCollisionCompound *)compoundBody->m_collision)->CalculateContacts(pair, proxy, 1);

	if (contactCount) {
		// prune close contacts
		pair->m_contactCount = dgInt16(PruneContacts(contactCount, contacts));
	}
}

void dgWorld::CompoundContacts(dgCollidingPairCollector::dgPair *const pair,
                               dgCollisionParamProxy &proxy) const {
	dgInt32 contactCount;
	dgBody *otherBody;
	dgBody *compoundBody;
	dgContact *constraint;
	dgContactPoint *const contacts = pair->m_contactBuffer;

	constraint = pair->m_contact;
	compoundBody = pair->m_body0;
	otherBody = pair->m_body1;

	pair->m_contactCount = 0;
	proxy.m_contacts = contacts;

	pair->m_isTrigger = 0;
	proxy.m_isTriggerVolume = 0;
	proxy.m_inTriggerVolume = 0;

	if (constraint) {
		contactCount = ValidateContactCache(compoundBody, otherBody, constraint);
		if (contactCount) {
			pair->m_contactCount = 0;
			pair->m_contactBuffer = NULL;
			return;
		}
	}

	contactCount =
	    ((dgCollisionCompound *)compoundBody->m_collision)->CalculateContacts(pair, proxy, 0);

	if (contactCount) {
		// prune close contacts
		pair->m_contactCount = dgInt16(PruneContacts(contactCount, contacts));
	}
}

void dgWorld::ConvexContactsSimd(dgCollidingPairCollector::dgPair *const pair,
                                 dgCollisionParamProxy &proxy) const {
	dgInt32 contactCount;
	dgBody *otherBody;
	dgBody *convexBody;
	dgContact *constraint;

	contactCount = 0;
	constraint = pair->m_contact;

	convexBody = pair->m_body0;
	otherBody = pair->m_body1;
	if (constraint) {
		contactCount = ValidateContactCache(convexBody, otherBody, constraint);
		if (contactCount) {
			pair->m_isTrigger = 0;
			pair->m_contactCount = 0;
			pair->m_contactBuffer = NULL;
			return;
		}
	}

	//  proxy.m_maxContacts = DG_MAX_CONTATCS;
	proxy.m_contacts = pair->m_contactBuffer;

	if (otherBody->m_collision->IsType(dgCollision::dgConvexCollision_RTTI)) {
		if (convexBody->m_invMass.m_w <= dgFloat32(1.0e-6f)) {
			Swap(convexBody, otherBody);
			pair->m_body0 = convexBody;
			pair->m_body1 = otherBody;
		}

		NEWTON_ASSERT(
		    convexBody->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));
		NEWTON_ASSERT(
		    otherBody->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));
		NEWTON_ASSERT(
		    pair->m_body0->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));
		NEWTON_ASSERT(
		    pair->m_body1->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));

		proxy.m_referenceBody = convexBody;
		proxy.m_floatingBody = otherBody;
		proxy.m_referenceCollision = convexBody->m_collision;
		proxy.m_floatingCollision = otherBody->m_collision;
		proxy.m_referenceMatrix = convexBody->m_collisionWorldMatrix;
		proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;
		// contactCount = CalculateConvexToConvexContacts (proxy);
		pair->m_contactCount = dgInt16(CalculateConvexToConvexContactsSimd(proxy));
		pair->m_isTrigger = proxy.m_inTriggerVolume;

	} else {
		NEWTON_ASSERT(
		    pair->m_body0->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));
		NEWTON_ASSERT(
		    convexBody->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));

		proxy.m_referenceBody = convexBody;
		proxy.m_floatingBody = otherBody;
		proxy.m_referenceCollision = convexBody->m_collision;
		proxy.m_floatingCollision = otherBody->m_collision;
		proxy.m_referenceMatrix = convexBody->m_collisionWorldMatrix;
		proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;
		// contactCount = CalculateConvexToNonConvexContacts (proxy);
		pair->m_contactCount = dgInt16(
		                           CalculateConvexToNonConvexContactsSimd(proxy));
		pair->m_isTrigger = proxy.m_inTriggerVolume;
	}
}

void dgWorld::ConvexContacts(dgCollidingPairCollector::dgPair *const pair,
                             dgCollisionParamProxy &proxy) const {
	dgInt32 contactCount = 0;
	dgContact *const constraint = pair->m_contact;

	dgBody *convexBody = pair->m_body0;
	dgBody *otherBody = pair->m_body1;
	if (constraint) {
		contactCount = ValidateContactCache(convexBody, otherBody, constraint);
		if (contactCount) {
			pair->m_isTrigger = 0;
			pair->m_contactCount = 0;
			pair->m_contactBuffer = NULL;
			return;
		}
	}

	//  proxy.m_maxContacts = DG_MAX_CONTATCS;
	proxy.m_contacts = pair->m_contactBuffer;

	if (otherBody->m_collision->IsType(dgCollision::dgConvexCollision_RTTI)) {
		if (convexBody->m_invMass.m_w <= dgFloat32(1.0e-6f)) {
			Swap(convexBody, otherBody);
			pair->m_body0 = convexBody;
			pair->m_body1 = otherBody;
		}

		NEWTON_ASSERT(
		    convexBody->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));
		NEWTON_ASSERT(
		    otherBody->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));
		NEWTON_ASSERT(
		    pair->m_body0->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));
		NEWTON_ASSERT(
		    pair->m_body1->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));

		proxy.m_referenceBody = convexBody;
		proxy.m_floatingBody = otherBody;
		proxy.m_referenceCollision = convexBody->m_collision;
		proxy.m_floatingCollision = otherBody->m_collision;
		proxy.m_referenceMatrix = convexBody->m_collisionWorldMatrix;
		proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;
		pair->m_contactCount = dgInt16(CalculateConvexToConvexContacts(proxy));
		pair->m_isTrigger = proxy.m_inTriggerVolume;

	} else {
		NEWTON_ASSERT(
		    pair->m_body0->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));
		NEWTON_ASSERT(
		    convexBody->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));

		proxy.m_referenceBody = convexBody;
		proxy.m_floatingBody = otherBody;
		proxy.m_referenceCollision = convexBody->m_collision;
		proxy.m_floatingCollision = otherBody->m_collision;
		proxy.m_referenceMatrix = convexBody->m_collisionWorldMatrix;
		proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;
		pair->m_contactCount = dgInt16(CalculateConvexToNonConvexContacts(proxy));
		pair->m_isTrigger = proxy.m_inTriggerVolume;
	}
}

void dgWorld::SceneContactsSimd(const dgCollisionScene::dgProxy &sceneProxy,
                                dgCollidingPairCollector::dgPair *const pair,
                                dgCollisionParamProxy &proxy) const {
	NEWTON_ASSERT(
	    pair->m_body1->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI));
	if (sceneProxy.m_shape->IsType(dgCollision::dgConvexCollision_RTTI)) {
		proxy.m_floatingCollision = sceneProxy.m_shape;
		proxy.m_floatingMatrix = sceneProxy.m_matrix;
		proxy.m_maxContacts =
		    ((DG_MAX_CONTATCS - pair->m_contactCount) > 16) ? 16 : DG_MAX_CONTATCS - pair->m_contactCount;

		proxy.m_contacts = &pair->m_contactBuffer[pair->m_contactCount];
		pair->m_contactCount = pair->m_contactCount + dgInt16(CalculateConvexToConvexContactsSimd(proxy));
		if (pair->m_contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
			pair->m_contactCount = dgInt16(
			                           ReduceContacts(pair->m_contactCount, pair->m_contactBuffer,
			                                   DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE));
		}
		//      pair->m_isTrigger = proxy.m_inTriggerVolume;

	} else {
		proxy.m_floatingCollision = sceneProxy.m_shape;
		proxy.m_floatingMatrix = sceneProxy.m_matrix;
		proxy.m_maxContacts =
		    ((DG_MAX_CONTATCS - pair->m_contactCount) > 32) ? 32 : DG_MAX_CONTATCS - pair->m_contactCount;

		proxy.m_contacts = &pair->m_contactBuffer[pair->m_contactCount];
		pair->m_contactCount = pair->m_contactCount + dgInt16(CalculateConvexToNonConvexContactsSimd(proxy));
		if (pair->m_contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
			pair->m_contactCount = dgInt16(
			                           ReduceContacts(pair->m_contactCount, pair->m_contactBuffer,
			                                   DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE));
		}
		//      pair->m_isTrigger = proxy.m_inTriggerVolume;
	}
}

void dgWorld::SceneContacts(const dgCollisionScene::dgProxy &sceneProxy,
                            dgCollidingPairCollector::dgPair *const pair,
                            dgCollisionParamProxy &proxy) const {
	NEWTON_ASSERT(
	    pair->m_body1->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI));
	if (sceneProxy.m_shape->IsType(dgCollision::dgConvexCollision_RTTI)) {
		proxy.m_floatingCollision = sceneProxy.m_shape;
		proxy.m_floatingMatrix = sceneProxy.m_matrix;
		proxy.m_maxContacts =
		    ((DG_MAX_CONTATCS - pair->m_contactCount) > 16) ? 16 : DG_MAX_CONTATCS - pair->m_contactCount;

		proxy.m_contacts = &pair->m_contactBuffer[pair->m_contactCount];
		pair->m_contactCount = pair->m_contactCount + dgInt16(CalculateConvexToConvexContacts(proxy));
		if (pair->m_contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
			pair->m_contactCount = dgInt16(
			                           ReduceContacts(pair->m_contactCount, pair->m_contactBuffer,
			                                   DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE));
		}
		//      pair->m_isTrigger = proxy.m_inTriggerVolume;

	} else {
		proxy.m_floatingCollision = sceneProxy.m_shape;
		proxy.m_floatingMatrix = sceneProxy.m_matrix;
		proxy.m_maxContacts =
		    ((DG_MAX_CONTATCS - pair->m_contactCount) > 32) ? 32 : DG_MAX_CONTATCS - pair->m_contactCount;

		proxy.m_contacts = &pair->m_contactBuffer[pair->m_contactCount];
		pair->m_contactCount = pair->m_contactCount + dgInt16(CalculateConvexToNonConvexContacts(proxy));
		if (pair->m_contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
			pair->m_contactCount = dgInt16(
			                           ReduceContacts(pair->m_contactCount, pair->m_contactBuffer,
			                                   DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE));
		}
		//      pair->m_isTrigger = proxy.m_inTriggerVolume;
	}
}

void dgWorld::SceneContacts(dgCollidingPairCollector::dgPair *const pair,
                            dgCollisionParamProxy &proxy) const {
	dgContact *const constraint = pair->m_contact;

	pair->m_isTrigger = 0;
	pair->m_contactCount = 0;

	proxy.m_isTriggerVolume = 0;
	proxy.m_inTriggerVolume = 0;
	//  proxy.m_contacts = contacts;

	//  NEWTON_ASSERT (pair->m_body0->m_invMass.m_w != dgFloat32 (0.0f));
	//  NEWTON_ASSERT (pair->m_body1->m_invMass.m_w == dgFloat32 (0.0f));
	if (constraint) {
		dgInt32 contactCount = ValidateContactCache(pair->m_body0, pair->m_body1,
		                       constraint);
		if (contactCount) {
			pair->m_contactCount = 0;
			pair->m_contactBuffer = NULL;
			return;
		}
	}

	dgCollisionScene *const scene =
	    (dgCollisionScene *)pair->m_body1->GetCollision();
	NEWTON_ASSERT(scene->IsType(dgCollision::dgCollisionScene_RTTI));
	if (pair->m_body0->GetCollision()->IsType(
	            dgCollision::dgConvexCollision_RTTI)) {
		proxy.m_referenceBody = pair->m_body0;
		proxy.m_floatingBody = pair->m_body1;
		proxy.m_referenceCollision = pair->m_body0->m_collision;
		proxy.m_floatingCollision = NULL;
		proxy.m_referenceMatrix = pair->m_body0->m_collisionWorldMatrix;

		scene->CollidePair(pair, proxy);
		if (pair->m_contactCount) {
			// prune close contacts
			pair->m_contactCount = dgInt16(
			                           PruneContacts(pair->m_contactCount, pair->m_contactBuffer));
		}

	} else {
		NEWTON_ASSERT(0);
	}
}

void dgWorld::SceneContactsSimd(dgCollidingPairCollector::dgPair *const pair,
                                dgCollisionParamProxy &proxy) const {
	dgContact *const constraint = pair->m_contact;

	pair->m_isTrigger = 0;
	pair->m_contactCount = 0;

	proxy.m_isTriggerVolume = 0;
	proxy.m_inTriggerVolume = 0;
	//  proxy.m_contacts = contacts;

	//  NEWTON_ASSERT (pair->m_body0->m_invMass.m_w != dgFloat32 (0.0f));
	//  NEWTON_ASSERT (pair->m_body1->m_invMass.m_w == dgFloat32 (0.0f));
	if (constraint) {
		dgInt32 contactCount = ValidateContactCache(pair->m_body0, pair->m_body1,
		                       constraint);
		if (contactCount) {
			pair->m_contactCount = 0;
			pair->m_contactBuffer = NULL;
			return;
		}
	}

	dgCollisionScene *const scene =
	    (dgCollisionScene *)pair->m_body1->GetCollision();
	NEWTON_ASSERT(scene->IsType(dgCollision::dgCollisionScene_RTTI));
	if (pair->m_body0->GetCollision()->IsType(
	            dgCollision::dgConvexCollision_RTTI)) {
		proxy.m_referenceBody = pair->m_body0;
		proxy.m_floatingBody = pair->m_body1;
		proxy.m_referenceCollision = pair->m_body0->m_collision;
		proxy.m_floatingCollision = NULL;
		proxy.m_referenceMatrix = pair->m_body0->m_collisionWorldMatrix;

		scene->CollidePairSimd(pair, proxy);

		if (pair->m_contactCount) {
			// prune close contacts
			pair->m_contactCount = dgInt16(
			                           PruneContacts(pair->m_contactCount, pair->m_contactBuffer));
		}

	} else {
		NEWTON_ASSERT(0);
	}
}

void dgWorld::CalculateContactsSimd(
    dgCollidingPairCollector::dgPair *const pair, dgFloat32 timestep,
    dgInt32 threadIndex) {
	dgBody *body0;
	dgBody *body1;
	const dgContactMaterial *material;

	dgCollisionParamProxy proxy(threadIndex);

	body0 = pair->m_body0;
	body1 = pair->m_body1;

	material = pair->m_material;
	proxy.m_timestep = timestep;
	proxy.m_unconditionalCast = 0;
	proxy.m_maxContacts = DG_MAX_CONTATCS;
	proxy.m_penetrationPadding = material->m_penetrationPadding;
	//  proxy.m_continueCollision = material->m_collisionContinueCollisionEnable & (body0->m_continueCollisionMode | body1->m_continueCollisionMode);
	proxy.m_continueCollision = dgInt32(
	                                ((material->m_flags & dgContactMaterial::m_collisionContinueCollisionEnable__) ? 1 : 0) & (body0->m_continueCollisionMode | body1->m_continueCollisionMode));
	proxy.m_isTriggerVolume = body0->m_collision->IsTriggerVolume() || body1->m_collision->IsTriggerVolume();

	if (body0->m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		Swap(pair->m_body0, pair->m_body1);
		SceneContactsSimd(pair, proxy);
	} else if (body1->m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		SceneContactsSimd(pair, proxy);
	} else if (body0->m_collision->IsType(dgCollision::dgCollisionCompound_RTTI)) {
		CompoundContactsSimd(pair, proxy);
	} else if (body1->m_collision->IsType(dgCollision::dgCollisionCompound_RTTI)) {
		Swap(pair->m_body0, pair->m_body1);
		CompoundContactsSimd(pair, proxy);
	} else if (body0->m_collision->IsType(dgCollision::dgConvexCollision_RTTI)) {
		ConvexContactsSimd(pair, proxy);
	} else if (body1->m_collision->IsType(dgCollision::dgConvexCollision_RTTI)) {
		Swap(pair->m_body0, pair->m_body1);
		ConvexContactsSimd(pair, proxy);
	}
}

void dgWorld::CalculateContacts(dgCollidingPairCollector::dgPair *const pair,
                                dgFloat32 timestep, dgInt32 threadIndex) {
	dgCollisionParamProxy proxy(threadIndex);

	dgBody *const body0 = pair->m_body0;
	dgBody *const body1 = pair->m_body1;

	const dgContactMaterial *const material = pair->m_material;
	proxy.m_timestep = timestep;
	proxy.m_unconditionalCast = 0;
	proxy.m_maxContacts = DG_MAX_CONTATCS;
	proxy.m_penetrationPadding = material->m_penetrationPadding;
	proxy.m_isTriggerVolume = body0->m_collision->IsTriggerVolume() || body1->m_collision->IsTriggerVolume();
	//  proxy.m_continueCollision = material->m_collisionContinueCollisionEnable & (body0->m_continueCollisionMode | body1->m_continueCollisionMode);
	proxy.m_continueCollision = dgInt32(
	                                ((material->m_flags & dgContactMaterial::m_collisionContinueCollisionEnable__) ? 1 : 0) & (body0->m_continueCollisionMode | body1->m_continueCollisionMode));

	if (body0->m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		Swap(pair->m_body0, pair->m_body1);
		SceneContacts(pair, proxy);
	} else if (body1->m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		SceneContacts(pair, proxy);
	} else if (body0->m_collision->IsType(dgCollision::dgCollisionCompound_RTTI)) {
		CompoundContacts(pair, proxy);
	} else if (body1->m_collision->IsType(dgCollision::dgCollisionCompound_RTTI)) {
		Swap(pair->m_body0, pair->m_body1);
		CompoundContacts(pair, proxy);
	} else if (body0->m_collision->IsType(dgCollision::dgConvexCollision_RTTI)) {
		ConvexContacts(pair, proxy);
	} else if (body1->m_collision->IsType(dgCollision::dgConvexCollision_RTTI)) {
		Swap(pair->m_body0, pair->m_body1);
		ConvexContacts(pair, proxy);
	}
}

// ***************************************************************************
//
// ***************************************************************************
dgInt32 dgWorld::CollideContinueSimd(dgCollision *const collisionA,
                                     const dgMatrix &matrixA, const dgVector &velocA, const dgVector &omegaA,
                                     dgCollision *const collisionB, const dgMatrix &matrixB,
                                     const dgVector &velocB, const dgVector &omegaB, dgFloat32 &retTimeStep,
                                     dgTriplex *const points, dgTriplex *const normals,
                                     dgFloat32 *const penetration, dgInt32 maxSize, dgInt32 threadIndex) {
	dgBody collideBodyA;
	dgBody collideBodyB;
	dgContactPoint contacts[DG_MAX_CONTATCS];

	dgInt32 count = 0;
	retTimeStep = dgFloat32(1.0e10f);
	maxSize = GetMin(dgInt32(DG_MAX_CONTATCS), maxSize);

	collideBodyA.m_world = this;
	collideBodyA.SetContinuesCollisionMode(true);
	collideBodyA.m_matrix = matrixA;
	collideBodyA.m_collision = collisionA;
	collideBodyA.m_masterNode = NULL;
	collideBodyA.m_collisionCell.m_cell = NULL;
	collideBodyA.m_collisionWorldMatrix = collisionA->m_offset * matrixA;
	collideBodyA.m_veloc = dgVector(velocA[0], velocA[1], velocA[2],
	                                dgFloat32(0.0f));
	collideBodyA.m_omega = dgVector(omegaA[0], omegaA[1], omegaA[2],
	                                dgFloat32(0.0f));
	collideBodyA.m_accel = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
	                                dgFloat32(0.0f), dgFloat32(0.0f));
	collideBodyA.m_alpha = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
	                                dgFloat32(0.0f), dgFloat32(0.0f));
	collideBodyA.m_invMass = dgVector(dgFloat32(1.0f), dgFloat32(1.0f),
	                                  dgFloat32(1.0f), dgFloat32(1.0f));
	collideBodyA.UpdateCollisionMatrixSimd(dgFloat32(1.0f), 0);

	collideBodyB.m_world = this;
	collideBodyB.SetContinuesCollisionMode(true);
	collideBodyB.m_matrix = matrixB;
	collideBodyB.m_collision = collisionB;
	collideBodyB.m_masterNode = NULL;
	collideBodyB.m_collisionCell.m_cell = NULL;
	collideBodyB.m_collisionWorldMatrix = collisionB->m_offset * matrixB;
	collideBodyB.m_veloc = dgVector(velocB[0], velocB[1], velocB[2],
	                                dgFloat32(0.0f));
	collideBodyB.m_omega = dgVector(omegaB[0], omegaB[1], omegaB[2],
	                                dgFloat32(0.0f));
	collideBodyB.m_accel = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
	                                dgFloat32(0.0f), dgFloat32(0.0f));
	collideBodyB.m_alpha = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
	                                dgFloat32(0.0f), dgFloat32(0.0f));
	collideBodyB.m_invMass = dgVector(dgFloat32(1.0f), dgFloat32(1.0f),
	                                  dgFloat32(1.0f), dgFloat32(1.0f));
	collideBodyB.UpdateCollisionMatrixSimd(dgFloat32(1.0f), 0);

	dgCollisionParamProxy proxy(threadIndex);
	proxy.m_timestep = dgFloat32(1.0f);
	proxy.m_unconditionalCast = 1;
	proxy.m_penetrationPadding = 0.0f;
	proxy.m_continueCollision = 1;
	//  proxy.m_maxContacts = DG_MAX_CONTATCS;
	proxy.m_maxContacts = maxSize;
	proxy.m_isTriggerVolume = 0;

	dgCollidingPairCollector::dgPair pair;
	pair.m_body0 = &collideBodyA;
	pair.m_body1 = &collideBodyB;
	pair.m_contact = NULL;
	pair.m_material = NULL;
	pair.m_contactCount = 0;
	pair.m_contactBuffer = contacts;

	dgFloat32 swapContactScale = dgFloat32(1.0f);
	if (collideBodyA.m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		swapContactScale = dgFloat32(-1.0f);
		Swap(pair.m_body0, pair.m_body1);
		SceneContactsSimd(&pair, proxy);
	} else if (collideBodyB.m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		SceneContactsSimd(&pair, proxy);

	} else if (collideBodyA.m_collision->IsType(
	               dgCollision::dgCollisionCompound_RTTI)) {
		CompoundContactsSimd(&pair, proxy);
	} else if (collideBodyB.m_collision->IsType(
	               dgCollision::dgCollisionCompound_RTTI)) {
		swapContactScale = dgFloat32(-1.0f);
		Swap(pair.m_body0, pair.m_body1);
		CompoundContactsSimd(&pair, proxy);

	} else if (collideBodyA.m_collision->IsType(
	               dgCollision::dgConvexCollision_RTTI)) {
		ConvexContactsSimd(&pair, proxy);
	} else if (collideBodyB.m_collision->IsType(
	               dgCollision::dgConvexCollision_RTTI)) {
		swapContactScale = dgFloat32(-1.0f);
		Swap(pair.m_body0, pair.m_body1);
		ConvexContactsSimd(&pair, proxy);
	}
	count = pair.m_contactCount;

	if (proxy.m_timestep < dgFloat32(1.0f)) {
		retTimeStep = proxy.m_timestep;
	}

	if (count) {
		retTimeStep = proxy.m_timestep;
		if (count > maxSize) {
			count = PruneContacts(count, contacts, maxSize);
		}

		for (dgInt32 i = 0; i < count; i++) {
			points[i].m_x = contacts[i].m_point.m_x;
			points[i].m_y = contacts[i].m_point.m_y;
			points[i].m_z = contacts[i].m_point.m_z;
			normals[i].m_x = contacts[i].m_normal.m_x * swapContactScale;
			normals[i].m_y = contacts[i].m_normal.m_y * swapContactScale;
			normals[i].m_z = contacts[i].m_normal.m_z * swapContactScale;
			penetration[i] = contacts[i].m_penetration;
		}
	}
	return count;
}

dgInt32 dgWorld::CollideContinue(dgCollision *const collisionA,
                                 const dgMatrix &matrixA, const dgVector &velocA, const dgVector &omegaA,
                                 dgCollision *const collisionB, const dgMatrix &matrixB,
                                 const dgVector &velocB, const dgVector &omegaB, dgFloat32 &retTimeStep,
                                 dgTriplex *const points, dgTriplex *const normals,
                                 dgFloat32 *const penetration, dgInt32 maxSize, dgInt32 threadIndex) {
	dgBody collideBodyA;
	dgBody collideBodyB;
	dgContactPoint contacts[DG_MAX_CONTATCS];

	// dgTrace (("%x -> p(%f, %f, %f) v(%f, %f, %f)\n", collisionB, matrixA.m_posit.m_x, matrixA.m_posit.m_y, matrixA.m_posit.m_z, velocA.m_x, velocA.m_y, velocA.m_z));

	dgInt32 count = 0;
	retTimeStep = dgFloat32(1.0e10f);
	maxSize = GetMin(dgInt32(DG_MAX_CONTATCS), maxSize);

	collideBodyA.m_world = this;
	collideBodyA.SetContinuesCollisionMode(true);
	collideBodyA.m_matrix = matrixA;
	collideBodyA.m_collision = collisionA;
	collideBodyA.m_masterNode = NULL;
	collideBodyA.m_collisionCell.m_cell = NULL;
	collideBodyA.m_collisionWorldMatrix = collisionA->m_offset * matrixA;
	collideBodyA.m_veloc = dgVector(velocA[0], velocA[1], velocA[2],
	                                dgFloat32(0.0f));
	collideBodyA.m_omega = dgVector(omegaA[0], omegaA[1], omegaA[2],
	                                dgFloat32(0.0f));
	collideBodyA.m_accel = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
	                                dgFloat32(0.0f), dgFloat32(0.0f));
	collideBodyA.m_alpha = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
	                                dgFloat32(0.0f), dgFloat32(0.0f));
	collideBodyA.m_invMass = dgVector(dgFloat32(1.0f), dgFloat32(1.0f),
	                                  dgFloat32(1.0f), dgFloat32(1.0f));
	collideBodyA.UpdateCollisionMatrix(dgFloat32(1.0f), 0);

	collideBodyB.m_world = this;
	collideBodyB.SetContinuesCollisionMode(true);
	collideBodyB.m_matrix = matrixB;
	collideBodyB.m_collision = collisionB;
	collideBodyB.m_masterNode = NULL;
	collideBodyB.m_collisionCell.m_cell = NULL;
	collideBodyB.m_collisionWorldMatrix = collisionB->m_offset * matrixB;
	collideBodyB.m_veloc = dgVector(velocB[0], velocB[1], velocB[2],
	                                dgFloat32(0.0f));
	collideBodyB.m_omega = dgVector(omegaB[0], omegaB[1], omegaB[2],
	                                dgFloat32(0.0f));
	collideBodyB.m_accel = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
	                                dgFloat32(0.0f), dgFloat32(0.0f));
	collideBodyB.m_alpha = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
	                                dgFloat32(0.0f), dgFloat32(0.0f));
	collideBodyB.m_invMass = dgVector(dgFloat32(1.0f), dgFloat32(1.0f),
	                                  dgFloat32(1.0f), dgFloat32(1.0f));
	collideBodyB.UpdateCollisionMatrix(dgFloat32(1.0f), 0);

	dgCollisionParamProxy proxy(threadIndex);
	proxy.m_timestep = dgFloat32(1.0f);
	proxy.m_unconditionalCast = 1;
	proxy.m_penetrationPadding = 0.0f;
	proxy.m_continueCollision = 1;
	//  proxy.m_maxContacts = DG_MAX_CONTATCS;
	proxy.m_maxContacts = maxSize;
	proxy.m_isTriggerVolume = 0;

	dgCollidingPairCollector::dgPair pair;
	pair.m_body0 = &collideBodyA;
	pair.m_body1 = &collideBodyB;
	pair.m_contact = NULL;
	pair.m_material = NULL;
	pair.m_contactCount = 0;
	pair.m_contactBuffer = contacts;

	dgFloat32 swapContactScale = dgFloat32(1.0f);
	if (collideBodyA.m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		swapContactScale = dgFloat32(-1.0f);
		Swap(pair.m_body0, pair.m_body1);
		SceneContactsSimd(&pair, proxy);
	} else if (collideBodyB.m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		SceneContactsSimd(&pair, proxy);

	} else if (collideBodyA.m_collision->IsType(
	               dgCollision::dgCollisionCompound_RTTI)) {
		CompoundContacts(&pair, proxy);
	} else if (collideBodyB.m_collision->IsType(
	               dgCollision::dgCollisionCompound_RTTI)) {
		swapContactScale = dgFloat32(-1.0f);
		Swap(pair.m_body0, pair.m_body1);
		CompoundContacts(&pair, proxy);

	} else if (collideBodyA.m_collision->IsType(
	               dgCollision::dgConvexCollision_RTTI)) {
		ConvexContacts(&pair, proxy);
	} else if (collideBodyB.m_collision->IsType(
	               dgCollision::dgConvexCollision_RTTI)) {
		swapContactScale = dgFloat32(-1.0f);
		Swap(pair.m_body0, pair.m_body1);
		ConvexContacts(&pair, proxy);
	}
	count = pair.m_contactCount;

	if (proxy.m_timestep < dgFloat32(1.0f)) {
		retTimeStep = proxy.m_timestep;
	}

	if (count) {

		if (count > maxSize) {
			count = PruneContacts(count, contacts, maxSize);
		}

		for (dgInt32 i = 0; i < count; i++) {
			points[i].m_x = contacts[i].m_point.m_x;
			points[i].m_y = contacts[i].m_point.m_y;
			points[i].m_z = contacts[i].m_point.m_z;
			normals[i].m_x = contacts[i].m_normal.m_x * swapContactScale;
			normals[i].m_y = contacts[i].m_normal.m_y * swapContactScale;
			normals[i].m_z = contacts[i].m_normal.m_z * swapContactScale;
			penetration[i] = contacts[i].m_penetration;
		}
	}

	return count;
}

dgInt32 dgWorld::Collide(dgCollision *const collisionA, const dgMatrix &matrixA,
                         dgCollision *const collisionB, const dgMatrix &matrixB,
                         dgTriplex *const points, dgTriplex *const normals,
                         dgFloat32 *const penetration, dgInt32 maxSize, dgInt32 threadIndex) {
	dgBody collideBodyA;
	dgBody collideBodyB;
	dgContactPoint contacts[DG_MAX_CONTATCS];

	dgInt32 count = 0;
	maxSize = GetMin(dgInt32(DG_MAX_CONTATCS), maxSize);

	collideBodyA.m_world = this;
	collideBodyA.m_masterNode = NULL;
	collideBodyA.m_collisionCell.m_cell = NULL;
	collideBodyA.SetContinuesCollisionMode(false);
	collideBodyA.m_matrix = matrixA;
	collideBodyA.m_collision = collisionA;
	//  collideBodyA.m_collisionWorldMatrix = collisionA->m_offset * matrixA;
	collideBodyA.UpdateCollisionMatrix(dgFloat32(0.0f), 0);

	collideBodyB.m_world = this;
	collideBodyB.m_masterNode = NULL;
	collideBodyB.m_collisionCell.m_cell = NULL;
	collideBodyB.SetContinuesCollisionMode(false);
	collideBodyB.m_matrix = matrixB;
	collideBodyB.m_collision = collisionB;
	//  collideBodyB.m_collisionWorldMatrix = collisionB->m_offset * matrixB;
	collideBodyB.UpdateCollisionMatrix(dgFloat32(0.0f), 0);

	//  isTriggerA = collisionA->IsTriggerVolume();
	//  isTriggerB = collisionB->IsTriggerVolume();

	dgCollisionParamProxy proxy(threadIndex);
	proxy.m_timestep = dgFloat32(0.0f);
	proxy.m_unconditionalCast = 1;
	proxy.m_penetrationPadding = 0.0f;
	proxy.m_continueCollision = 0;
	proxy.m_maxContacts = maxSize;
	proxy.m_isTriggerVolume = 0;

	dgCollidingPairCollector::dgPair pair;
	pair.m_body0 = &collideBodyA;
	pair.m_body1 = &collideBodyB;
	pair.m_contact = NULL;
	pair.m_material = NULL;
	pair.m_contactCount = 0;
	pair.m_contactBuffer = contacts;

	dgFloat32 swapContactScale = dgFloat32(1.0f);
	if (collideBodyA.m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		swapContactScale = dgFloat32(-1.0f);
		Swap(pair.m_body0, pair.m_body1);
		SceneContacts(&pair, proxy);
	} else if (collideBodyB.m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
		SceneContacts(&pair, proxy);

	} else if (collideBodyA.m_collision->IsType(
	               dgCollision::dgCollisionCompound_RTTI)) {
		CompoundContacts(&pair, proxy);
	} else if (collideBodyB.m_collision->IsType(
	               dgCollision::dgCollisionCompound_RTTI)) {
		swapContactScale = dgFloat32(-1.0f);
		Swap(pair.m_body0, pair.m_body1);
		CompoundContacts(&pair, proxy);

	} else if (collideBodyA.m_collision->IsType(
	               dgCollision::dgConvexCollision_RTTI)) {
		ConvexContacts(&pair, proxy);
	} else if (collideBodyB.m_collision->IsType(
	               dgCollision::dgConvexCollision_RTTI)) {
		swapContactScale = dgFloat32(-1.0f);
		Swap(pair.m_body0, pair.m_body1);
		ConvexContacts(&pair, proxy);
	}

	count = pair.m_contactCount;
	if (count > maxSize) {
		count = ReduceContacts(count, contacts, maxSize,
		                       DG_REDUCE_CONTACT_TOLERANCE);
	}

	for (dgInt32 i = 0; i < count; i++) {
		points[i].m_x = contacts[i].m_point.m_x;
		points[i].m_y = contacts[i].m_point.m_y;
		points[i].m_z = contacts[i].m_point.m_z;
		normals[i].m_x = contacts[i].m_normal.m_x * swapContactScale;
		normals[i].m_y = contacts[i].m_normal.m_y * swapContactScale;
		normals[i].m_z = contacts[i].m_normal.m_z * swapContactScale;
		penetration[i] = contacts[i].m_penetration;
	}

	return count;
}