/* 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 "dgWorld.h"
#include "dgCollisionBVH.h"
#include "dgCollisionConvex.h"
#include "dgCollisionEllipse.h"
#include "dgCollisionCompound.h"
#include "dgCollisionHeightField.h"
#include "dgCollisionConvexModifier.h"

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




dgCollisionCompound::OOBBTestData::OOBBTestData (const dgMatrix& matrix)
	:m_matrix (matrix)
{
	for (dgInt32 i = 0; i < 3; i ++) {
		m_absMatrix[i][3] = dgFloat32 (0.0f);
		dgVector dir(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
		dir[i] = dgFloat32 (1.0f);
		for (dgInt32 j = 0; j < 3; j ++) {
			m_absMatrix[i][j] = dgAbsf (m_matrix[i][j]);
			dgVector axis (dir * m_matrix[j]);
			m_crossAxis[i][j] = axis;
			m_crossAxisAbs[i][j] = dgVector (dgAbsf (axis.m_x), dgAbsf (axis.m_y), dgAbsf (axis.m_z), dgFloat32 (0.0f));
			m_crossAxisDotAbs[i][j] = dgVector (dgAbsf (axis % matrix[0]), dgAbsf (axis % matrix[1]), dgAbsf (axis % matrix[2]), dgFloat32 (0.0f));
		}
	}
	m_absMatrix[3][3] = dgFloat32 (1.0f);
}





dgCollisionCompound::OOBBTestData::OOBBTestData (const dgMatrix& matrix, const dgVector& p0, const dgVector& p1)
	:m_matrix (matrix), m_localP0(p0), m_localP1(p1)
{
	m_size = (m_localP1 - m_localP0).Scale (dgFloat32 (0.5f));
	m_origin = (m_localP1 + m_localP0).Scale (dgFloat32 (0.5f));

	for (dgInt32 i = 0; i < 3; i ++) {
		m_absMatrix[i][3] = dgFloat32 (0.0f);
		dgVector dir(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
		dir[i] = dgFloat32 (1.0f);
		for (dgInt32 j = 0; j < 3; j ++) {
			m_absMatrix[i][j] = dgAbsf (m_matrix[i][j]);
			m_crossAxis[i][j] = dir * m_matrix[j];
		}
	}
	m_absMatrix[3][3] = dgFloat32 (1.0f);

	dgVector size (m_absMatrix.RotateVector(m_size));
	dgVector origin (m_matrix.TransformVector (m_origin));
	m_aabbP0 = origin - size;
	m_aabbP1 = origin + size;

	for (dgInt32 i = 0; i < 3; i ++) {
		for (dgInt32 j = 0; j < 3; j ++) {
			dgFloat32 d;
			dgFloat32 c;
			dgVector& axis = m_crossAxis[i][j];
			d = m_size.m_x * dgAbsf (axis % m_matrix[0]) + m_size.m_y * dgAbsf (axis % m_matrix[1]) + m_size.m_z * dgAbsf (axis % m_matrix[2]) + dgFloat32 (1.0e-3f); 
			c = origin % axis;

			m_extends[i][j] = dgVector (c - d, c + d, dgFloat32 (0.0f), dgFloat32 (0.0f));

			_ASSERTE (m_extends[i][j].m_x <= m_extends[i][j].m_y);
			m_crossAxisAbs[i][j] = dgVector (dgAbsf (axis.m_x), dgAbsf (axis.m_y), dgAbsf (axis.m_z), dgFloat32 (0.0f));
		}
	}
}

dgCollisionCompound::dgNodeBase::dgNodeBase () 
{
	m_id = -1;
	m_shape = NULL; 
	m_left = NULL; 
	m_right = NULL;
}


dgCollisionCompound::dgNodeBase::dgNodeBase (dgCollisionConvex* const shape, dgInt32 id)
{
	m_id = id;
	m_left = NULL;
	m_right = NULL;
	m_parent = NULL;
	m_shape = shape;
	m_shape->AddRef();
	m_type = m_leaf;
	m_shape->CalcAABB(shape->GetOffsetMatrix (), m_p0, m_p1);
	m_p0.m_w = 0.0f;
	m_p1.m_w = 0.0f;
	m_size = (m_p1 - m_p0).Scale (dgFloat32 (0.5f));
	m_origin = (m_p1 + m_p0).Scale (dgFloat32 (0.5f));

	dgVector size1(m_size.m_y, m_size.m_z, m_size.m_x, dgFloat32 (0.0f));
	m_area = m_size % size1;
}

dgCollisionCompound::dgNodeBase::dgNodeBase (dgNodeBase* const left, dgNodeBase* const right, dgInt32 id)
{
	m_id = id;
	m_left = left;
	m_right = right;
	m_parent = NULL;
	m_type = m_node;
	m_shape = NULL;

	m_p0.m_x = GetMin (left->m_p0.m_x, right->m_p0.m_x);
	m_p0.m_y = GetMin (left->m_p0.m_y, right->m_p0.m_y);
	m_p0.m_z = GetMin (left->m_p0.m_z, right->m_p0.m_z);
	m_p1.m_x = GetMax (left->m_p1.m_x, right->m_p1.m_x);
	m_p1.m_y = GetMax (left->m_p1.m_y, right->m_p1.m_y);
	m_p1.m_z = GetMax (left->m_p1.m_z, right->m_p1.m_z);
	m_p0.m_w = 0.0f;
	m_p1.m_w = 0.0f;

	m_size = (m_p1 - m_p0).Scale (dgFloat32 (0.5f));
	m_origin = (m_p1 + m_p0).Scale (dgFloat32 (0.5f));

	dgVector size1(m_size.m_y, m_size.m_z, m_size.m_x, dgFloat32 (0.0f));
	m_area = m_size % size1;
}

dgCollisionCompound::dgNodeBase::~dgNodeBase()
{
	if (m_shape) {
		m_shape->Release ();
	}
	if (m_left) {
		delete m_left;
	}
	if (m_right) {
		delete m_right;
	}
}


bool dgCollisionCompound::dgNodeBase::BoxTest (const OOBBTestData& data, const dgNodeBase* const otherNode) const
{
	dgVector otherOrigin (data.m_matrix.TransformVector(otherNode->m_origin));
	dgVector otherSize (data.m_absMatrix.RotateVector(otherNode->m_size));
	dgVector otherP0 (otherOrigin - otherSize);
	dgVector otherP1 (otherOrigin + otherSize);
	if (dgOverlapTest (m_p0, m_p1, otherP0, otherP1)) {

		dgVector origin (data.m_matrix.UntransformVector(m_origin));
		dgVector size (data.m_absMatrix.UnrotateVector(m_size));
		dgVector p0 (origin - size);
		dgVector p1 (origin + size);
		if (dgOverlapTest (p0, p1, otherNode->m_p0, otherNode->m_p1)) {
			for (dgInt32 i = 0; i < 3; i ++) {
				for (dgInt32 j = 0; j < 3; j ++) {
					dgFloat32 x0;
					dgFloat32 x1;
					dgFloat32 z0;
					dgFloat32 z1;
					dgFloat32 d;
					dgFloat32 c;

					const dgVector& axis = data.m_crossAxis[i][j];

					const dgVector& axisAbs = data.m_crossAxisAbs[i][j];
					d = m_size.m_x * axisAbs.m_x + m_size.m_y * axisAbs.m_y + m_size.m_z * axisAbs.m_z + dgFloat32 (1.0e-3f); 
					c = m_origin % axis;
					x0 = c - d;
					x1 = c + d;
					_ASSERTE (x0 <= x1);

					const dgVector& axisDotAbs = data.m_crossAxisDotAbs[i][j]; 
					d = otherNode->m_size.m_x * axisDotAbs.m_x + otherNode->m_size.m_y * axisDotAbs.m_y + otherNode->m_size.m_z * axisDotAbs.m_z + dgFloat32 (1.0e-3f); 
					c = otherOrigin % axis;
					z0 = c - d;
					z1 = c + d;
					_ASSERTE (z0 <= z1);

					if ((x1 < z0) || (x0 > z1)) {
						return false;
					}
				}
			}

			return true;
		}
	}
	return false;
}


bool dgCollisionCompound::dgNodeBase::BoxTest (const OOBBTestData& data) const
{
	if (dgOverlapTest (data.m_aabbP0, data.m_aabbP1, m_p0, m_p1)) {

		dgVector origin (data.m_matrix.UntransformVector(m_origin));
		dgVector size (data.m_absMatrix.UnrotateVector(m_size));
		dgVector p0 (origin - size);
		dgVector p1 (origin + size);

		if (dgOverlapTest (p0, p1, data.m_localP0, data.m_localP1)) {
			for (dgInt32 i = 0; i < 3; i ++) {
				for (dgInt32 j = 0; j < 3; j ++) {
					dgFloat32 x0;
					dgFloat32 x1;

					dgFloat32 d;
					dgFloat32 c;
					const dgVector& axis = data.m_crossAxisAbs[i][j];
					d = m_size.m_x * axis.m_x + m_size.m_y * axis.m_y + m_size.m_z * axis.m_z + dgFloat32 (1.0e-3f); 
					c = m_origin % data.m_crossAxis[i][j];

					x0 = c - d;
					x1 = c + d;
					_ASSERTE (x0 <= x1);
					const dgVector& extend = data.m_extends[i][j];
					if ((x1 < extend.m_x) || (x0 > extend.m_y)) {
						return false;
					}
				}
			}
			return true;
		}
	}

	return false;
}

dgFloat32 dgCollisionCompound::dgNodeBase::BoxClosestDistance (const dgVector* const points, dgInt32 count) const
{
	dgVector box[8];
	box[0] = dgVector (m_p0.m_x, m_p0.m_y, m_p0.m_z, dgFloat32 (0.0f));
	box[1] = dgVector (m_p0.m_x, m_p0.m_y, m_p1.m_z, dgFloat32 (0.0f));
	box[2] = dgVector (m_p0.m_x, m_p1.m_y, m_p0.m_z, dgFloat32 (0.0f));
	box[3] = dgVector (m_p0.m_x, m_p1.m_y, m_p1.m_z, dgFloat32 (0.0f));
	box[4] = dgVector (m_p1.m_x, m_p0.m_y, m_p0.m_z, dgFloat32 (0.0f));
	box[5] = dgVector (m_p1.m_x, m_p0.m_y, m_p1.m_z, dgFloat32 (0.0f));
	box[6] = dgVector (m_p1.m_x, m_p1.m_y, m_p0.m_z, dgFloat32 (0.0f));
	box[7] = dgVector (m_p1.m_x, m_p1.m_y, m_p1.m_z, dgFloat32 (0.0f));

	dgFloat32 dist = dgFloat32 (1.0e10f);
	for (dgInt32 i = 0; i < count; i ++) {
		for (dgInt32 j = 0; j < 8; j ++) {
			dgVector dp (points[i] - box[j]) ;
			dgFloat32 dist1 = dp % dp;
			if (dist1 < dist) {
				dist = dist1;
			}
		}
	}

	return dist;
}


dgCollisionCompound::dgCollisionCompound(dgWorld* world)
	:dgCollision (world->GetAllocator(), 0, dgGetIdentityMatrix(), m_compoundCollision) 
{
	m_world = world;
	m_root = NULL;
	m_count = 0;
}


dgCollisionCompound::dgCollisionCompound(dgInt32 count, dgCollisionConvex* const shapeArray[], dgWorld* world)
	:dgCollision (world->GetAllocator(), 0, dgGetIdentityMatrix(), m_compoundCollision) 
{
	m_world = world;
	m_root = NULL;
	if (count) {
		m_root = BuildTree(count, shapeArray);
	}

	Init (count, shapeArray);
}

dgCollisionCompound::dgCollisionCompound (const dgCollisionCompound& source)
	:dgCollision (source.GetAllocator(), 0, dgGetIdentityMatrix(), m_compoundCollision) 
{
	int stack;
	dgNodeBase* pool[DG_COMPOUND_STACK_DEPTH];
	dgNodeBase** parent[DG_COMPOUND_STACK_DEPTH];

	m_root = NULL;
	m_world = source.m_world;

	parent[0] = &m_root;
	pool[0] = source.m_root;

	stack = 1;
	while (stack) {
		dgNodeBase* node;
		dgNodeBase** parentNode;

		stack --;
		node = pool [stack];
		parentNode = parent[stack];

		if (node->m_type == m_leaf) {
			*parentNode = new (m_allocator) dgNodeBase (node->m_shape, node->m_id);
		} else {
			dgNodeBase* newNode;
			newNode = new (m_allocator) dgNodeBase (*node);
			if (!m_root) {
				m_root = newNode;
			}
			*parentNode = newNode;

			pool[stack] = node->m_left;
			parent[stack] = &newNode->m_left;
			stack ++;

			pool[stack] = node->m_right;
			parent[stack] = &newNode->m_right;
			stack ++;
		}
	}

	Init (source.m_count, source.m_array);
	m_preCollisionFilter = source.m_preCollisionFilter;
}

dgCollisionCompound::dgCollisionCompound (dgWorld* const world, dgDeserialize deserialization, void* const userData)
	:dgCollision (world, deserialization, userData)
{
	dgInt32 stack;
	dgInt32 count;
	dgInt32 data[4];
	dgNodeBase* pool[DG_COMPOUND_STACK_DEPTH];
//	dgNodeBase* parentPool[DG_COMPOUND_STACK_DEPTH];

	deserialization (userData, data, sizeof (data));

	count = data[0];
	m_world = world;

	dgStack<dgCollisionConvex*> array(data[0]);
	for (dgInt32 i = 0; i < count; i ++) {
		dgCollision* collision;
		collision = world->CreateFromSerialization (deserialization, userData); 
		array[i] = (dgCollisionConvex*)collision;
	}

	struct Data: public dgNodeBase
	{
		Data()
		{
		}
		~Data()
		{
			memset (this, 0, sizeof (dgNodeBase));
		}

		dgInt8 m_padding[128];
	};

	m_root = NULL;
	Data nodeInfo;
	for (dgInt32 i = 0; i < 2 * count - 1; i ++) {
		dgNodeBase* newNode;
		deserialization (userData, &nodeInfo.m_p0, sizeof (dgNodeBase));

		if (nodeInfo.m_type == m_leaf) {
			dgNodeBase* cell;
			cell = new (m_allocator) dgNodeBase (nodeInfo);
			cell->m_shape = array[nodeInfo.m_id];
			cell->m_shape->AddRef();
			newNode = cell;
		} else {
			dgNodeBase* node;
			node = new (m_allocator) dgNodeBase ((dgNodeBase&)nodeInfo);
			node->m_left = NULL;
			node->m_right = NULL;
			newNode = node;
		}
		if (!m_root) {
			m_root = newNode;
		} else {
			stack = 1;
			pool[0] = m_root;
			while (stack) {
				dgNodeBase* node;

				stack --;
				node = pool[stack];


	//			if (node->m_id == dgInt32 (newNode->m_parent)) {
				if (((dgNodeBase*) dgInt64(node->m_id)) == newNode->m_parent) {
					if (node->m_left == NULL) {
						node->m_left = newNode;
					} else {
						_ASSERTE (!node->m_right);
						node->m_right = newNode;
					}
					break;
				}

				if (node->m_type == m_node) {
					if (node->m_left) {
						pool[stack] = node->m_left;
						stack ++;
					}
					if (node->m_right) {
						pool[stack] = node->m_right;
						stack ++;
					}
				}
			}
		}
	}


	Init (count, &array[0]);
	for (dgInt32 i = 0; i < count; i ++) {
		world->ReleaseCollision(array[i]);
	}
}



dgCollisionCompound::~dgCollisionCompound()
{
	if (m_root) {
		delete m_root;
	}

	for (dgInt32 i = 0; i < m_count; i ++) {
		m_world->ReleaseCollision(m_array[i]);
	}
	m_allocator->Free(m_array);
}



void dgCollisionCompound::Init (dgInt32 count, dgCollisionConvex* const shapeArray[])
{
	m_count = count;
	m_rtti |= dgCollisionCompound_RTTI;
	m_preCollisionFilter = NULL;

	m_array = (dgCollisionConvex**) m_allocator->Malloc (m_count * dgInt32 (sizeof (dgCollisionConvex*)));
	for (dgInt32 i = 0; i < m_count; i ++) {
		m_array[i] = shapeArray[i];
		m_array[i]->AddRef();
	}

	m_boxMinRadius = GetMin(m_root->m_size.m_x, m_root->m_size.m_y, m_root->m_size.m_z);
	m_boxMaxRadius = dgSqrt (m_root->m_size % m_root->m_size);

	LinkParentNodes();
}

void dgCollisionCompound::LinkParentNodes()
{
	dgInt32 stack;
	dgNodeBase* pool[DG_COMPOUND_STACK_DEPTH];
	dgNodeBase* parentPool[DG_COMPOUND_STACK_DEPTH];

	pool[0] = m_root;
	parentPool[0] = NULL;
	stack = 1;
	while (stack) {
		dgNodeBase* node;
		dgNodeBase* parent;

		stack --;
		node = pool[stack];
		parent = parentPool[stack];
		node ->m_parent = parent;


		if (node->m_type == m_node) {
			parentPool[stack] = node;
			pool[stack] = node->m_right;
			stack ++;

			parentPool[stack] = node;
			pool[stack] = node->m_left;
			stack ++;
		} 
	}
}

void dgCollisionCompound::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__)
{
	_ASSERTE (0);
}

dgInt32 dgCollisionCompound::CalculateSignature () const
{
	_ASSERTE (0);
	return 0;
}



void dgCollisionCompound::CalcAABB (const dgMatrix &matrix, dgVector& p0, dgVector& p1) const
{
	dgVector origin (matrix.TransformVector(m_root->m_origin));
	dgVector size (m_root->m_size.m_x * dgAbsf(matrix[0][0]) + m_root->m_size.m_y * dgAbsf(matrix[1][0]) + m_root->m_size.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,  
				   m_root->m_size.m_x * dgAbsf(matrix[0][1]) + m_root->m_size.m_y * dgAbsf(matrix[1][1]) + m_root->m_size.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,  
		           m_root->m_size.m_x * dgAbsf(matrix[0][2]) + m_root->m_size.m_y * dgAbsf(matrix[1][2]) + m_root->m_size.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
				   dgFloat32 (0.0f));

	p0 = origin - size;
	p1 = origin + size;
}


void dgCollisionCompound::CalcAABBSimd (const dgMatrix &matrix, dgVector& p0, dgVector& p1) const
{

	dgVector origin (matrix.TransformVector(m_root->m_origin));
	dgVector size (m_root->m_size.m_x * dgAbsf(matrix[0][0]) + m_root->m_size.m_y * dgAbsf(matrix[1][0]) + m_root->m_size.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,  
				   m_root->m_size.m_x * dgAbsf(matrix[0][1]) + m_root->m_size.m_y * dgAbsf(matrix[1][1]) + m_root->m_size.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,  
				   m_root->m_size.m_x * dgAbsf(matrix[0][2]) + m_root->m_size.m_y * dgAbsf(matrix[1][2]) + m_root->m_size.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
				   dgFloat32 (0.0f));

	p0 = origin - size;
	p1 = origin + size;
}

//void dgCollisionCompound::dgMatrix& matrix, DebugCollisionMeshCallback callback) const
void dgCollisionCompound::DebugCollision (const dgMatrix& matrix, OnDebugCollisionMeshCallback callback, void* const userData) const
{
	for (dgInt32 i = 0; i < m_count; i ++) {
		m_array[i]->DebugCollision (matrix, callback, userData);
	}
}


dgFloat32 dgCollisionCompound::RayCast (const dgVector& localP0, const dgVector& localP1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
	const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];

	if (!m_root) {
		return dgFloat32 (1.2f);
	}

	dgInt32 stack = 1;
	stackPool[0] = m_root;
	dgFloat32 maxParam = dgFloat32 (1.2f);

	FastRayTest ray (localP0, localP1);
	while (stack) {
		stack --;
		const dgNodeBase* const me = stackPool[stack];

		if (me && ray.BoxTest (me->m_p0, me->m_p1)) {
			if (me->m_type == m_leaf) {
				dgFloat32 param;
				dgContactPoint tmpContactOut;
				dgCollisionConvex* const shape = me->m_shape;

				dgVector p0 (shape->m_offset.UntransformVector (localP0));
				dgVector p1 (shape->m_offset.UntransformVector (localP1));
				//param = shape->RayCast (p0, p1, tmpContactOut, NULL, NULL, NULL);
				param = shape->RayCast (p0, p1, tmpContactOut, preFilter, body, userData);
				if (param < maxParam) {
					maxParam = param;
					contactOut.m_normal = shape->m_offset.RotateVector (tmpContactOut.m_normal);;
					contactOut.m_userId = tmpContactOut.m_userId;
					ray.Reset (maxParam) ;
				}

			} else {
				_ASSERTE (me->m_type == m_node);
				stackPool[stack] = me->m_left;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));
				
				stackPool[stack] = me->m_right;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));
			}
		}
	}
	return maxParam;
}

dgFloat32 dgCollisionCompound::RayCastSimd (const dgVector& localP0, const dgVector& localP1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
	const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];

	if (!m_root) {
		return dgFloat32 (1.2f);
	}

	dgInt32 stack = 1;
	stackPool[0] = m_root;
	dgFloat32 maxParam = dgFloat32 (1.2f);

	FastRayTest ray (localP0, localP1);
	while (stack) {
		stack --;
		const dgNodeBase* const me = stackPool[stack];

		if (me && ray.BoxTestSimd (me->m_p0, me->m_p1)) {
			if (me->m_type == m_leaf) {
				dgContactPoint tmpContactOut;
				dgCollisionConvex* const shape = me->m_shape;

				dgVector p0 (shape->m_offset.UntransformVector (localP0));
				dgVector p1 (shape->m_offset.UntransformVector (localP1));
				//param = shape->RayCastSimd (p0, p1, tmpContactOut, NULL, NULL, NULL);
				dgFloat32 param = shape->RayCastSimd (p0, p1, tmpContactOut, preFilter, body, userData);
				if (param < maxParam) {
					maxParam = param;
					contactOut.m_normal = shape->m_offset.RotateVector (tmpContactOut.m_normal);
					contactOut.m_userId = tmpContactOut.m_userId;
					ray.Reset (maxParam) ;
				}

			} else {
				_ASSERTE (me->m_type == m_node);
				stackPool[stack] = me->m_left;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

				stackPool[stack] = me->m_right;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));
			}
		}
	}
	return maxParam;
}


dgFloat32 dgCollisionCompound::GetVolume () const
{
	dgFloat32 volume = dgFloat32 (0.0f);
	for (dgInt32 i = 0; i < m_count; i ++) {
		volume += m_array[i]->GetVolume();
    }
	return volume;
}

dgFloat32 dgCollisionCompound::GetBoxMinRadius () const
{
	return m_boxMinRadius;
}

dgFloat32 dgCollisionCompound::GetBoxMaxRadius () const
{
	return m_boxMaxRadius;
}



dgVector dgCollisionCompound::CalculateVolumeIntegral (const dgMatrix& globalMatrix, GetBuoyancyPlane bouyancyPlane, void* const context) const
{
	dgInt32 i;
	dgFloat32 scale;

	dgVector totalVolume (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	for (i = 0; i < m_count; i ++) {
		dgMatrix matrix (m_array[i]->m_offset * globalMatrix);
//		dgVector vol (m_array[i]->CalculateVolumeIntegral (m_collisionMatrix[i], bouyancyPlane, context));
		dgVector vol (m_array[i]->CalculateVolumeIntegral (matrix, bouyancyPlane, context));
		totalVolume.m_x += vol.m_x * vol.m_w;
		totalVolume.m_y += vol.m_y * vol.m_w;
		totalVolume.m_z += vol.m_z * vol.m_w;
		totalVolume.m_w	+= vol.m_w;
    }

	scale = dgFloat32 (1.0f) / (totalVolume.m_w + dgFloat32 (1.0e-6f));
	totalVolume.m_x *= scale;
	totalVolume.m_y *= scale;
	totalVolume.m_z *= scale;
	return totalVolume;

}


void dgCollisionCompound::CalculateInertia (dgVector& inertia, dgVector& origin) const
{
	dgInt32 i;
	dgCollisionConvex* collision;

	dgFloat32 invVolume;
	dgFloat32 totalVolume;
	dgVector tmpOrigin;
	dgVector tmpInertia;
	dgVector tmpCrossInertia;
	dgVector totalOrigin (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	dgVector totalInertia(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	dgVector totalCrossInertia (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));

	#define DG_MIN_SIDE		dgFloat32 (1.0e-2f) 
	#define DG_MIN_VOLUME	(DG_MIN_SIDE * DG_MIN_SIDE * DG_MIN_SIDE) 


	totalVolume = dgFloat32 (0.0f);
	for (i = 0; i < m_count; i ++) {
		collision = m_array[i];
		totalVolume += collision->CalculateMassProperties (tmpInertia, tmpCrossInertia, tmpOrigin);
		totalOrigin += tmpOrigin;
		totalInertia += tmpInertia;
		totalCrossInertia += tmpCrossInertia;
	}


	invVolume = dgFloat32 (1.0f) / GetMax (totalVolume, DG_MIN_VOLUME);

	origin = totalOrigin.Scale (invVolume);
	totalInertia = totalInertia.Scale(invVolume);
	totalCrossInertia = totalCrossInertia.Scale(invVolume);

	inertia.m_x = totalInertia.m_x - (origin.m_y * origin.m_y + origin.m_z * origin.m_z);
	inertia.m_y = totalInertia.m_y - (origin.m_z * origin.m_z + origin.m_x * origin.m_x);
	inertia.m_z = totalInertia.m_z - (origin.m_x * origin.m_x + origin.m_y * origin.m_y);
//	crossInertia.m_x += totalCrossInertia.m_x + origin.m_y * origin.m_z;
//	crossInertia.m_y += totalCrossInertia.m_x + origin.m_z * origin.m_x;
//	crossInertia.m_z += totalCrossInertia.m_x + origin.m_x * origin.m_y;

	_ASSERTE (inertia[0] > 0.0f);
	_ASSERTE (inertia[1] > 0.0f);
	_ASSERTE (inertia[2] > 0.0f);
}


void dgCollisionCompound::AddCollision (dgCollisionConvex* part)
{
_ASSERTE (0);
/*
	dgInt32 i;
	dgInt8 *ptr;
	dgCollisionConvex** array;
	dgMatrix* collisionMatrix;
	AABB* aabb;

	_ASSERTE (0);
	if (m_count >= m_maxCount) {
		m_maxCount = m_maxCount * 2;

		ptr = (dgInt8*) m_allocator->Malloc (m_maxCount * (sizeof (dgMatrix) + sizeof (dgCollisionConvex*) + sizeof(AABB)));

		collisionMatrix = (dgMatrix*) ptr;
		aabb = (AABB*) &ptr [m_maxCount * sizeof (dgMatrix)];
		array = (dgCollisionConvex**) &ptr [m_maxCount * (sizeof (dgMatrix) + sizeof(AABB))];
		for (i = 0; i < m_count; i ++) {
			array[i] = m_array[i];
			collisionMatrix[i] = m_collisionMatrix[i];
			aabb[i] = m_aabb[i];
		}
		m_allocator->Free (m_collisionMatrix);
	
		m_aabb = aabb;
		m_array = array;
		m_collisionMatrix = collisionMatrix;
	}

	m_array[m_count] = part;
	m_array[m_count]->AddRef();
	m_count ++;
*/
}


