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

#include "dgPhysicsStdafx.h"
#include "dgBody.h"
#include "dgWorld.h"
#include "dgContact.h"
#include "dgIntersections.h"
#include "dgCollisionScene.h"
#include "dgCollisionNull.h"

#define DG_SCENE_MAX_STACK_DEPTH  128
#define DG_SCENE_AABB_SCALE		dgFloat32 (4.0f)
#define DG_SCENE_AABB_INV_SCALE	(dgFloat32 (1.0f) / DG_SCENE_AABB_SCALE)


dgCollisionScene::dgNode::dgNode ()
	:m_parent(NULL)
	,m_left(NULL)
	,m_right(NULL)
	,m_fitnessNode(NULL)
{
}

dgCollisionScene::dgNode::dgNode (dgNode* const sibling, dgNode* const myNode)
	:m_parent(sibling->m_parent)
	,m_left(sibling)
	,m_right(myNode)
	,m_fitnessNode(NULL)
{
	if (m_parent) {
		if (m_parent->m_left == sibling) {
			m_parent->m_left = this;
		} else {
			_ASSERTE (m_parent->m_right == sibling);
			m_parent->m_right = this;
		}
	}

	sibling->m_parent = this;
	myNode->m_parent = this;

	dgNode* const left = m_left;
	dgNode* const right = m_right;

	m_minBox = dgVector (GetMin (left->m_minBox.m_x, right->m_minBox.m_x), GetMin (left->m_minBox.m_y, right->m_minBox.m_y), GetMin (left->m_minBox.m_z, right->m_minBox.m_z), dgFloat32 (0.0f));
	m_maxBox = dgVector (GetMax (left->m_maxBox.m_x, right->m_maxBox.m_x), GetMax (left->m_maxBox.m_y, right->m_maxBox.m_y), GetMax (left->m_maxBox.m_z, right->m_maxBox.m_z), dgFloat32 (0.0f));

	dgVector side0 (m_maxBox - m_minBox);
	dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f));
	m_surfaceArea = side0 % side1;
}


dgCollisionScene::dgNode::~dgNode ()
{
	if (m_left) {
		delete (m_left);
	}
	if (m_right) {
		delete (m_right);
	}
}

dgCollisionScene::dgProxy::dgProxy (dgCollision* m_shape, const dgMatrix& matrix, dgCollisionScene* const owner)
	:dgNode ()
	,m_shape (m_shape)
	,m_owner (owner)
	,m_userData(NULL)
{
	dgVector boxP0;
	dgVector boxP1;

	m_matrix = m_shape->GetOffsetMatrix() * matrix;
	m_shape->CalcAABB (m_matrix, boxP0, boxP1);

	dgVector p0 (boxP0.CompProduct(dgVector (DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE, dgFloat32 (0.0f))));
	dgVector p1 (boxP1.CompProduct(dgVector (DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE, dgFloat32 (0.0f))));

	m_minBox.m_x = dgFloor (p0.m_x) * DG_SCENE_AABB_INV_SCALE; 
	m_minBox.m_y = dgFloor (p0.m_y) * DG_SCENE_AABB_INV_SCALE;  
	m_minBox.m_z = dgFloor (p0.m_z) * DG_SCENE_AABB_INV_SCALE;  
	m_minBox.m_w = dgFloat32 (0.0f);

	m_maxBox.m_x = dgFloor (p1.m_x + dgFloat32 (1.0f)) * DG_SCENE_AABB_INV_SCALE;  
	m_maxBox.m_y = dgFloor (p1.m_y + dgFloat32 (1.0f)) * DG_SCENE_AABB_INV_SCALE;  
	m_maxBox.m_z = dgFloor (p1.m_z + dgFloat32 (1.0f)) * DG_SCENE_AABB_INV_SCALE;  
	m_maxBox.m_w = dgFloat32 (0.0f);

	dgVector side0 (m_maxBox - m_minBox);
	dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f));
	m_surfaceArea = side0 % side1;
}

dgCollisionScene::dgProxy::~dgProxy()
{
}


dgCollisionScene::dgCollisionScene(dgWorld* world)
	:dgCollision (world->GetAllocator(), 0, dgGetIdentityMatrix(), m_sceneCollision)
	,m_lock(0)
	,m_list(world->GetAllocator())
	,m_fitnessList(world->GetAllocator())
{
	m_world = world;
	m_rootNode = NULL;

	m_rtti |= dgCollisionScene_RTTI;
	SetCollisionBBox (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)),
					  dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)));

}