void dgCollisionCompound::RemoveCollision (dgNodeBase* treeNode)
{
	m_array[treeNode->m_id]->Release();
	m_count --;
	m_array[treeNode->m_id] = m_array[m_count];

	if (!treeNode->m_parent) {
		delete (m_root);
		m_root = NULL;
	} else if (!treeNode->m_parent->m_parent) {
		dgNodeBase* const root = m_root;
		if (treeNode->m_parent->m_left == treeNode) {
			m_root = treeNode->m_parent->m_right;
			treeNode->m_parent->m_right = NULL;
		} else {
			m_root = treeNode->m_parent->m_left;
			treeNode->m_parent->m_left= NULL;
		}
		m_root->m_parent = NULL;
		delete (root);

	} else {
		dgNodeBase* const root = treeNode->m_parent->m_parent;
		if (treeNode->m_parent == root->m_left) {
			if (treeNode->m_parent->m_right == treeNode) {
				root->m_left = treeNode->m_parent->m_left;
				treeNode->m_parent->m_left = NULL;
			} else {
				root->m_left = treeNode->m_parent->m_right;
				treeNode->m_parent->m_right = NULL;
			}
			root->m_left->m_parent = root;
		} else {
			if (treeNode->m_parent->m_right == treeNode) {
				root->m_right = treeNode->m_parent->m_left;
				treeNode->m_parent->m_left = NULL;
			} else {
				root->m_right = treeNode->m_parent->m_right;
				treeNode->m_parent->m_right = NULL;
			}
			root->m_right->m_parent = root;
		}
		delete (treeNode->m_parent);
	}
}

dgVector dgCollisionCompound::SupportVertex (const dgVector& dir) const
{
	dgInt32 ix;
	dgInt32 iy;
	dgInt32 iz;
	dgInt32 stack;
	dgFloat32 maxProj;
	dgFloat32 aabbProjection[DG_COMPOUND_STACK_DEPTH];
	const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];

	stack = 1;
	stackPool[0] = m_root;
	aabbProjection[0] = dgFloat32 (1.0e10f);

	maxProj = dgFloat32 (-1.0e20f); 
	dgVector searchDir (m_offset.UnrotateVector(dir));

	ix = (searchDir[0] > dgFloat32 (0.0f)) ? 1 : 0;
	iy = (searchDir[1] > dgFloat32 (0.0f)) ? 1 : 0;
	iz = (searchDir[2] > dgFloat32 (0.0f)) ? 1 : 0;
	dgVector supportVertex (dgFloat32 (0.0f), dgFloat32 (0.0f),  dgFloat32 (0.0f),  dgFloat32 (0.0f));   

	while (stack) {
		dgFloat32 boxSupportValue;

		stack--;
		boxSupportValue = aabbProjection[stack];
		if (boxSupportValue > maxProj) {
			const dgNodeBase* const me = stackPool[stack];

			if (me->m_type == m_leaf) {

				dgFloat32 dist;
				dgCollision* const shape = me->m_shape;

				dgVector newDir (shape->m_offset.UnrotateVector(searchDir)); 
				dgVector vertex (shape->m_offset.TransformVector (shape->SupportVertex(newDir)));		
				dist = dir % vertex;
				if (dist > maxProj) {
					maxProj = dist;
					supportVertex = vertex;		
				}

			} else {
				dgFloat32 dist0;	
				dgFloat32 dist1;	

				const dgNodeBase* const left = me->m_left;
				const dgNodeBase* const right = me->m_right;

				const dgVector* const box0 = &left->m_p0;
				dgVector p0 (box0[ix].m_x, box0[iy].m_y, box0[iz].m_z, dgFloat32 (0.0f));

				const dgVector* const box1 = &right->m_p0;
				dgVector p1 (box1[ix].m_x, box1[iy].m_y, box1[iz].m_z, dgFloat32 (0.0f));

				dist0 = p0 % dir;
				dist1 = p1 % dir;
				if (dist0 > dist1) {
					stackPool[stack] = right;
					aabbProjection[stack] = dist1;
					stack ++;

					stackPool[stack] = left;
					aabbProjection[stack] = dist0;
					stack ++;
				} else {
					stackPool[stack] = left;
					aabbProjection[stack] = dist0;
					stack ++;

					stackPool[stack] = right;
					aabbProjection[stack] = dist1;
					stack ++;
				} 
			}
		}
	}

	return m_offset.TransformVector (supportVertex);
}