dgCollisionScene::dgCollisionScene (dgWorld* const world, dgDeserialize deserialization, void* const userData)
	:dgCollision (world, deserialization, userData)
	,m_lock(0)
	,m_list(world->GetAllocator())
	,m_fitnessList(world->GetAllocator())
{
	dgInt32 data[4];

	m_world = world;
	m_rtti |= dgCollisionScene_RTTI;

	deserialization (userData, &data, sizeof (data));
	for (dgInt32 i = 0; i < data[0]; i ++) {
		dgMatrix matrix;
		void* data;
		deserialization (userData, &matrix, sizeof (dgMatrix));
		deserialization (userData, &data, sizeof (void*));
		dgCollision* const collision = m_world->CreateFromSerialization (deserialization, userData);
		dgList<dgProxy*>::dgListNode* const proxyNode = (dgList<dgProxy*>::dgListNode*) AddProxy (collision, matrix);
		dgProxy* const proxy = proxyNode->GetInfo();
		proxy->m_userData = data;
		collision->Release();
	}

	ImproveTotalFitness();
}

dgCollisionScene::~dgCollisionScene(void)
{
	for (dgList<dgProxy*>::dgListNode* node = m_list.GetFirst(); node; ) {
		dgList<dgProxy*>::dgListNode* const proxyNode = node;
		node = node->GetNext();
		RemoveProxy(proxyNode);
	}
}

void dgCollisionScene::Serialize(dgSerialize callback, void* const userData) const
{
	dgInt32 data[4];

	SerializeLow(callback, userData);

	data[0] = m_list.GetCount();
	data[1] = 0;
	data[2] = 0;
	data[3] = 0;
	callback (userData, &data, sizeof (data));

	for (dgList<dgProxy*>::dgListNode* node = m_list.GetFirst(); node; node = node->GetNext()) {
		dgProxy* const proxy = node->GetInfo();
		callback (userData, &proxy->m_matrix, sizeof (dgMatrix));
		callback (userData, &proxy->m_userData, sizeof (void*));
		m_world->Serialize (proxy->m_shape, callback, userData);
	}
}


void dgCollisionScene::SetProxyMatrix (void* proxy, const dgMatrix& matrix)
{
	dgVector boxP0;
	dgVector boxP1;

	dgProxy* const entry = ((dgList<dgProxy*>::dgListNode*) proxy)->GetInfo();

	entry->m_matrix = entry->m_shape->GetOffsetMatrix() * matrix;
	entry->m_shape->CalcAABB (entry->m_matrix, boxP0, boxP1);

	dgVector p0 (boxP0.CompProduct(dgVector (DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE, dgFloat32 (0.0f))));
	dgVector p1 (boxP1.CompProduct(dgVector (DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE, dgFloat32 (0.0f))));

	p0.m_x = dgFloor (p0.m_x) * DG_SCENE_AABB_INV_SCALE; 
	p0.m_y = dgFloor (p0.m_y) * DG_SCENE_AABB_INV_SCALE;  
	p0.m_z = dgFloor (p0.m_z) * DG_SCENE_AABB_INV_SCALE;  

	p1.m_x = dgFloor (p1.m_x + dgFloat32 (1.0f)) * DG_SCENE_AABB_INV_SCALE;  
	p1.m_y = dgFloor (p1.m_y + dgFloat32 (1.0f)) * DG_SCENE_AABB_INV_SCALE;  
	p1.m_z = dgFloor (p1.m_z + dgFloat32 (1.0f)) * DG_SCENE_AABB_INV_SCALE;  

	entry->m_minBox = p0;
	entry->m_maxBox = p1;

	dgVector side0 (p1 - p0);
	dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f));
	entry->m_surfaceArea = side0 % side1;

	for (dgNode* parent = entry->m_parent; parent; parent = parent->m_parent) {
		dgVector minBox;
		dgVector maxBox;
		dgFloat32 area = CalculateSurfaceArea (parent->m_left, parent->m_right, minBox, maxBox);
		if (!((parent->m_minBox.m_x < minBox.m_x) || (parent->m_minBox.m_y < minBox.m_y) || (parent->m_minBox.m_z < minBox.m_z) ||
			(parent->m_maxBox.m_x > maxBox.m_x) || (parent->m_maxBox.m_y < maxBox.m_y) || (parent->m_maxBox.m_z < maxBox.m_z))) {
				break;
		}

		m_world->dgGetIndirectLock (&m_lock);
		parent->m_minBox = minBox;
		parent->m_maxBox = maxBox;
		parent->m_surfaceArea = area;
		m_world->dgReleaseIndirectLock (&m_lock);
	}
}