void dgCollisionCompound::GetCollisionInfo(dgCollisionInfo* info) const
{
	dgCollision::GetCollisionInfo(info);

	info->m_offsetMatrix = GetOffsetMatrix();
	info->m_compoundCollision.m_chidrenCount = m_count;
	info->m_compoundCollision.m_chidren = (dgCollision**) m_array;
	info->m_collisionType = m_compoundCollision;
}

void dgCollisionCompound::Serialize(dgSerialize callback, void* const userData) const
{
	dgInt32 stack;
	dgInt32 data[4];
	dgNodeBase* pool[DG_COMPOUND_STACK_DEPTH];


	data[0] = m_count;
	data[1] = 0;
	data[2] = 0;
	data[3] = 0;

	SerializeLow(callback, userData);
	callback (userData, &data, sizeof (data));

	for (dgInt32 i = 0; i < m_count; i ++) {
		dgCollision *collision;

		collision = m_array[i];

		m_world->Serialize (collision, callback, userData);
	}

	pool[0] = m_root;

	stack = 1;
	while (stack) {
		dgNodeBase* node;
		dgNodeBase* parent;

		stack --;
		node = pool[stack];
		
		parent = NULL;
		if (node->m_parent) {
			parent = node->m_parent;
			node->m_parent = (dgNodeBase*) (dgInt64 (node->m_parent->m_id));
		}
		callback (userData, &node->m_p0, sizeof (dgNodeBase));
		node->m_parent = parent;
		

		

		if (node->m_type == m_node) {
			pool[stack] = node->m_right;
			stack ++;
			pool[stack] = node->m_left;
			stack ++;
		} 
	}
}


bool dgCollisionCompound::OOBBTest (const dgMatrix& matrix, const dgCollisionConvex* const shape, void* const cacheOrder) const
{
	_ASSERTE (0);
	return true;
}

/*
dgInt32 dgCollisionCompound::GetAxis (dgNodeBase** const proxiArray, dgInt32 boxCount) const
{
	dgInt32 axis;
	dgFloat32 maxVal;
	dgVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	dgVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	for (dgInt32 i = 0; i < boxCount; i ++) {

		const dgNodeBase* const proxy = proxiArray[i];
		const dgVector& p0 = proxy->m_p0;
		const dgVector& p1 = proxy->m_p1;

		median += p0;
		median += p1;

		varian += p0.CompProduct(p0);
		varian += p1.CompProduct(p1);
	}

	boxCount *= 2;
	varian.m_x = boxCount * varian.m_x - median.m_x * median.m_x;
	varian.m_y = boxCount * varian.m_y - median.m_y * median.m_y;
	varian.m_z = boxCount * varian.m_z - median.m_z * median.m_z;

	axis = 0;
	maxVal = varian[0];
	for (dgInt32 i = 1; i < 3; i ++) {
		if (varian[i] > maxVal) {
			axis = i;
			maxVal = varian[i];
		}
	}
	return axis;
}

dgInt32 dgCollisionCompound::CompareBox (const dgNodeBase* const boxA, const dgNodeBase* const boxB, void* const context)
{
	dgInt32 axis;

	axis = *((dgInt32*) context);

	if (boxA->m_p0[axis] < boxB->m_p0[axis]) {
		return -1;
	} else if (boxA->m_p0[axis] > boxB->m_p0[axis]) {
		return 1;
	}
	return 0;
}


dgCollisionCompound::dgNodeBase* dgCollisionCompound::BuildBottomUpTree(dgInt32 count, dgNodeBase** const proxiArray)
{
	dgInt32 id;
	dgStack<dgHeapNodePair> pool (count / 4 + DG_COMPOUND_STACK_DEPTH);
	dgDownHeap<dgHeapNodePair, dgFloat32> heap (&pool[0], pool.GetSizeInBytes() - 64);

	id = count;
	while (count > 1){
		dgInt32 axis;
		dgInt32 newCount;

		axis = GetAxis (proxiArray, count);

		dgSortIndirect (proxiArray, count, CompareBox, &axis);
		heap.Flush();

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

			dgInt32 bestProxi;
			dgFloat32 smallestVolume;
			dgFloat32 breakValue;
			dgNodeBase* nodeA;

			nodeA = proxiArray[i];
			bestProxi = -1;
			smallestVolume = dgFloat32 (1.0e20f); 
			breakValue = ((count - i) < 32) ? dgFloat32 (1.0e20f) : nodeA->m_p1[axis] + dgFloat32 (2.0f);
			if (breakValue < proxiArray[i + 1]->m_p0[axis]) {
				breakValue = proxiArray[i + 1]->m_p0[axis] + dgFloat32 (2.0f); 
			}

			for (dgInt32 j = i + 1; (j < count) && (proxiArray[j]->m_p0[axis] < breakValue); j ++) {

				dgFloat32 volume;
				dgVector p0;
				dgVector p1;
				dgNodeBase* nodeB;

				nodeB = proxiArray[j];
				p0.m_x = GetMin (nodeA->m_p0.m_x, nodeB->m_p0.m_x);
				p0.m_y = GetMin (nodeA->m_p0.m_y, nodeB->m_p0.m_y);
				p0.m_z = GetMin (nodeA->m_p0.m_z, nodeB->m_p0.m_z);
				p0.m_w = dgFloat32 (0.0f);
				p1.m_x = GetMax (nodeA->m_p1.m_x, nodeB->m_p1.m_x);
				p1.m_y = GetMax (nodeA->m_p1.m_y, nodeB->m_p1.m_y);
				p1.m_z = GetMax (nodeA->m_p1.m_z, nodeB->m_p1.m_z);
				p1.m_w = dgFloat32 (0.0f);
				dgVector dist (p1 - p0);
				volume = dist.m_x * dist.m_y * dist.m_z; 
				if (volume < smallestVolume) {
					bestProxi = j;
					smallestVolume = volume;
				}
			}

			_ASSERTE (bestProxi != -1);

			dgHeapNodePair pair;
			pair.m_nodeA = i;
			pair.m_nodeB = bestProxi;

			if (heap.GetCount() < heap.GetMaxCount()) {
				heap.Push(pair, smallestVolume);
			} else {
				if (smallestVolume < heap.Value()) {
					heap.Pop();
					heap.Push(pair, smallestVolume);
				}
			}
		}

		heap.Sort ();

		for (dgInt32 j = heap.GetCount() - 1; j >= 0; j --) {
			dgHeapNodePair pair (heap[j]);

			if ((proxiArray[pair.m_nodeA]->m_p0.m_w == dgFloat32 (0.0f)) && (proxiArray[pair.m_nodeB]->m_p0.m_w == dgFloat32 (0.0f))) {
				proxiArray[pair.m_nodeA]->m_p0.m_w = dgFloat32 (1.0f);
				proxiArray[pair.m_nodeB]->m_p0.m_w = dgFloat32 (1.0f);

				proxiArray[count] = new (m_allocator) dgNodeBase (proxiArray[pair.m_nodeA], proxiArray[pair.m_nodeB], id);
				proxiArray[pair.m_nodeA]->m_parent = proxiArray[count];
				proxiArray[pair.m_nodeB]->m_parent = proxiArray[count];

				id ++;
				count ++;
			}
		}

		newCount = 0;
		for (dgInt32 i = 0; i < count; i ++) {
			if (proxiArray[i]->m_p0.m_w == dgFloat32 (0.0f)) {
				proxiArray[newCount] = proxiArray[i];
				newCount ++;
			}
		}

		_ASSERTE (newCount < count);
		count = newCount;
	}
	return proxiArray[0];
}
*/

dgCollisionCompound::dgNodeBase* dgCollisionCompound::BuildTopDownTree(dgInt32 count, dgNodeBase** const proxiArray, dgInt32& id)
{
	dgNodeBase* tree = NULL;
	if (count == 1) {
		tree = proxiArray[0];
	} else {
		dgInt32 i0 = 1;
		if (count > 2) {
			dgVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
			dgVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
			for (dgInt32 i = 0; i < count; i ++) {
				const dgNodeBase* const proxy = proxiArray[i];
				const dgVector& p0 = proxy->m_p0;
				const dgVector& p1 = proxy->m_p1;

				median += p0;
				median += p1;

				varian += p0.CompProduct(p0);
				varian += p1.CompProduct(p1);
			}

			dgInt32 pointCount = count * 2;
			varian.m_x = pointCount * varian.m_x - median.m_x * median.m_x;
			varian.m_y = pointCount * varian.m_y - median.m_y * median.m_y;
			varian.m_z = pointCount * varian.m_z - median.m_z * median.m_z;

			dgInt32 axis = 0;
			dgFloat32 maxVal = varian[0];
			for (dgInt32 i = 1; i < 3; i ++) {
				if (varian[i] > maxVal) {
					axis = i;
					maxVal = varian[i];
				}
			}

			dgVector center = median.Scale (dgFloat32 (1.0f) / dgFloat32 (pointCount));
			dgFloat32 test = center[axis];

			dgInt32 i1 = count - 1;
			do {    
				for (; i0 <= i1; i0 ++) {
					const dgNodeBase* const proxy = proxiArray[i0];
					dgFloat32 val = (proxy->m_p0[axis] + proxy->m_p1[axis]) * dgFloat32 (0.5f);
					if (val > test) {
						break;
					}
				}

				for (; i1 >= i0; i1 --) {
					const dgNodeBase* const proxy = proxiArray[i1];
					dgFloat32 val = (proxy->m_p0[axis] + proxy->m_p1[axis]) * dgFloat32 (0.5f);
					if (val < test) {
						break;
					}
				}

				if (i0 < i1)	{
					Swap(proxiArray[i0], proxiArray[i1]);
					i0++; 
					i1--;
				}
			} while (i0 <= i1);

			if (i0 == 0){
				i0 = 1;
			}
			if (i0 >= count - 1) {
				i0 = count - 1;
			}
		}

		dgNodeBase* const left = BuildTopDownTree(i0, &proxiArray[0], id);
		dgNodeBase* const right = BuildTopDownTree(count - i0, &proxiArray[i0], id);
		tree = new (m_allocator) dgNodeBase (left, right, id);
		left->m_parent = tree;
		right->m_parent = tree;

		id ++;
	}
	
	return tree;
}


void dgCollisionCompound::PushNodes (dgNodeBase* const root, dgNodeBase** const proxiArray, dgInt32& index) const
{
	if (root->m_left) {
		PushNodes (root->m_left, proxiArray, index);
	}
	if (root->m_right) {
		PushNodes (root->m_right, proxiArray, index);
	}
	if (!root->m_shape) {
		proxiArray[index] = root;
		index ++;
	}
}

dgFloat32 dgCollisionCompound::CalculateSurfaceArea (dgNodeBase* const node0, dgNodeBase* const node1, dgVector& minBox, dgVector& maxBox) const
{
	minBox = dgVector (GetMin (node0->m_p0.m_x, node1->m_p0.m_x), GetMin (node0->m_p0.m_y, node1->m_p0.m_y), GetMin (node0->m_p0.m_z, node1->m_p0.m_z), dgFloat32 (0.0f));
	maxBox = dgVector (GetMax (node0->m_p1.m_x, node1->m_p1.m_x), GetMax (node0->m_p1.m_y, node1->m_p1.m_y), GetMax (node0->m_p1.m_z, node1->m_p1.m_z), dgFloat32 (0.0f));		
	dgVector side0 ((maxBox - minBox).Scale (dgFloat32 (0.5f)));
	dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f));
	return side0 % side1;
}


void dgCollisionCompound::ImproveNodeFitness (dgNodeBase* const node) const
{
	_ASSERTE (node->m_left);
	_ASSERTE (node->m_right);

	if (node->m_parent)	{
		if (node->m_parent->m_left == node) {
			dgFloat32 cost0 = node->m_area;

			dgVector cost1P0;
			dgVector cost1P1;		
			dgFloat32 cost1 = CalculateSurfaceArea (node->m_right, node->m_parent->m_right, cost1P0, cost1P1);

			dgVector cost2P0;
			dgVector cost2P1;		
			dgFloat32 cost2 = CalculateSurfaceArea (node->m_left, node->m_parent->m_right, cost2P0, cost2P1);

			if ((cost1 <= cost0) && (cost1 <= cost2)) {
				dgNodeBase* const parent = node->m_parent;
				node->m_p0 = parent->m_p0;
				node->m_p1 = parent->m_p1;
				node->m_area = parent->m_area; 
				node->m_size = parent->m_size;
				node->m_origin = parent->m_origin;

				if (parent->m_parent) {
					if (parent->m_parent->m_left == parent) {
						parent->m_parent->m_left = node;
					} else {
						_ASSERTE (parent->m_parent->m_right == parent);
						parent->m_parent->m_right = node;
					}
				}
				node->m_parent = parent->m_parent;
				parent->m_parent = node;
				node->m_right->m_parent = parent;
				parent->m_left = node->m_right;
				node->m_right = parent;
				parent->m_p0 = cost1P0;
				parent->m_p1 = cost1P1;		
				parent->m_area = cost1;
				parent->m_size = (parent->m_p1 - parent->m_p0).Scale(dgFloat32 (0.5f));
				parent->m_origin = (parent->m_p1 + parent->m_p0).Scale(dgFloat32 (0.5f));

			} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
				dgNodeBase* const parent = node->m_parent;
				node->m_p0 = parent->m_p0;
				node->m_p1 = parent->m_p1;
				node->m_area = parent->m_area; 
				node->m_size = parent->m_size;
				node->m_origin = parent->m_origin;

				if (parent->m_parent) {
					if (parent->m_parent->m_left == parent) {
						parent->m_parent->m_left = node;
					} else {
						_ASSERTE (parent->m_parent->m_right == parent);
						parent->m_parent->m_right = node;
					}
				}
				node->m_parent = parent->m_parent;
				parent->m_parent = node;
				node->m_left->m_parent = parent;
				parent->m_left = node->m_left;
				node->m_left = parent;

				parent->m_p0 = cost2P0;
				parent->m_p1 = cost2P1;		
				parent->m_area = cost2;
				parent->m_size = (parent->m_p1 - parent->m_p0).Scale(dgFloat32 (0.5f));
				parent->m_origin = (parent->m_p1 + parent->m_p0).Scale(dgFloat32 (0.5f));
			}
		} else {
			dgFloat32 cost0 = node->m_area;

			dgVector cost1P0;
			dgVector cost1P1;		
			dgFloat32 cost1 = CalculateSurfaceArea (node->m_left, node->m_parent->m_left, cost1P0, cost1P1);

			dgVector cost2P0;
			dgVector cost2P1;		
			dgFloat32 cost2 = CalculateSurfaceArea (node->m_right, node->m_parent->m_left, cost2P0, cost2P1);

			if ((cost1 <= cost0) && (cost1 <= cost2)) {

				dgNodeBase* const parent = node->m_parent;
				node->m_p0 = parent->m_p0;
				node->m_p1 = parent->m_p1;
				node->m_area = parent->m_area; 
				node->m_size = parent->m_size;
				node->m_origin = parent->m_origin;

				if (parent->m_parent) {
					if (parent->m_parent->m_left == parent) {
						parent->m_parent->m_left = node;
					} else {
						_ASSERTE (parent->m_parent->m_right == parent);
						parent->m_parent->m_right = node;
					}
				}
				node->m_parent = parent->m_parent;
				parent->m_parent = node;
				node->m_left->m_parent = parent;
				parent->m_right = node->m_left;
				node->m_left = parent;

				parent->m_p0 = cost1P0;
				parent->m_p1 = cost1P1;		
				parent->m_area = cost1;
				parent->m_size = (parent->m_p1 - parent->m_p0).Scale(dgFloat32 (0.5f));
				parent->m_origin = (parent->m_p1 + parent->m_p0).Scale(dgFloat32 (0.5f));

			} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
				dgNodeBase* const parent = node->m_parent;
				node->m_p0 = parent->m_p0;
				node->m_p1 = parent->m_p1;
				node->m_area = parent->m_area; 
				node->m_size = parent->m_size;
				node->m_origin = parent->m_origin;

				if (parent->m_parent) {
					if (parent->m_parent->m_left == parent) {
						parent->m_parent->m_left = node;
					} else {
						_ASSERTE (parent->m_parent->m_right == parent);
						parent->m_parent->m_right = node;
					}
				}
				node->m_parent = parent->m_parent;
				parent->m_parent = node;
				node->m_right->m_parent = parent;
				parent->m_right = node->m_right;
				node->m_right = parent;

				parent->m_p0 = cost2P0;
				parent->m_p1 = cost2P1;		
				parent->m_area = cost2;
				parent->m_size = (parent->m_p1 - parent->m_p0).Scale(dgFloat32 (0.5f));
				parent->m_origin = (parent->m_p1 + parent->m_p0).Scale(dgFloat32 (0.5f));
			}
		}
	} else {
		// in the future I can handle this but it is too much work for little payoff
	}
}


dgCollisionCompound::dgNodeBase* dgCollisionCompound::BuildTree(dgInt32 count, dgCollisionConvex* const shapeArray[])
{

#if 0
	dgStack<dgNodeBase*> nodeList(count * 2);
	dgNodeBase** const proxiArray = &nodeList[0];
	for (dgInt32 i = 0; i < count; i ++) {
		proxiArray[i] = new (m_allocator) dgNodeBase (shapeArray[i], i);
	}

	dgNodeBase* tree = BuildBottomUpTree(count, proxiArray);
#else
	dgStack<dgNodeBase*> nodeList(count);
	dgNodeBase** const proxiArray = &nodeList[0];
	for (dgInt32 i = 0; i < count; i ++) {
		proxiArray[i] = new (m_allocator) dgNodeBase (shapeArray[i], i);
	}

	dgInt32 id = count;
	dgNodeBase* tree = BuildTopDownTree(count, proxiArray, id);
#endif

	dgInt32 index = 0;
	PushNodes (tree, proxiArray, index);

	dgInt32 maxPasses = 2 * exp_2 (index * 2) + 1;
	dgFloat64 newCost = dgFloat32 (1.0e20f);
	dgFloat64 prevCost = newCost;
	do {
		prevCost = newCost;
		for (dgInt32 i = 0; i < index; i ++) {
			dgNodeBase* const node = proxiArray[i];
			ImproveNodeFitness (node);
		}
		newCost = dgFloat32 (0.0f);
		for (dgInt32 i = 0; i < index; i ++) {
			dgNodeBase* const node = proxiArray[i];
			newCost += node->m_area;
		}
	
		maxPasses --;
	} while (maxPasses && (newCost < prevCost));

	if (tree->m_parent) {
		_ASSERTE (index);
		for (tree = proxiArray[index-1]; tree->m_parent; tree = tree->m_parent);
	}
	return tree;
}