dgMatrix dgCollisionScene::GetProxyMatrix (void* const proxy)
{
	dgProxy* const entry = ((dgList<dgProxy*>::dgListNode*) proxy)->GetInfo();
	return entry->m_shape->GetOffsetMatrix().Inverse() * entry->m_matrix;
}


void dgCollisionScene::SetProxyUserData (void* const proxy, void* const userData)
{
	dgProxy* const entry = ((dgList<dgProxy*>::dgListNode*) proxy)->GetInfo();
	entry->m_userData = userData;
}

void* dgCollisionScene::GetProxyUserData (void* const proxy) const
{
	dgProxy* const entry = ((dgList<dgProxy*>::dgListNode*) proxy)->GetInfo();
	return entry->m_userData;
}


void dgCollisionScene::SetCollisionCallback (dgCollisionMeshCollisionCallback debugCallback)
{
_ASSERTE (0);
/*
	for (dgList<dgProxy>::dgListNode* node = m_list.GetFirst(); node; node = node->GetNext()) {
		const dgCollisionScene::dgProxy& entry = node->GetInfo();
		if (entry.m_shape->IsType (dgCollisionMesh_RTTI)) {
			dgCollisionMesh* mesh;
			mesh = (dgCollisionMesh*)entry.m_shape;
			mesh->SetCollisionCallback (debugCallback);
		}
	}
*/
}




void dgCollisionScene::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const
{
	dgVector origin (matrix.TransformVector(m_boxOrigin));
	dgVector size (m_boxSize.m_x * dgAbsf(matrix[0][0]) + m_boxSize.m_y * dgAbsf(matrix[1][0]) + m_boxSize.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,  
				   m_boxSize.m_x * dgAbsf(matrix[0][1]) + m_boxSize.m_y * dgAbsf(matrix[1][1]) + m_boxSize.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,  
				   m_boxSize.m_x * dgAbsf(matrix[0][2]) + m_boxSize.m_y * dgAbsf(matrix[1][2]) + m_boxSize.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
				   dgFloat32 (0.0f));

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

#ifdef DG_DEBUG_AABB
	dgInt32 i;
	dgVector q0;
	dgVector q1;
	dgMatrix trans (matrix.Transpose());
	for (i = 0; i < 3; i ++) {
		q0[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i].Scale (-1.0f)))[i];
		q1[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i]))[i];
	}

	dgVector err0 (p0 - q0);
	dgVector err1 (p1 - q1);
	dgFloat32 err; 
	err = GetMax (size.m_x, size.m_y, size.m_z) * 0.5f; 
	_ASSERTE ((err0 % err0) < err);
	_ASSERTE ((err1 % err1) < err);
#endif
}

void dgCollisionScene::CalcAABBSimd(const dgMatrix &matrix,	dgVector &p0, dgVector &p1) const
{
	CalcAABB(matrix, p0, p1);
}







void dgCollisionScene::DebugCollision (const dgMatrix& matrix, OnDebugCollisionMeshCallback callback, void* const userData) const
{
	for (dgList<dgProxy*>::dgListNode* node = m_list.GetFirst(); node; node = node->GetNext()) {
		const dgCollisionScene::dgProxy* entry = node->GetInfo();
		dgMatrix proxyMatrix (entry->m_matrix * matrix);
		entry->m_shape->DebugCollision (entry->m_shape->GetOffsetMatrix() * proxyMatrix, callback, userData);
	}
}


dgFloat32 dgCollisionScene::GetVolume () const
{
	_ASSERTE (0);
	return dgFloat32 (0.0f);
}


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

dgFloat32 dgCollisionScene::GetBoxMinRadius () const 
{
	return dgFloat32 (0.0f);
}

dgFloat32 dgCollisionScene::GetBoxMaxRadius () const 
{
	return dgFloat32 (0.0f);
}