dgInt32 dgCollisionCompound::CalculateContacts (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy, dgInt32 useSimd) const
{
	dgInt32 contactCount = 0;
	if (m_root) {
		_ASSERTE (IsType (dgCollision::dgCollisionCompound_RTTI));
		if (pair->m_body1->m_collision->IsType (dgCollision::dgConvexCollision_RTTI)) {
			contactCount = CalculateContactsToSingle (pair, proxy, useSimd);
		} else if (pair->m_body1->m_collision->IsType (dgCollision::dgCollisionCompound_RTTI)) {
			contactCount = CalculateContactsToCompound (pair, proxy, useSimd);
		} else if (pair->m_body1->m_collision->IsType (dgCollision::dgCollisionBVH_RTTI)) {
			contactCount = CalculateContactsToCollisionTree (pair, proxy, useSimd);
		} else if (pair->m_body1->m_collision->IsType (dgCollision::dgCollisionHeightField_RTTI)) {
			contactCount = CalculateContactsToHightField (pair, proxy, useSimd);
		} else {
			_ASSERTE (pair->m_body1->m_collision->IsType (dgCollision::dgCollisionUserMesh_RTTI));
			contactCount = CalculateContactsBruteForce (pair, proxy, useSimd);
		}
	}
	return contactCount;
}


dgInt32 dgCollisionCompound::CalculateContactsToSingle (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy, dgInt32 useSimd) const
{
	dgVector p0;
	dgVector p1;
	dgContactPoint* const contacts = pair->m_contactBuffer;
	const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];
	
	dgBody* const otherBody = pair->m_body1;
	dgBody* const compoundBody = pair->m_body0;

	_ASSERTE (pair->m_body0->m_collision == this);
	_ASSERTE (otherBody->m_collision->IsType (dgCollision::dgConvexCollision_RTTI));

	//dgInt32 lru = m_world->m_broadPhaseLru;
	proxy.m_referenceBody = compoundBody;

	proxy.m_floatingBody = otherBody;
	proxy.m_floatingCollision = otherBody->m_collision;
	proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;

	dgInt32 contactCount = 0;
	dgMatrix myMatrix (m_offset * compoundBody->m_matrix);
	dgMatrix matrix (otherBody->m_collisionWorldMatrix * myMatrix.Inverse());
	otherBody->m_collision->CalcAABB(dgGetIdentityMatrix(), p0, p1);
	if (proxy.m_unconditionalCast) {
		dgVector step ((otherBody->m_veloc - compoundBody->m_veloc).Scale (proxy.m_timestep));
		step = otherBody->m_collisionWorldMatrix.UnrotateVector(step);
		for (dgInt32 j = 0; j < 3; j ++) {
			if (step[j] > dgFloat32 (0.0f)) {
				p1[j] += step[j]; 
			} else {
				p0[j] += step[j]; 
			}
		}
	}

	OOBBTestData data (matrix, p0, p1);
	
	dgInt32 stack = 1;
	stackPool[0] = m_root;
	while (stack) {

		stack --;
		const dgNodeBase* const me = stackPool[stack];
		_ASSERTE (me);
		
		if (me->BoxTest (data)) {
			if (me->m_type == m_leaf) {
				dgInt32 processContacts;

				processContacts = 1;
				if (pair->m_material && pair->m_material->m_compoundAABBOverlap) {
					processContacts = pair->m_material->m_compoundAABBOverlap (*pair->m_material, *compoundBody, *otherBody, proxy.m_threadIndex);
				}
				if (processContacts) {
					proxy.m_referenceCollision = me->m_shape;

					proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix;

//					proxy.m_floatingCollision = otherBody->m_collision;
//					proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;

					proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
					proxy.m_contacts = &contacts[contactCount];

					if (useSimd) {
						contactCount += m_world->CalculateConvexToConvexContactsSimd (proxy);
					} else {
						contactCount += m_world->CalculateConvexToConvexContacts (proxy);
					}

					if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
						contactCount = m_world->ReduceContacts (contactCount, contacts, DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
					}
				}

			} else {
				_ASSERTE (me->m_type == m_node);
				stackPool[stack] = me->m_left;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

				stackPool[stack] = me->m_right;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));
			}
		}
	}

	return contactCount;
}


dgInt32 dgCollisionCompound::CalculateContactsToCompound (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy, dgInt32 useSimd) const
{
	dgContactPoint* const contacts = pair->m_contactBuffer;
	const dgNodeBase* stackPool[4 * DG_COMPOUND_STACK_DEPTH][2];

	dgInt32 contactCount = 0;
	dgBody* const myBody = pair->m_body0;
	dgBody* const otherBody = pair->m_body1;

	_ASSERTE (pair->m_body0->m_collision == this);
	dgCollisionCompound* const otherCompound = (dgCollisionCompound*)otherBody->m_collision;

	//dgInt32 lru = m_world->m_broadPhaseLru;
	proxy.m_referenceBody = myBody;
	proxy.m_floatingBody = otherBody;
	
	dgMatrix myMatrix (m_offset * myBody->m_matrix);
	dgMatrix otherMatrix (otherCompound->m_offset * otherBody->m_matrix);
	OOBBTestData data (otherMatrix * myMatrix.Inverse());

	dgInt32 stack = 1;
	stackPool[0][0] = m_root;
	stackPool[0][1] = otherCompound->m_root;
	while (stack) {

		stack --;
		const dgNodeBase* const me = stackPool[stack][0];
		const dgNodeBase* const other = stackPool[stack][1];

		_ASSERTE (me && other);

		if (me->BoxTest (data, other)) {
			if ((me->m_type == m_leaf) && (other->m_type == m_leaf)) {
				dgInt32 processContacts;

				processContacts = 1;
				if (pair->m_material && pair->m_material->m_compoundAABBOverlap) {
					processContacts = pair->m_material->m_compoundAABBOverlap (*pair->m_material, *myBody, *otherBody, proxy.m_threadIndex);
				}
				if (processContacts) {
//					dgShapeCell* const myCell = (dgShapeCell*)me;
//					dgShapeCell* const otherCell = (dgShapeCell*)other;
					
//					if (myCell->m_lru != lru) {
//						m_world->dgGetIndirectLock(&myCell->m_criticalSection);
//						myCell->m_lru = lru;
//						myCell->m_collisionMatrix = myCell->m_shape->m_offset * myMatrix;
//						m_world->dgReleaseIndirectLock(&myCell->m_criticalSection);
//					}
					

//					if (otherCell->m_lru != lru) {
//						m_world->dgGetIndirectLock(&otherCell->m_criticalSection);
//						otherCell->m_lru = lru;
//						otherCell->m_collisionMatrix = otherCell->m_shape->m_offset * otherMatrix;
//						m_world->dgReleaseIndirectLock(&otherCell->m_criticalSection);
//					}

					proxy.m_referenceCollision = me->m_shape;
					proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix ;
					
					proxy.m_floatingCollision = other->m_shape;
					proxy.m_floatingMatrix = other->m_shape->m_offset * otherMatrix ;

					proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
					proxy.m_contacts = &contacts[contactCount];

					if (useSimd) {
						contactCount += m_world->CalculateConvexToConvexContactsSimd (proxy);
					} else {
						contactCount += m_world->CalculateConvexToConvexContacts (proxy);
					}
					if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
						contactCount = m_world->ReduceContacts (contactCount, contacts, DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
					}
				}


			} else if (me->m_type == m_leaf) {
//				dgNode* const otherNode = (dgNode*)other;
				_ASSERTE (other->m_type == m_node);

				stackPool[stack][0] = me;
				stackPool[stack][1] = other->m_left;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

				stackPool[stack][0] = me;
				stackPool[stack][1] = other->m_right;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));


			} else if (other->m_type == m_leaf) {
//				dgNode* const myNode = (dgNode*)me;
				_ASSERTE (me->m_type == m_node);

				stackPool[stack][0] = me->m_left;
				stackPool[stack][1] = other;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

				stackPool[stack][0] = me->m_right;
				stackPool[stack][1] = other;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

			} else if (other->m_area > me->m_area) {
//				dgNode* const otherNode = (dgNode*)other;
				_ASSERTE (me->m_type == m_node);
				_ASSERTE (other->m_type == m_node);

				stackPool[stack][0] = me;
				stackPool[stack][1] = other->m_left;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

				stackPool[stack][0] = me;
				stackPool[stack][1] = other->m_right;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

			} else {
//				dgNode* const myNode = (dgNode*)me;
				_ASSERTE (me->m_type == m_node);
				_ASSERTE (other->m_type == m_node);

				stackPool[stack][0] = me->m_left;
				stackPool[stack][1] = other;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

				stackPool[stack][0] = me->m_right;
				stackPool[stack][1] = other;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));
			}
		}
	}

	return contactCount;
}


dgInt32 dgCollisionCompound::CalculateContactsToCollisionTree (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy, dgInt32 useSimd) const
{
	dgContactPoint* const contacts = pair->m_contactBuffer;
	dgTree<const dgNodeBase*, const dgNodeBase*> filter(m_allocator);

	struct NodePairs
	{
		dgNodeBase* m_myNode;
		dgInt32 m_treeNodeIsLeaf;
		const void* m_treeNode;
	};
	NodePairs stackPool[4 * DG_COMPOUND_STACK_DEPTH];
	
	dgInt32 contactCount = 0;

	dgBody* const myBody = pair->m_body0;
	dgBody* const treeBody = pair->m_body1;

	_ASSERTE (pair->m_body0->m_collision == this);
	dgCollisionBVH* const treeCollision = (dgCollisionBVH*)treeBody->m_collision;

//	dgInt32 lru = m_world->m_broadPhaseLru;
	proxy.m_referenceBody = myBody;
	proxy.m_floatingBody = treeBody;

	proxy.m_floatingCollision = treeCollision;
	proxy.m_floatingMatrix = treeBody->m_collisionWorldMatrix;

	dgMatrix myMatrix (m_offset * myBody->m_matrix);
	OOBBTestData data (proxy.m_floatingMatrix * myMatrix.Inverse());

	dgInt32 stack = 1;
	stackPool[0].m_myNode = m_root;
	stackPool[0].m_treeNode = treeCollision->GetRootNode();
	stackPool[0].m_treeNodeIsLeaf = 0;

	dgNodeBase nodeProxi;
	nodeProxi.m_left = NULL;
	nodeProxi.m_right = NULL;

	while (stack) {
		dgInt32 treeNodeIsLeaf;
		dgVector p0;
		dgVector p1;

		stack --;
		dgNodeBase* const me = stackPool[stack].m_myNode;
		const void* const other = stackPool[stack].m_treeNode;
		treeNodeIsLeaf = stackPool[stack].m_treeNodeIsLeaf;

		_ASSERTE (me && other);

		treeCollision->GetNodeAABB(other, nodeProxi.m_p0, nodeProxi.m_p1);
		nodeProxi.m_size = (nodeProxi.m_p1 - nodeProxi.m_p0).Scale (dgFloat32 (0.5f));
		nodeProxi.m_origin = (nodeProxi.m_p1 + nodeProxi.m_p0).Scale (dgFloat32 (0.5f));
		dgVector size (nodeProxi.m_size.m_y, nodeProxi.m_size.m_z, nodeProxi.m_size.m_x, dgFloat32 (0.0f));
		nodeProxi.m_area = nodeProxi.m_size  % size;
		_ASSERTE (nodeProxi.m_area > dgFloat32 (0.0f));

		if (me->BoxTest (data, &nodeProxi)) {
			if ((me->m_type == m_leaf) && treeNodeIsLeaf) {
				if (!filter.Find(me)) {
					m_world->dgGetUserLock();
					filter.Insert(me, me);
					m_world->dgReleasedUserLock();

//					dgShapeCell* const myCell = (dgShapeCell*)me;
//					m_world->dgGetIndirectLock(&myCell->m_criticalSection);
//					if (myCell->m_lru != lru) {
//						myCell->m_lru = lru;
//						myCell->m_collisionMatrix = myCell->m_shape->m_offset * myMatrix;
//					}
//					m_world->dgReleaseIndirectLock(&myCell->m_criticalSection);

					proxy.m_referenceCollision = me->m_shape;
					proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix ;

					proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
					proxy.m_contacts = &contacts[contactCount];

					if (useSimd) {
						contactCount += m_world->CalculateConvexToNonConvexContactsSimd (proxy);
					} else {
						contactCount += m_world->CalculateConvexToNonConvexContacts (proxy);
					}
					if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
						contactCount = m_world->ReduceContacts (contactCount, contacts, DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
					}
					if (filter.GetCount() == m_count) {
						break;
					}
				}


			} else if (me->m_type == m_leaf) {

				void* const frontNode = treeCollision->GetFrontNode(other);
				void* const backNode = treeCollision->GetBackNode(other);
				if (backNode && frontNode) {
					stackPool[stack].m_myNode = (dgNodeBase*) me;
					stackPool[stack].m_treeNode = backNode;
					stackPool[stack].m_treeNodeIsLeaf = 0;
					stack++;

					stackPool[stack].m_myNode = (dgNodeBase*) me;
					stackPool[stack].m_treeNode = frontNode;
					stackPool[stack].m_treeNodeIsLeaf = 0;
					stack++;

				} else if (backNode && !frontNode) {
					stackPool[stack].m_myNode = (dgNodeBase*) me;
					stackPool[stack].m_treeNode = backNode;
					stackPool[stack].m_treeNodeIsLeaf = 0;
					stack++;

					stackPool[stack].m_myNode = (dgNodeBase*) me;
					stackPool[stack].m_treeNode = other;
					stackPool[stack].m_treeNodeIsLeaf = 1;
					stack++;

				} else if (!backNode && frontNode) {
					stackPool[stack].m_myNode = (dgNodeBase*) me;
					stackPool[stack].m_treeNode = frontNode;
					stackPool[stack].m_treeNodeIsLeaf = 0;
					stack++;

					stackPool[stack].m_myNode = (dgNodeBase*) me;
					stackPool[stack].m_treeNode = other;
					stackPool[stack].m_treeNodeIsLeaf = 1;
					stack++;

				} else {
					stackPool[stack].m_myNode = (dgNodeBase*) me;
					stackPool[stack].m_treeNode = other;
					stackPool[stack].m_treeNodeIsLeaf = 1;
					stack++;
				}

			} else if (treeNodeIsLeaf) {
				stackPool[stack].m_myNode = me->m_left;
				stackPool[stack].m_treeNode = other;
				stackPool[stack].m_treeNodeIsLeaf = 1;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

				stackPool[stack].m_myNode = me->m_right;
				stackPool[stack].m_treeNode = other;
				stackPool[stack].m_treeNodeIsLeaf = 1;
				stack++;
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

			} else if (nodeProxi.m_area > me->m_area) {
				_ASSERTE (me->m_type == m_node);
				void* const frontNode = treeCollision->GetFrontNode(other);
				void* const backNode = treeCollision->GetBackNode(other);
				if (backNode && frontNode) {
					stackPool[stack].m_myNode = (dgNodeBase*) me;
					stackPool[stack].m_treeNode = backNode;
					stackPool[stack].m_treeNodeIsLeaf = 0;
					stack++;

					stackPool[stack].m_myNode = (dgNodeBase*) me;
					stackPool[stack].m_treeNode = frontNode;
					stackPool[stack].m_treeNodeIsLeaf = 0;
					stack++;
				} else if (backNode && !frontNode) {
					stackPool[stack].m_myNode = (dgNodeBase*) me;
					stackPool[stack].m_treeNode = backNode;
					stackPool[stack].m_treeNodeIsLeaf = 0;
					stack++;

					stackPool[stack].m_myNode = me->m_left;
					stackPool[stack].m_treeNode = other;
					stackPool[stack].m_treeNodeIsLeaf = 1;
					stack++;

					stackPool[stack].m_myNode = me->m_right;
					stackPool[stack].m_treeNode = other;
					stackPool[stack].m_treeNodeIsLeaf = 1;
					stack++;

				} else if (!backNode && frontNode) {
					stackPool[stack].m_myNode = me;
					stackPool[stack].m_treeNode = frontNode;
					stackPool[stack].m_treeNodeIsLeaf = 0;
					stack++;

					stackPool[stack].m_myNode = me->m_left;
					stackPool[stack].m_treeNode = other;
					stackPool[stack].m_treeNodeIsLeaf = 1;
					stack++;

					stackPool[stack].m_myNode = me->m_right;
					stackPool[stack].m_treeNode = other;
					stackPool[stack].m_treeNodeIsLeaf = 1;
					stack++;


				} else {
					stackPool[stack].m_myNode = me->m_left;
					stackPool[stack].m_treeNode = other;
					stackPool[stack].m_treeNodeIsLeaf = 1;
					stack++;

					stackPool[stack].m_myNode = me->m_right;
					stackPool[stack].m_treeNode = other;
					stackPool[stack].m_treeNodeIsLeaf = 1;
					stack++;
				}

			} else {
				_ASSERTE (me->m_type == m_node);
				stackPool[stack].m_myNode = me->m_left;
				stackPool[stack].m_treeNode = other;
				stackPool[stack].m_treeNodeIsLeaf = treeNodeIsLeaf;
				stack++;

				stackPool[stack].m_myNode = me->m_right;
				stackPool[stack].m_treeNode = other;
				stackPool[stack].m_treeNodeIsLeaf = treeNodeIsLeaf;
				stack++;
			}
		}
	}
	
	if (filter.GetCount()) {
		m_world->dgGetUserLock();
		filter.RemoveAll();
		m_world->dgReleasedUserLock();
	}
	
	return contactCount;
}


dgInt32 dgCollisionCompound::CalculateContactsToHightField (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy, dgInt32 useSimd) const
{
	dgNodeBase nodeProxi;
	dgContactPoint* const contacts = pair->m_contactBuffer;
	const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];

	dgInt32 contactCount = 0;
	dgBody* const myBody = pair->m_body0;
	dgBody* const terrainBody = pair->m_body1;

	_ASSERTE (pair->m_body0->m_collision == this);
	dgCollisionHeightField* const terrainCollision = (dgCollisionHeightField*)terrainBody->m_collision;

//	dgInt32 lru = m_world->m_broadPhaseLru;
	proxy.m_referenceBody = myBody;
	proxy.m_floatingBody = terrainBody;

	proxy.m_floatingCollision = terrainCollision;
	proxy.m_floatingMatrix = terrainBody->m_collisionWorldMatrix;

	dgMatrix myMatrix (m_offset * myBody->m_matrix);
	OOBBTestData data (proxy.m_floatingMatrix * myMatrix.Inverse());

	dgInt32 stack = 1;
	stackPool[0] = m_root;

	nodeProxi.m_left = NULL;
	nodeProxi.m_right = NULL;
	while (stack) {
		stack --;
		const dgNodeBase* const me = stackPool[stack];
		
		dgVector origin (data.m_matrix.UntransformVector(me->m_origin));
		dgVector size (data.m_absMatrix.UnrotateVector(me->m_size));
		dgVector p0 (origin - size);
		dgVector p1 (origin + size);

		terrainCollision->GetLocalAABB (p0, p1, nodeProxi.m_p0, nodeProxi.m_p1);
		nodeProxi.m_size = (nodeProxi.m_p1 - nodeProxi.m_p0).Scale (dgFloat32 (0.5f));
		nodeProxi.m_origin = (nodeProxi.m_p1 + nodeProxi.m_p0).Scale (dgFloat32 (0.5f));
		if (me->BoxTest (data, &nodeProxi)) {
			if (me->m_type == m_leaf) {
//				dgShapeCell* const myCell = (dgShapeCell*)me;

//				m_world->dgGetIndirectLock(&myCell->m_criticalSection);
//				if (myCell->m_lru != lru) {
//					myCell->m_lru = lru;
//					myCell->m_collisionMatrix = myCell->m_shape->m_offset * myMatrix;
//				}
//				m_world->dgReleaseIndirectLock(&myCell->m_criticalSection);

				proxy.m_referenceCollision = me->m_shape;
				proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix ;

				proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
				proxy.m_contacts = &contacts[contactCount];

				if (useSimd) {
					contactCount += m_world->CalculateConvexToNonConvexContactsSimd (proxy);
				} else {
					contactCount += m_world->CalculateConvexToNonConvexContacts (proxy);
				}
				if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
					contactCount = m_world->ReduceContacts (contactCount, contacts, DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
				}
	
			} else {
				_ASSERTE (me->m_type == m_node);
				stackPool[stack] = me->m_left;
				stack++;

				stackPool[stack] = me->m_right;
				stack++;
			}
		}
	}

	return contactCount;
}