void dgCollisionScene::SetCollisionBBox (const dgVector& p0, const dgVector& p1)
{
	_ASSERTE (p0.m_x <= p1.m_x);
	_ASSERTE (p0.m_y <= p1.m_y);
	_ASSERTE (p0.m_z <= p1.m_z);

	m_boxSize = (p1 - p0).Scale (dgFloat32 (0.5f)); 
	m_boxOrigin = (p1 + p0).Scale (dgFloat32 (0.5f)); 
}

void dgCollisionScene::CalculateInertia (dgVector& inertia, dgVector& origin) const
{
	inertia.m_x = dgFloat32 (0.0f);
	inertia.m_y = dgFloat32 (0.0f);
	inertia.m_z = dgFloat32 (0.0f);

	origin.m_x = dgFloat32 (0.0f);
	origin.m_y = dgFloat32 (0.0f);
	origin.m_z = dgFloat32 (0.0f);
}

dgVector dgCollisionScene::CalculateVolumeIntegral (const dgMatrix& globalMatrix, GetBuoyancyPlane bouyancyPlane, void* const context) const
{
	_ASSERTE (0);
	return dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}


void dgCollisionScene::GetCollisionInfo(dgCollisionInfo* info) const
{
	dgCollision::GetCollisionInfo(info);
	info->m_offsetMatrix = dgGetIdentityMatrix();
	info->m_sceneCollision.m_childrenProxyCount = m_list.GetCount();
}


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

dgVector dgCollisionScene::SupportVertex (const dgVector& dir) const
{
	_ASSERTE (0);
	return dgVector (0, 0, 0, 0);
}



dgFloat32 dgCollisionScene::RayCastSimd (const dgVector& localP0, const dgVector& localP1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
	const dgNode *stackPool[DG_SCENE_MAX_STACK_DEPTH];
	if (!m_rootNode) {
		return dgFloat32 (1.2f);
	}

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

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

		if (ray.BoxTestSimd (me->m_minBox, me->m_maxBox)) {
			if (!me->m_left) {
				_ASSERTE (!me->m_right);
				dgContactPoint tmpContactOut;
				const dgProxy* const proxy = (dgProxy*) me;
				dgVector l0 (proxy->m_matrix.UntransformVector (localP0));
				dgVector l1 (proxy->m_matrix.UntransformVector (localP1));
				dgFloat32 param = proxy->m_shape->RayCastSimd (l0, l1, tmpContactOut, preFilter, body, userData);
				_ASSERTE (param >= dgFloat32 (0.0f));
				if (param < maxParam) {
					contactOut.m_normal = proxy->m_matrix.RotateVectorSimd(tmpContactOut.m_normal);
					maxParam = param;
					ray.Reset (maxParam);
				}
			} else {
				_ASSERTE (me->m_left);
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNode*));
				stackPool[stack] = me->m_left;
				stack++;

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

}




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

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

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