dgInt32 dgCollisionCompound::CalculateContactsBruteForce (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy, dgInt32 useSimd) const
{
	dgContactPoint* const contacts = pair->m_contactBuffer;
	const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];

	dgInt32 contactCount = 0;

	dgBody* const myBody = pair->m_body0;
	dgBody* const userBody = pair->m_body1;

	_ASSERTE (pair->m_body0->m_collision == this);
	dgCollisionMesh* const userCollision = (dgCollisionMesh*)userBody->m_collision;

//	dgInt32 lru = m_world->m_broadPhaseLru;
	proxy.m_referenceBody = myBody;
	proxy.m_floatingBody = userBody;

	proxy.m_floatingCollision = userCollision;
	proxy.m_floatingMatrix = userBody->m_collisionWorldMatrix;

	dgMatrix myMatrix (m_offset * myBody->m_matrix);

	dgInt32 stack = 1;
	stackPool[0] = m_root;
	dgNodeBase nodeProxi;
	nodeProxi.m_left = NULL;
	nodeProxi.m_right = NULL;

	while (stack) {
		stack --;
		const dgNodeBase* const me = stackPool[stack];
		
		if (me->m_type == m_leaf) {
//			dgShapeCell* const myCell = (dgShapeCell*)me;

//			m_world->dgGetIndirectLock(&myCell->m_criticalSection);
//			if (myCell->m_lru != lru) {
//				myCell->m_lru = lru;
//				myCell->m_collisionMatrix = myCell->m_shape->m_offset * myMatrix;
//			}
//			m_world->dgReleaseIndirectLock(&myCell->m_criticalSection);

			proxy.m_referenceCollision = me->m_shape;
			proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix ;

			proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
			proxy.m_contacts = &contacts[contactCount];

			if (useSimd) {
				contactCount += m_world->CalculateConvexToNonConvexContactsSimd (proxy);
			} else {
				contactCount += m_world->CalculateConvexToNonConvexContacts (proxy);
			}
			if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
				contactCount = m_world->ReduceContacts (contactCount, contacts, DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
			}

		} else {
			_ASSERTE (me->m_type == m_node);
			stackPool[stack] = me->m_left;
			stack++;

			stackPool[stack] = me->m_right;
			stack++;
		}
	}

	return contactCount;
}


dgInt32 dgCollisionCompound::ClosestDitance (dgBody* const compoundBody, dgTriplex& contactA, dgBody* const bodyB, dgTriplex& contactB, dgTriplex& normalAB) const
{
	if (!m_root) {
		return 0;
	}


#if 0
	_ASSERTE (compoundBody->m_collision == this);
	dgCollisionConvex* const collisionB = (dgCollisionConvex*) bodyB->m_collision;

	//	dgInt32 lru;
	//	dgInt32 stack;
	//	dgInt32 contactCount;
	//	dgBody* otherBody; 
	//	dgBody* compoundBody;
	//	dgVector p0;
	//	dgVector p1;
	//	dgContactPoint* const contacts = pair->m_contactBuffer;
	//	const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];
	//	otherBody = pair->m_body1;
	//	compoundBody = pair->m_body0;

	//	_ASSERTE (pair->m_body0->m_collision == this);
	//	_ASSERTE (otherBody->m_collision->IsType (dgCollision::dgConvexCollision_RTTI));

	//	lru = m_world->m_broadPhaseLru;
	//	proxy.m_referenceBody = compoundBody;
	//	proxy.m_floatingBody = otherBody;
	//	proxy.m_floatingCollision = otherBody->m_collision;
	//	proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;
	//	contactCount = 0;

	dgVector p0;
	dgVector p1;
	dgMatrix myMatrix (m_offset * compoundBody->m_matrix);
	dgMatrix matrix (bodyB->m_collisionWorldMatrix * myMatrix.Inverse());
	collisionB->CalcAABB(dgGetIdentityMatrix(), p0, p1);

	dgFloat32 distPool[DG_COMPOUND_STACK_DEPTH];
	const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];


	

	dgVector points[8];
	points[0] = dgVector (p0.m_x, p0.m_y, p0.m_z, dgFloat32 (0.0f));
	points[1] = dgVector (p0.m_x, p0.m_y, p1.m_z, dgFloat32 (0.0f));
	points[2] = dgVector (p0.m_x, p1.m_y, p0.m_z, dgFloat32 (0.0f));
	points[3] = dgVector (p0.m_x, p1.m_y, p1.m_z, dgFloat32 (0.0f));
	points[4] = dgVector (p1.m_x, p0.m_y, p0.m_z, dgFloat32 (0.0f));
	points[5] = dgVector (p1.m_x, p0.m_y, p1.m_z, dgFloat32 (0.0f));
	points[6] = dgVector (p1.m_x, p1.m_y, p0.m_z, dgFloat32 (0.0f));
	points[7] = dgVector (p1.m_x, p1.m_y, p1.m_z, dgFloat32 (0.0f));
	matrix.TransformTriplex(points, sizeof (dgVector), points, sizeof (dgVector), 8);

	dgInt32 stack = 1;
	stackPool[0] = m_root;
	distPool[0] = m_root->BoxClosestDistance (points, 8);

	dgFloat32 baseDist = dgFloat32 (1.0e10f);
	while (stack) {
		stack --;
		
		dgFloat32 dist = distPool[stack];
		const dgNodeBase* const me = stackPool[stack];
		_ASSERTE (me);

//		if (me->BoxTest (data)) {
		if (dist < baseDist) {

			if (me->m_type == m_leaf) {
				_ASSERTE (0);
/*
				dgInt32 processContacts;

				processContacts = 1;
				if (pair->m_material && pair->m_material->m_compoundAABBOverlap) {
					processContacts = pair->m_material->m_compoundAABBOverlap (*pair->m_material, *compoundBody, *otherBody, proxy.m_threadIndex);
				}
				if (processContacts) {
					proxy.m_referenceCollision = me->m_shape;

					proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix;

					//					proxy.m_floatingCollision = otherBody->m_collision;
					//					proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;

					proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
					proxy.m_contacts = &contacts[contactCount];

					if (useSimd) {
						contactCount += m_world->CalculateConvexToConvexContactsSimd (proxy);
					} else {
						contactCount += m_world->CalculateConvexToConvexContacts (proxy);
					}

					if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
						contactCount = m_world->ReduceContacts (contactCount, contacts, DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
					}
				}
*/
			} else {
				const dgNodeBase* const left = me->m_left;
				dgFloat32 leftDist = dgFloat32 (0.0f);
				if (left->m_type != m_leaf) {
					leftDist = left->BoxClosestDistance (points, 8);
				}

				const dgNodeBase* const right = me->m_right;;
				dgFloat32 rightDist = dgFloat32 (0.0f);
				if (right->m_type != m_leaf) {
					rightDist = right->BoxClosestDistance (points, 8);
				}

				_ASSERTE (me->m_type == m_node);
				if (leftDist > rightDist) {
					distPool[stack] = leftDist;
					stackPool[stack] = me->m_left;
					stack++;
					_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));

					distPool[stack] = rightDist;
					stackPool[stack] = me->m_right;
					stack++;
					_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));
				} else {
					distPool[stack] = rightDist;
					stackPool[stack] = me->m_right;
					stack++;
					_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));
					
					distPool[stack] = leftDist;
					stackPool[stack] = me->m_left;
					stack++;
					_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNodeBase*));
				}
			}
		}
	}
	return 0;
#endif

	// this is temporary until I implement the code above using the spacial organization 
	dgInt32 retFlag = 1;
	dgContactPoint contact0;
	dgContactPoint contact1;
	if (bodyB->m_collision->IsType (dgCollision::dgConvexCollision_RTTI)) {
		dgCollisionParamProxy proxy(0);
		dgContactPoint contacts[16];

		proxy.m_referenceBody = compoundBody;
		proxy.m_floatingBody = bodyB;
		proxy.m_floatingCollision = bodyB->m_collision;
		proxy.m_floatingMatrix = bodyB->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 myMatrix (m_offset * compoundBody->m_matrix);
		dgFloat32 minDist2 = dgFloat32 (1.0e10f);
		for (dgInt32 i = 0; (i < m_count) && retFlag; i ++) {
			retFlag = 0;
			proxy.m_referenceCollision = m_array[i];
			proxy.m_referenceMatrix = m_array[i]->m_offset * myMatrix;
			dgInt32 flag = m_world->ClosestPoint (proxy);
			if (flag) {
				retFlag = 1;
				dgVector err (contacts[0].m_point - contacts[1].m_point);
				dgFloat32 dist2 = err % err;
				if (dist2 < minDist2) {
					minDist2 = dist2;
					contact0 = contacts[0];
					contact1 = contacts[1];
				}
			}
		}
	} else {

		dgCollisionParamProxy proxy(0);
		dgContactPoint contacts[16];

		_ASSERTE (bodyB->m_collision->IsType (dgCollision::dgCollisionCompound_RTTI));
		dgCollisionCompound *const compoundCollision1 = (dgCollisionCompound *) bodyB->m_collision;
		dgInt32 count1 = compoundCollision1->m_count;
		dgCollisionConvex** collisionArray1 = compoundCollision1->m_array;

		proxy.m_referenceBody = compoundBody;
		proxy.m_floatingBody = bodyB;
		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 myMatrix (m_offset * compoundBody->m_matrix);
		dgMatrix otherMatrix (compoundCollision1->m_offset * bodyB->m_matrix);
		dgFloat32 minDist2 = dgFloat32 (1.0e10f);
		for (dgInt32 i = 0; (i < m_count) && retFlag; i ++) {
			proxy.m_referenceCollision = m_array[i];
			proxy.m_referenceMatrix = m_array[i]->m_offset * myMatrix;;

			for (dgInt32 j = 0; (j < count1) && retFlag; j ++) {
				retFlag = 0;
				proxy.m_floatingCollision = collisionArray1[j];
				proxy.m_floatingMatrix = collisionArray1[j]->m_offset * otherMatrix;

				dgInt32 flag = m_world->ClosestPoint (proxy);
				if (flag) {
					retFlag = 1;
					dgVector err (contacts[0].m_point - contacts[1].m_point);
					dgFloat32 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;
}