//int xxx = 0;
	FastRayTest ray (localP0, localP1);
	while (stack) {
		stack --;
		const dgNode* const me = stackPool[stack];

//xxx ++;
		if (ray.BoxTest (me->m_minBox, me->m_maxBox)) {
			if (!me->m_left) {
				_ASSERTE (!me->m_right);
				dgContactPoint tmpContactOut;
				const dgProxy* const proxy = (dgProxy*) me;
				dgVector l0 (proxy->m_matrix.UntransformVector (localP0));
				dgVector l1 (proxy->m_matrix.UntransformVector (localP1));
				dgFloat32 param = proxy->m_shape->RayCast (l0, l1, tmpContactOut, preFilter, body, userData);
				_ASSERTE (param >= dgFloat32 (0.0f));
				if (param < maxParam) {
					contactOut.m_normal = proxy->m_matrix.RotateVector(tmpContactOut.m_normal);
					maxParam = param;
					ray.Reset (maxParam);
				}
			} else {
				_ASSERTE (me->m_left);
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNode*));
				stackPool[stack] = me->m_left;
				stack++;

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

void dgCollisionScene::CollidePairSimd (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy) const
{
	const dgNode *stackPool[DG_SCENE_MAX_STACK_DEPTH];

	_ASSERTE (pair->m_body1->GetCollision() == this);
	_ASSERTE (pair->m_body1->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI));

	dgVector p0;
	dgVector p1;
	_ASSERTE (m_world == pair->m_body1->GetWorld());
	dgMatrix matrix (pair->m_body0->m_matrix * pair->m_body1->m_matrix.Inverse());
	pair->m_body0->GetCollision()->CalcAABBSimd (matrix, p0, p1);

	dgInt32 stack = 1;
	stackPool[0] = m_rootNode;
	while (stack) {
		stack --;
		const dgNode* const me = stackPool[stack];

		if (dgOverlapTestSimd (me->m_minBox, me->m_maxBox, p0, p1)) {

			if (!me->m_left) {
				_ASSERTE (!me->m_right);
				const dgProxy* const sceneProxy = (dgProxy*) me;
				m_world->SceneContactsSimd (*sceneProxy, pair, proxy);
			} else {
				_ASSERTE (me->m_left);
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNode*));
				stackPool[stack] = me->m_left;
				stack++;

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

}

void dgCollisionScene::CollidePair (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy) const
{
	const dgNode *stackPool[DG_SCENE_MAX_STACK_DEPTH];

	_ASSERTE (pair->m_body1->GetCollision() == this);
	_ASSERTE (pair->m_body1->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI));

	dgVector p0;
	dgVector p1;
	_ASSERTE (m_world == pair->m_body1->GetWorld());
	dgMatrix matrix (pair->m_body0->m_matrix * pair->m_body1->m_matrix.Inverse());
	pair->m_body0->GetCollision()->CalcAABB (matrix, p0, p1);

	dgInt32 stack = 1;
	stackPool[0] = m_rootNode;
	while (stack) {
		stack --;
		const dgNode* const me = stackPool[stack];

		if (dgOverlapTest (me->m_minBox, me->m_maxBox, p0, p1)) {

			if (!me->m_left) {
				_ASSERTE (!me->m_right);
				const dgProxy* const sceneProxy = (dgProxy*) me;
				m_world->SceneContacts (*sceneProxy, pair, proxy);
			} else {
				_ASSERTE (me->m_left);
				_ASSERTE (stack < sizeof (stackPool) / sizeof (dgNode*));
				stackPool[stack] = me->m_left;
				stack++;

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


void dgCollisionScene::ImproveNodeFitness (dgNode* const node)
{
	_ASSERTE (node->m_left);
	_ASSERTE (node->m_right);

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

			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)) {
				dgNode* const parent = node->m_parent;
				node->m_minBox = parent->m_minBox;
				node->m_maxBox = parent->m_maxBox;
				node->m_surfaceArea = parent->m_surfaceArea; 
				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;
					}
				} else {
					m_rootNode = 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_minBox = cost1P0;
				parent->m_maxBox = cost1P1;		
				parent->m_surfaceArea = cost1;


			} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
				dgNode* const parent = node->m_parent;
				node->m_minBox = parent->m_minBox;
				node->m_maxBox = parent->m_maxBox;
				node->m_surfaceArea = parent->m_surfaceArea; 

				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;
					}
				} else {
					m_rootNode = 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_minBox = cost2P0;
				parent->m_maxBox = cost2P1;		
				parent->m_surfaceArea = cost2;
			}
		} else {
			dgFloat32 cost0 = node->m_surfaceArea;

			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)) {

				dgNode* const parent = node->m_parent;
				node->m_minBox = parent->m_minBox;
				node->m_maxBox = parent->m_maxBox;
				node->m_surfaceArea = parent->m_surfaceArea; 
				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;
					}
				} else {
					m_rootNode = 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_minBox = cost1P0;
				parent->m_maxBox = cost1P1;		
				parent->m_surfaceArea = cost1;

			} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
				dgNode* const parent = node->m_parent;
				node->m_minBox = parent->m_minBox;
				node->m_maxBox = parent->m_maxBox;
				node->m_surfaceArea = parent->m_surfaceArea; 
				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;
					}
				} else {
					m_rootNode = 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_minBox = cost2P0;
				parent->m_maxBox = cost2P1;		
				parent->m_surfaceArea = cost2;
			}
		}
	}

	_ASSERTE (!m_rootNode->m_parent);
}



void dgCollisionScene::ImproveTotalFitness()
{
	dgInt32 maxPasses = 2 * exp_2 (m_fitnessList.GetCount()) + 1;

	dgFloat64 newCost = dgFloat32 (1.0e20f);
	dgFloat64 prevCost = newCost;
	do {
		prevCost = newCost;
		for (dgList<dgNode*>::dgListNode* node = m_fitnessList.GetFirst(); node; node = node->GetNext()) {
			ImproveNodeFitness (node->GetInfo());
		}

		newCost	= dgFloat32 (0.0f);
		for (dgList<dgNode*>::dgListNode* node = m_fitnessList.GetFirst(); node; node = node->GetNext()) {
			newCost += node->GetInfo()->m_surfaceArea;
		}
		maxPasses --;
	} while (maxPasses && (newCost < prevCost));

	SetCollisionBBox (m_rootNode->m_minBox, m_rootNode->m_maxBox);
}

dgFloat32 dgCollisionScene::CalculateSurfaceArea (const dgNode* const node0, const dgNode* const node1, dgVector& minBox, dgVector& maxBox) const
{
	minBox = dgVector (GetMin (node0->m_minBox.m_x, node1->m_minBox.m_x), GetMin (node0->m_minBox.m_y, node1->m_minBox.m_y), GetMin (node0->m_minBox.m_z, node1->m_minBox.m_z), dgFloat32 (0.0f));
	maxBox = dgVector (GetMax (node0->m_maxBox.m_x, node1->m_maxBox.m_x), GetMax (node0->m_maxBox.m_y, node1->m_maxBox.m_y), GetMax (node0->m_maxBox.m_z, node1->m_maxBox.m_z), dgFloat32 (0.0f));		
	dgVector side0 (maxBox - minBox);
	dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f));
	return side0 % side1;
}


void* dgCollisionScene::GetFirstProxy () const
{
	return m_list.GetFirst();
}

void* dgCollisionScene::GetNextProxy (void* const proxy) const
{
	dgList<dgProxy*>::dgListNode* const node = (dgList<dgProxy*>::dgListNode*) proxy;
	return node->GetNext();
}


void* dgCollisionScene::AddProxy (dgCollision* const shape, const dgMatrix& matrix)
{
	shape->AddRef();
	dgProxy* const newNode = new (m_world->GetAllocator()) dgProxy (shape, matrix, this);
	newNode->m_myNode = m_list.Append(newNode);

	if (!m_rootNode) {
		m_rootNode = newNode;
	} else {
		dgVector p0;
		dgVector p1;		
		dgNode* sibling = m_rootNode;
		dgFloat32 surfaceArea = CalculateSurfaceArea (newNode, sibling, p0, p1);
		while(sibling->m_left && sibling->m_right) {
			if (surfaceArea > sibling->m_surfaceArea) {
				break;
			} 

			sibling->m_minBox = p0;
			sibling->m_maxBox = p1;
			sibling->m_surfaceArea = surfaceArea;

			dgVector leftP0;
			dgVector leftP1;		
			dgFloat32 leftSurfaceArea = CalculateSurfaceArea (newNode, sibling->m_left, leftP0, leftP1);

			dgVector rightP0;
			dgVector rightP1;		
			dgFloat32 rightSurfaceArea = CalculateSurfaceArea (newNode, sibling->m_right, rightP0, rightP1);

			if (leftSurfaceArea < rightSurfaceArea) {
				sibling = sibling->m_left;
				p0 = leftP0;
				p1 = leftP1;
				surfaceArea = leftSurfaceArea;
			} else {
				sibling = sibling->m_right;
				p0 = rightP0;
				p1 = rightP1;
				surfaceArea = rightSurfaceArea;
			}
		} 

		dgNode* const parent = new (m_world->GetAllocator()) dgNode (sibling, newNode);
		parent->m_fitnessNode = m_fitnessList.Append (parent);

		if (!parent->m_parent) {
			m_rootNode = parent;
		}
	}

	return newNode->m_myNode;
}

void dgCollisionScene::RemoveProxy (void* const proxy)
{
	dgList<dgProxy*>::dgListNode* const node = (dgList<dgProxy*>::dgListNode*) proxy;

	dgProxy* const treeNode = node->GetInfo();
	m_world->ReleaseCollision (treeNode->m_shape);

	m_list.Remove (node);
	
	if (!treeNode->m_parent) {
		delete (m_rootNode);
		m_rootNode = NULL;
	} else if (!treeNode->m_parent->m_parent) {
		m_fitnessList.Remove(treeNode->m_parent->m_fitnessNode);

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

	} else {
		m_fitnessList.Remove(treeNode->m_parent->m_fitnessNode);
		dgNode* 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);
	}
}