/* 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 "dgCollisionConvex.h"
#include "dgWorldDynamicUpdate.h"
#include "dgBroadPhaseCollision.h"


dgSortArray::dgSortArray()
	:dgList<dgSortArrayEntry>(NULL)
{
	m_index = 0;
	m_isSorted = 0;
}

dgSortArray::~dgSortArray ()
{
	_ASSERTE (GetCount() == 0);
}


void dgSortArray::Add (dgBody* const body)
{
	m_isSorted = 0;
	dgFloat32 val = body->m_minAABB[m_index];
	dgListNode* const node = Append ();
	node->GetInfo().m_key = val;
	node->GetInfo().m_body = body;
	body->m_collisionCell.m_axisArrayNode[dgInt32(m_index)] = node;
}


void dgSortArray::Remove (dgBody* const body)
{
	dgListNode* const node = (dgListNode*) body->m_collisionCell.m_axisArrayNode[dgInt32(m_index)];
	_ASSERTE (node);

	dgList<dgSortArrayEntry>::Remove (node);
	body->m_collisionCell.m_axisArrayNode[dgInt32(m_index)] = NULL;
}


dgFloat32 dgSortArray::Sort ()
{
//	dgFloat32 sum;
//	dgFloat32 sum2;

	m_isSorted = 1;

	dgFloat32 sum = GetFirst()->GetInfo().m_body->m_minAABB[m_index];
	dgFloat32 sum2 = sum * sum;
	GetFirst()->GetInfo().m_key = sum;
	for (dgListNode* node = GetFirst()->GetNext(); node; ) { 
//		dgBody* body;
//		dgFloat32 key;
		dgListNode* prev;
//		dgListNode* entry;

		dgListNode* const entry = node;
		node = node->GetNext();
		dgBody* const body = entry->GetInfo().m_body;
		dgFloat32 key = body->m_minAABB[m_index];
		entry->GetInfo().m_key = key;
		
		sum += key;
		sum2 += key * key;
		for (prev = entry->GetPrev(); prev && (key < prev->GetInfo().m_key) ; prev = prev->GetPrev()) {
		}

		if (!prev) {
			RotateToBegin (entry);
		} else {
			InsertAfter (prev, entry);
		}
	}
	//	_ASSERTE ((GetCount() * sum2 - sum * sum) > dgFloat32 (-10.0f));
	return GetCount() * sum2 - sum * sum;
}


dgFloat32 dgSortArray::RayCast (
	dgFloat32 minT, 
	const dgLineBox& line,
	OnRayCastAction filter, 
	OnRayPrecastAction prefilter, 
	void* const userData) const
{
	if (m_isSorted) {
//		dgFloat32 minVal = line.m_boxL0[m_index];
		dgFloat32 maxVal = line.m_boxL1[m_index];
		_ASSERTE (line.m_boxL0[m_index] <= maxVal);

		for (dgListNode* node = GetFirst(); node && (node->GetInfo().m_key < maxVal); node = node->GetNext() ) {
			minT = node->GetInfo().m_body->RayCast (line, filter, prefilter, userData, minT);
		}
	} else {
		for (dgListNode* node = GetFirst(); node; node = node->GetNext() ) {
			minT = node->GetInfo().m_body->RayCast (line, filter, prefilter, userData, minT);
		}
	}

	return minT;
}


void dgSortArray::InvalidateCache ()
{
	for (dgListNode* node = GetFirst()->GetNext(); node; ) { 
//		dgInt32 key;
		dgListNode* prev;
//		dgListNode* entry;
	
		dgListNode* const entry = node;
		node = node->GetNext();
		dgInt32 key = entry->GetInfo().m_body->m_uniqueID;
		for (prev = entry->GetPrev(); prev && (prev->GetInfo().m_body->m_uniqueID > key) ; prev = prev->GetPrev()) {
		}

		if (!prev) {
			RotateToBegin (entry);
		} else {
			InsertAfter (prev, entry);
		}
	}
}


bool dgSortArray::SanityCheck()
{
	dgFloat32 val;
	dgListNode* node;

	val = GetFirst()->GetInfo().m_key;
	for (node = GetFirst()->GetNext(); node; node = node->GetNext()) {
		if (val > node->GetInfo().m_key) {
			return false;
		}
		val = node->GetInfo().m_key;
	}
	return true;
}


dgBroadPhaseCell::dgBroadPhaseCell ()
{
}

dgBroadPhaseCell::~dgBroadPhaseCell()
{
	_ASSERTE (m_count == 0);
}

void dgBroadPhaseCell::Init (dgInt32 layer, dgMemoryAllocator* allocator)
{
	m_count = 0;
	m_active = 0;
	m_layerIndex = dgInt8 (layer);
	m_sort[0].m_index = 0;
	m_sort[1].m_index = 1;
	m_sort[2].m_index = 2;
	m_sort[0].SetAllocator(allocator);
	m_sort[1].SetAllocator(allocator);
	m_sort[2].SetAllocator(allocator);

	m_lastSortArray = &m_sort[0];
}


void dgBroadPhaseCell::Add (dgBody* const body)
{
	m_count ++;
	m_active = 1;

	_ASSERTE (!body->m_collisionCell.m_cell);

	m_sort[0].Add (body);
	m_sort[1].Add (body);	
	m_sort[2].Add (body);	
	body->m_collisionCell.m_cell = this;
}

void dgBroadPhaseCell::Remove (dgBody* const body)
{
	m_count --;
	m_active = 1;

	_ASSERTE (m_count >= 0);
	_ASSERTE (body->m_collisionCell.m_cell);

	m_sort[0].Remove(body);
	m_sort[1].Remove(body);
	m_sort[2].Remove(body);
	body->m_collisionCell.m_cell = NULL;
}


void dgBroadPhaseCell::Sort ()
{
//	dgInt32 axis;
//	dgFloat32 maxVariance;
	dgFloat32 variance[3];

	variance[0] = m_sort[0].Sort();
	variance[1] = m_sort[1].Sort();
	variance[2] = m_sort[2].Sort();

	dgInt32 axis = 0;
	dgFloat32 maxVariance = variance[0];
	for (dgInt32 i = 1; i < 3; i ++) {
		if (variance[i] > maxVariance) {
			axis = i;
			maxVariance = variance[i];
		}
	}
	m_lastSortArray = &m_sort[axis];
}


void dgBroadPhaseCell::UpdateAutoPair(dgWorld * const world, dgInt32 threadIndex)
{
//	dgInt32 indexX;
//	dgFloat32 maxVal;
//	dgBody *body0;
//	dgBody *body1;
//	dgSortArray* lastSort;
//	dgSortArray::dgListNode* outerNode;
//	dgSortArray::dgListNode* innerNode;

	dgSortArray* const lastSort = m_lastSortArray;
	dgInt32 indexX = lastSort->m_index;
	dgCollidingPairCollector& contactPair = *world;

	for (dgSortArray::dgListNode* outerNode = lastSort->GetFirst(); outerNode; outerNode = outerNode->GetNext() ) {
		dgBody *const body0 = outerNode->GetInfo().m_body;
		if (!body0->m_collision->IsType (dgCollision::dgCollisionNull_RTTI)) {
			dgFloat32 maxVal = body0->m_maxAABB[indexX];

			for (dgSortArray::dgListNode* innerNode = outerNode->GetNext(); innerNode && innerNode->GetInfo().m_key <= maxVal; innerNode = innerNode->GetNext() ) {
				dgBody * const body1 = innerNode->GetInfo().m_body;
				if (!body1->m_collision->IsType (dgCollision::dgCollisionNull_RTTI)) {
					_ASSERTE (body0 != body1);
					if (OverlapTest(body0, body1)) {
						contactPair.AddPair(body0, body1, threadIndex);
					}
				}
			}
		}
	}
}



dgBroadPhaseLayer::dgBroadPhaseLayer()
	:dgTree<dgBroadPhaseCell, dgUnsigned32>(NULL)
{
	m_me = NULL;
	m_cellSize = dgFloat32 (0.0f);
	m_invCellSize = dgFloat32 (0.0f);
}

dgBroadPhaseLayer::~dgBroadPhaseLayer()
{
	_ASSERTE (!GetCount());
}

void dgBroadPhaseLayer::Init (dgWorld* const world, dgFloat32 cellSize, dgInt32 layerIndex)
{
	_ASSERTE (GetCount() == 0);
	m_me = world;
	m_layerIndex = dgInt16(layerIndex);
	m_cellSize = cellSize;

	m_invCellSize = dgFloat32 (1.0f) / m_cellSize;
}

dgBroadPhaseCell* dgBroadPhaseLayer::FindCreate (dgInt32 x, dgInt32 z)
{
	dgUnsigned32 key = GetKey (x, z);
	dgTreeNode* node = dgTree<dgBroadPhaseCell, dgUnsigned32>::Find (key);
	if (!node) {
		dgBroadPhaseCell cell;
		cell.m_count = 0;
		node = Insert(cell, key);
		node->GetInfo().Init (m_layerIndex, m_me->GetAllocator());
	}

	 return &node->GetInfo();
}



dgBroadPhaseCollision::dgBroadPhaseCollision(dgMemoryAllocator* allocator)
	:m_min (-dgFloat32(1000.0f), -dgFloat32(1000.0f), -dgFloat32(1000.0f), dgFloat32(0.0f)),
	 m_max ( dgFloat32(1000.0f),  dgFloat32(1000.0f), dgFloat32(1000.0f), dgFloat32(0.0f)), 
	 m_appMinBox (-dgFloat32(1000.0f), -dgFloat32(1000.0f), -dgFloat32(1000.0f), dgFloat32(0.0f)),
	 m_appMaxBox ( dgFloat32(1000.0f),  dgFloat32(1000.0f), dgFloat32(1000.0f), dgFloat32(0.0f)) 
{
//	m_me = NULL;	
	m_inactiveList.Init(0, allocator);

	for (dgInt32 i = 0; i < DG_OCTREE_MAX_DEPTH; i ++) {
		m_layerMap[i].SetAllocator(allocator);
	}

}

dgBroadPhaseCollision::~dgBroadPhaseCollision()
{
}

void dgBroadPhaseCollision::Init()
{
//	m_me = me;
	m_worlSize = dgFloat32 (0.0f);

	dgVector p0 (m_min);
	dgVector p1 (m_max);
	SetWorldSize (p0, p1);
}

void dgBroadPhaseCollision::GetWorldSize (dgVector& p0, dgVector& p1) const
{
	p0 = m_appMinBox;
	p1 = m_appMaxBox;
}

void dgBroadPhaseCollision::SetWorldSize (const dgVector& min, const dgVector& max)
{
	dgFloat32 cellSize;

	// remove all bodies for the map
	dgBodyMasterList& masterList (*((dgWorld*)this));
	for (dgBodyMasterList::dgListNode* node = masterList.GetFirst(); node; node = node->GetNext()) { 
		dgBody* body;
		body = node->GetInfo().GetBody();
		Remove(body);
	}

	m_appMinBox = min;
	m_appMaxBox = max;


	// recalculate new map definitions
	m_min = min;
	m_max = max;

	m_min.m_x = dgFloor (m_min.m_x / dgFloat32 (1 << DG_OCTREE_MAX_DEPTH)) * dgFloat32 (1 << DG_OCTREE_MAX_DEPTH);
	m_min.m_y = dgFloor (m_min.m_y / dgFloat32 (1 << DG_OCTREE_MAX_DEPTH)) * dgFloat32 (1 << DG_OCTREE_MAX_DEPTH);
	m_min.m_z = dgFloor (m_min.m_z / dgFloat32 (1 << DG_OCTREE_MAX_DEPTH)) * dgFloat32 (1 << DG_OCTREE_MAX_DEPTH);

	m_max.m_x = dgCeil (m_max.m_x / dgFloat32 (1 << DG_OCTREE_MAX_DEPTH)) * dgFloat32 (1 << DG_OCTREE_MAX_DEPTH);
	m_max.m_y = dgCeil (m_max.m_y / dgFloat32 (1 << DG_OCTREE_MAX_DEPTH)) * dgFloat32 (1 << DG_OCTREE_MAX_DEPTH);
	m_max.m_z = dgCeil (m_max.m_z / dgFloat32 (1 << DG_OCTREE_MAX_DEPTH)) * dgFloat32 (1 << DG_OCTREE_MAX_DEPTH);

	dgVector size (m_max - m_min); 
	cellSize = GetMax (GetMax (size[0], size[2]), size[1]);
	cellSize = dgPow (dgFloat32 (2.0f), dgCeil (dgLog (cellSize) / dgLog (dgFloat32 (2.0f))));

	for (dgInt32 i = 0; i < DG_OCTREE_MAX_DEPTH; i ++) {
		cellSize *= dgFloat32 (0.5f);
	}
	//	cellSize = dgCeil (cellSize * (1 << DG_OCTREE_MAX_DEPTH)) /  dgFloat32 (1 << DG_OCTREE_MAX_DEPTH);

	m_worlSize = cellSize;
	for (dgInt32 i = 0; i < DG_OCTREE_MAX_DEPTH; i ++) {
		m_worlSize *= dgFloat32 (2.0f);
	}

	dgWorld* const me = (dgWorld*)this;
	m_inactiveList.Init(0, me->GetAllocator());
	cellSize = m_worlSize; 
	for (dgInt32 i = 0; i < DG_OCTREE_MAX_DEPTH; i ++) {
		_ASSERTE (m_layerMap[i].GetCount() == 0);
		m_layerMap[i].Init(me, cellSize, i);
		cellSize *= dgFloat32 (0.5f);
	}

	// first first cell to layer zero
	for (dgBodyMasterList::dgListNode* node = masterList.GetFirst(); node; node = node->GetNext()) { 
		dgBody* body;
		body = node->GetInfo().GetBody();
		Add(body);
		body->SetMatrix(body->GetMatrix());
	}

	m_boxSize = m_max - m_min;
}

void dgBroadPhaseCollision::InvalidateCache ()
{
/*
	dgBodyMasterList& masterList (*m_me);
	for (dgBodyMasterList::dgListNode* node = masterList.GetFirst(); node; node = node->GetNext()) { 
		dgBody* body;
		body = node->GetInfo().GetBody();
		Remove(body);

		// invalidate AABB so next add can promote the body to the correct CELL.
		body->m_minAABB = dgVector ( 1.0e10f,  1.0e10f,  1.0e10f, 0.0f);
		body->m_maxAABB = dgVector (-1.0e10f, -1.0e10f, -1.0e10f, 0.0f);
	}

	for (dgBodyMasterList::dgListNode* node = masterList.GetFirst(); node; node = node->GetNext()) { 
		dgBody* body;
		body = node->GetInfo().GetBody();
		Add(body);
		body->SetMatrix(body->GetMatrix());
	}
*/

	for (int layer = 0; layer < DG_OCTREE_MAX_DEPTH; layer ++) {
		dgBroadPhaseLayer::Iterator iter (m_layerMap[layer]);
		for (iter.Begin(); iter; iter ++) {
			dgBroadPhaseCell *cell;
			cell = &iter.GetNode()->GetInfo();

			cell->m_active = 1;
			cell->m_lastSortArray = &cell->m_sort[0];
//			cell->m_sort[0].InvalidateCache();
//			cell->m_sort[1].InvalidateCache();;
//			cell->m_sort[2].InvalidateCache();
		}
	}
}

void dgBroadPhaseCollision::Add (dgBody* const body)
{
	_ASSERTE (!body->m_collisionCell.m_cell);
	// new bodies are added to the root node, and the function set matrix relocate them
	m_layerMap[0].FindCreate (0, 0)->Add (body);
}


void dgBroadPhaseCollision::Remove (dgBody* const body)
{
//	dgBroadPhaseCell* obtreeCell;
//	dgBroadPhaseLayer::dgTreeNode* node;

	_ASSERTE (body->m_collisionCell.m_cell);
	dgBroadPhaseCell* const obtreeCell = body->m_collisionCell.m_cell;
	obtreeCell->Remove (body);

	if (!obtreeCell->m_count) {
		if (obtreeCell != &m_inactiveList) {
			dgBroadPhaseLayer::dgTreeNode* const node = m_layerMap[dgInt32(obtreeCell->m_layerIndex)].GetNodeFromInfo(*obtreeCell);
			_ASSERTE (node);
			m_layerMap[dgInt32(obtreeCell->m_layerIndex)].Remove(node);
		}
	}
}


void dgBroadPhaseCollision::UpdatePairs (dgBody* const body0, dgSortArray::dgListNode* const srcnode, dgInt32 axisX, dgInt32 threadIndex) const
{
//	dgFloat32 val;
//	dgBody* body1;

	if (!body0->m_collision->IsType (dgCollision::dgCollisionNull_RTTI)) {
		dgFloat32 val = body0->m_maxAABB[axisX];
		dgCollidingPairCollector& contactPair = *((dgWorld*)this);
		for (dgSortArray::dgListNode* node = srcnode; node && (node->GetInfo().m_key < val); node = node->GetNext() ) {
			dgBody* const body1 = node->GetInfo().m_body;
			if (!body1->m_collision->IsType (dgCollision::dgCollisionNull_RTTI)) {
				_ASSERTE (body0 != body1);
				if (OverlapTest(body0, body1)) {
					contactPair.AddPair(body0, body1, threadIndex);
				}
			}
		}
	}
}

void dgBroadPhaseCollision::UpdatePairs (dgBroadPhaseCell& cellA, dgBroadPhaseCell& cellB, dgInt32 threadIndex) const
{
	dgInt32 axisX = cellA.m_lastSortArray->m_index;
	dgSortArray* const listA = &cellA.m_sort[axisX];
	dgSortArray* const listB = &cellB.m_sort[axisX];

	dgSortArray::dgListNode* nodeA = listA->GetFirst();
	dgSortArray::dgListNode* nodeB = listB->GetFirst();
	while(nodeA && nodeB) {
		if (nodeA->GetInfo().m_key < nodeB->GetInfo().m_key) {
			UpdatePairs (nodeA->GetInfo().m_body, nodeB, axisX, threadIndex);
			nodeA = nodeA->GetNext();
		} else {
			UpdatePairs (nodeB->GetInfo().m_body, nodeA, axisX, threadIndex);
			nodeB = nodeB->GetNext();
		}
	} 
}


void dgBroadPhaseCollision::ForEachBodyInAABB (const dgVector& p0, const dgVector& p1, OnBodiesInAABB callback, void* const userdata) const
{
	if (dgOverlapTest (p0, p1, m_appMinBox, m_appMaxBox)) {
		dgBody* const sentinel = ((dgWorld*)this)->GetSentinelBody();
		dgFloat32 x0 = GetMax (p0.m_x - m_min.m_x, dgFloat32 (0.0f));
//		dgFloat32 y0 = GetMax (p0.m_y - m_min.m_y, dgFloat32 (0.0f));
		dgFloat32 z0 = GetMax (p0.m_z - m_min.m_z, dgFloat32 (0.0f));
		dgFloat32 x1 = GetMin (p1.m_x - m_min.m_x, m_worlSize * dgFloat32 (0.999f));
//		dgFloat32 y1 = GetMin (p1.m_y - m_min.m_y, m_worlSize * dgFloat32 (0.999f));
		dgFloat32 z1 = GetMin (p1.m_z - m_min.m_z, m_worlSize * dgFloat32 (0.999f));
		for (dgInt32 layer = 0; layer < DG_OCTREE_MAX_DEPTH; layer ++) {

			const dgBroadPhaseLayer& layerMap = m_layerMap[layer];
			if (layerMap.GetCount()) {
				dgFloat32 cellScale = layerMap.m_invCellSize;
				dgInt32 ix0 = dgFastInt (x0 * cellScale);
				dgInt32 ix1 = dgFastInt (x1 * cellScale);
				for (dgInt32 xIndex = ix0; xIndex <= ix1; xIndex ++) {
					dgInt32 iz0 = dgFastInt (z0 * cellScale);
					dgInt32 iz1 = dgFastInt (z1 * cellScale);
					for (dgInt32 zIndex = iz0; zIndex <= iz1; zIndex ++) {
						dgBroadPhaseCell *const cell = layerMap.Find (xIndex, zIndex);
						if (cell) {
							for (dgSortArray::dgListNode *node = cell->m_sort[0].GetFirst(); node; node = node->GetNext()) {
								dgBody* const body = node->GetInfo().m_body;
								if (dgOverlapTest (body->m_minAABB, body->m_maxAABB, p0, p1)) {
									if (body != sentinel) {
										callback (body, userdata);
									}
								}
							}
						}
					}
				}
			}
		}
	}
}

dgInt32 dgBroadPhaseCollision::ConvexCast (
	dgCollision* const shape, 
	const dgMatrix& matrixOrigin, 
	const dgVector& target, 
	dgFloat32& timeToImpact,
	OnRayPrecastAction prefilter, 
	void* const userData, 
	dgConvexCastReturnInfo* const info,
	dgInt32 maxContacts,
	dgInt32 threadIndex) const
{
	dgVector p0;
	dgVector p1;
	dgVector q0;
	dgVector q1;
	dgMatrix matrixTarget (matrixOrigin);

	matrixTarget.m_posit = target;
	dgCollisionConvex* const collision = (dgCollisionConvex*)shape;

	collision->CalcAABB (matrixOrigin, p0, p1);
	collision->CalcAABB (matrixTarget, q0, q1);
	p0.m_x = GetMin (p0.m_x, q0.m_x);
	p0.m_y = GetMin (p0.m_y, q0.m_y);
	p0.m_z = GetMin (p0.m_z, q0.m_z);
	p1.m_x = GetMax (p1.m_x, q1.m_x);
	p1.m_y = GetMax (p1.m_y, q1.m_y);
	p1.m_z = GetMax (p1.m_z, q1.m_z);

	dgInt32 totalCount = 0;
	timeToImpact = dgFloat32 (1.2f);
	if (dgOverlapTest (p0, p1, m_appMinBox, m_appMaxBox)) {

		#define CONVEX_CAST_POOLSIZE 32
		dgTriplex points[CONVEX_CAST_POOLSIZE]; 
		dgTriplex normals[CONVEX_CAST_POOLSIZE]; 
		dgFloat32 penetration[CONVEX_CAST_POOLSIZE]; 

		if (maxContacts > CONVEX_CAST_POOLSIZE) {
			maxContacts = CONVEX_CAST_POOLSIZE;
		} 
		
		dgWorld* const me = (dgWorld*)this;
		dgInt32 cpu = me->m_cpu;
//		dgBody* const sentinel = me->GetSentinelBody();

		dgFloat32 timestep = 1.2f;
		dgMatrix alignedMatrix (matrixOrigin);
		dgVector velocA (target - matrixOrigin.m_posit);
		dgVector velocB (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));


		dgFloat32 x0 = GetMax (p0.m_x - m_min.m_x, dgFloat32 (0.0f));
//		dgFloat32 y0 = GetMax (p0.m_y - m_min.m_y, dgFloat32 (0.0f));
		dgFloat32 z0 = GetMax (p0.m_z - m_min.m_z, dgFloat32 (0.0f));
		dgFloat32 x1 = GetMin (p1.m_x - m_min.m_x, m_worlSize * dgFloat32 (0.999f));
//		dgFloat32 y1 = GetMin (p1.m_y - m_min.m_y, m_worlSize * dgFloat32 (0.999f));
		dgFloat32 z1 = GetMin (p1.m_z - m_min.m_z, m_worlSize * dgFloat32 (0.999f));
		for (dgInt32 layer = 0; layer < DG_OCTREE_MAX_DEPTH; layer ++) {
			if (m_layerMap[layer].GetCount()) {
				dgFloat32 cellScale = m_layerMap[layer].m_invCellSize;
				dgInt32 ix0 = dgFastInt (x0 * cellScale);
				dgInt32 ix1 = dgFastInt (x1 * cellScale);
				for (dgInt32 xIndex = ix0; xIndex <= ix1; xIndex ++) {
					dgInt32 iz0 = dgFastInt (z0 * cellScale);
					dgInt32 iz1 = dgFastInt (z1 * cellScale);
					for (dgInt32 zIndex = iz0; zIndex <= iz1; zIndex ++) {
						dgBroadPhaseCell *const cell = m_layerMap[layer].Find (xIndex, zIndex);
						if (cell) {
							for (dgSortArray::dgListNode *node = cell->m_sort[0].GetFirst(); node; node = node->GetNext()) {
								const dgBody* const body = node->GetInfo().m_body;
								if (dgOverlapTest (body->m_minAABB, body->m_maxAABB, p0, p1)) {
//									if (body != sentinel) {
									if (!body->m_collision->IsType(dgCollision::dgCollisionNull_RTTI)) {
										if (!PREFILTER_RAYCAST (prefilter, body, collision, userData)) {
											dgInt32 count;
											dgFloat32 time;

											if (cpu == dgSimdPresent) {
												count = me->CollideContinueSimd (collision, alignedMatrix, velocA, velocB,
																				   body->m_collision, body->m_matrix, velocB, velocB,
																				   time, points, normals, penetration, CONVEX_CAST_POOLSIZE, threadIndex);


											} else {
												count = me->CollideContinue (collision, alignedMatrix, velocA, velocB,
																			   body->m_collision, body->m_matrix, velocB, velocB,
																			   time, points, normals, penetration, CONVEX_CAST_POOLSIZE, threadIndex);

											}

											timeToImpact = GetMin (time, timeToImpact);

											if (count) {
												if (time <= timestep) {		
													if ((timestep - time)> dgFloat32 (1.0e-3f)) {
														totalCount = 0;
														timestep = time;
													} 
													if (count >= (maxContacts - totalCount)) {
														count = maxContacts - totalCount;
													}

													for (dgInt32 i = 0; i < count; i ++) {
														info[totalCount].m_hitBody = body;
														info[totalCount].m_point[0] = points[i].m_x;
														info[totalCount].m_point[1] = points[i].m_y;
														info[totalCount].m_point[2] = points[i].m_z;
														info[totalCount].m_normal[0] = normals[i].m_x;
														info[totalCount].m_normal[1] = normals[i].m_y;
														info[totalCount].m_normal[2] = normals[i].m_z;
														info[totalCount].m_penetration = penetration[i];
														info[totalCount].m_contaID = 0;
														totalCount ++;
													}
												}
											}
										}
									}
								}
							}
						}
						
					}
				}
			}
		}


		if (totalCount) {
			#define DG_RAY_TEST_LENGTH  dgFloat32 (0.015625f)
			dgVector dir (velocA.Scale (DG_RAY_TEST_LENGTH * dgRsqrt (velocA % velocA)));
		
			for (dgInt32 i = 0; i < totalCount; i ++) {
//				dgFloat32 t;
				dgContactPoint contact;
				dgVector p0 (info[i].m_point[0], info[i].m_point[1], info[i].m_point[2], dgFloat32 (0.0f));
				dgVector p1 (p0 + dir);
				p0 -= dir;

				const dgMatrix& matrix = info[i].m_hitBody->m_matrix;

				dgVector l0 (matrix.UntransformVector(p0));
				dgVector l1 (matrix.UntransformVector(p1));
				info[i].m_normalOnHitPoint[0] = info[i].m_normal[0];
			
				// bug fixed by thedmd
				//dgFloat32 t = info[i].m_hitBody->m_collision->RayCast (l0, l1, contact, NULL, NULL, NULL);
				dgFloat32 t = info[i].m_hitBody->m_collision->RayCast (l0, l1, contact, NULL, info[i].m_hitBody, NULL);
				if (t >= dgFloat32 (0.0f) && t <= dgFloat32 (dgFloat32(1.0f))) {
					contact.m_normal = matrix.RotateVector (contact.m_normal);
					info[i].m_normalOnHitPoint[0] = contact.m_normal[0]; 
					info[i].m_normalOnHitPoint[1] = contact.m_normal[1]; 
					info[i].m_normalOnHitPoint[2] = contact.m_normal[2]; 
					info[i].m_normalOnHitPoint[3] = dgFloat32 (0.0f);
				} else {
					info[i].m_normalOnHitPoint[0] = info[i].m_normal[0];
					info[i].m_normalOnHitPoint[1] = info[i].m_normal[1];
					info[i].m_normalOnHitPoint[2] = info[i].m_normal[2];
					info[i].m_normalOnHitPoint[3] = dgFloat32 (0.0f);

				}
			}
		}
	}
	return totalCount;
}


void dgBroadPhaseCalculateContactsWorkerThread::Realloc (dgInt32 jointsCount, dgInt32 contactCount, dgInt32 threadIndex)
{
	m_world->dgGetUserLock();

	dgCollidingPairCollector::dgPair *const pairs = m_world->m_pairs;
	dgContactPoint* const contactBuffer = (dgContactPoint*) m_world->m_contactBuffers[threadIndex];
	
	dgInt32 size = m_world->m_contactBuffersSizeInBytes[threadIndex] * 2;
	dgContactPoint* const newBuffer = (dgContactPoint*) m_world->GetAllocator()->MallocLow (size);  
	memcpy (newBuffer, contactBuffer, contactCount * sizeof (dgContactPoint));

	dgInt32 index = 0;
	for (dgInt32 j = 0; j < jointsCount; j += m_step) {
		dgCollidingPairCollector::dgPair& pair = pairs[j + threadIndex];
		if (pair.m_contactBuffer) {
			pair.m_contactBuffer = &newBuffer[index];
			index += pair.m_contactCount;
		}
	}

	_ASSERTE (index == contactCount);

	m_world->GetAllocator()->FreeLow(m_world->m_contactBuffers[threadIndex]);
	m_world->m_contactBuffersSizeInBytes[threadIndex] = size;
	m_world->m_contactBuffers[threadIndex] = newBuffer;

	m_world->dgReleasedUserLock();
}



void dgBroadPhaseCellPairsWorkerThread::ThreadExecute()
{
	dgInt32 step = m_step; 
	dgInt32 count = m_count;
	dgBroadPhaseCollision& broadPhase =  *m_world;
	for (dgInt32 i = 0; i < count; i += step) {
		if(m_pairs[i].m_cell_B) {
			broadPhase.UpdatePairs (*m_pairs[i].m_cell_A, *m_pairs[i].m_cell_B, m_threadIndex);
		} else {
			m_pairs[i].m_cell_A->UpdateAutoPair(m_world, m_threadIndex);
		}
	}
}

void dgBroadPhaseApplyExternalForce::ThreadExecute()
{
	dgInt32 step = m_step; 
	dgInt32 count = m_count;
	dgBody** const bodyArray = m_bodies;

	if (m_skipForceUpdate) {
		if (m_world->m_cpu == dgSimdPresent) {
			for (dgInt32 i = 0; i < count; i += step) {
				dgBody* const body = bodyArray[i];
				_ASSERTE (body->m_collision->IsType(dgCollision::dgConvexCollision_RTTI) || body->m_collision->IsType(dgCollision::dgCollisionCompound_RTTI) ||	body->m_collision->IsType(dgCollision::dgCollisionConvexModifier_RTTI));
				if (!body->IsInEquelibrium()) {
					body->UpdateCollisionMatrixSimd(m_timeStep, m_threadIndex);
				}
			}
		} else {
			for (dgInt32 i = 0; i < count; i += step) {
				dgBody* const body = bodyArray[i];
				_ASSERTE (body->m_collision->IsType(dgCollision::dgConvexCollision_RTTI) || body->m_collision->IsType(dgCollision::dgCollisionCompound_RTTI) ||	body->m_collision->IsType(dgCollision::dgCollisionConvexModifier_RTTI));
				if (!body->IsInEquelibrium()) {
					body->UpdateCollisionMatrixSimd(m_timeStep, m_threadIndex);
				}
			}
		}
	} else {
		if (m_world->m_cpu == dgSimdPresent) {
			for (dgInt32 i = 0; i < count; i += step) {
				dgBody* const body = bodyArray[i];

				body->m_solverInContinueCollision = false;
				_ASSERTE (body->m_invMass.m_w > dgFloat32(0.0f));

				_ASSERTE (body->m_collision->IsType(dgCollision::dgConvexCollision_RTTI) || body->m_collision->IsType(dgCollision::dgCollisionCompound_RTTI) ||	body->m_collision->IsType(dgCollision::dgCollisionConvexModifier_RTTI));

				body->ApplyExtenalForces (m_timeStep, m_threadIndex);
				if (!body->IsInEquelibrium()) {
					body->m_sleeping = false;
					body->m_equilibrium = false;
					body->UpdateCollisionMatrixSimd(m_timeStep, m_threadIndex);
				}
				body->m_prevExternalForce = body->m_accel;
				body->m_prevExternalTorque = body->m_alpha;
			}
		} else {
			for (dgInt32 i = 0; i < count; i += step) {
				dgBody* const body = bodyArray[i];

				body->m_solverInContinueCollision = false;
				_ASSERTE (body->m_invMass.m_w > dgFloat32(0.0f));

				_ASSERTE (body->m_collision->IsType(dgCollision::dgConvexCollision_RTTI) || body->m_collision->IsType(dgCollision::dgCollisionCompound_RTTI) ||	body->m_collision->IsType(dgCollision::dgCollisionConvexModifier_RTTI));

				body->ApplyExtenalForces (m_timeStep, m_threadIndex);
				if (!body->IsInEquelibrium()) {
					body->m_sleeping = false;
					body->m_equilibrium = false;
					body->UpdateCollisionMatrix (m_timeStep, m_threadIndex);
				}
				body->m_prevExternalForce = body->m_accel;
				body->m_prevExternalTorque = body->m_alpha;
			}
		}
	}
}



void dgBroadPhaseCalculateContactsWorkerThread::ThreadExecute()
{
	dgInt32 step = m_step; 
	dgInt32 count = m_count;
	dgCollidingPairCollector::dgPair *const pairs = m_world->m_pairs;

	dgInt32 contactIndex = 0;
	dgInt32 contactSize = dgInt32 (m_world->m_contactBuffersSizeInBytes[m_threadIndex] / sizeof (dgContactPoint));
	dgContactPoint* contactBuffer = (dgContactPoint*) m_world->m_contactBuffers[m_threadIndex];

	if (m_useSimd) {
		for (dgInt32 i = 0; i < count; i += step) {
			dgCollidingPairCollector::dgPair& pair = pairs[i + m_threadIndex];

			if ((contactIndex + DG_MAX_CONTATCS) > contactSize) {
				Realloc (i, contactIndex, m_threadIndex);
				contactSize = dgInt32 (m_world->m_contactBuffersSizeInBytes[m_threadIndex] / sizeof (dgContactPoint));
				contactBuffer = (dgContactPoint*) m_world->m_contactBuffers[m_threadIndex];
			}

			pair.m_contactBuffer = &contactBuffer[contactIndex];
			m_world->CalculateContactsSimd(&pair, m_timestep, m_threadIndex);

			contactIndex += pair.m_contactCount;
			_ASSERTE (contactIndex < contactSize);
		}
	} else {
		for (dgInt32 i = 0; i < count; i += step) {
			dgCollidingPairCollector::dgPair& pair = pairs[i + m_threadIndex];

			if ((contactIndex + DG_MAX_CONTATCS) > contactSize) {
				Realloc (i, contactIndex, m_threadIndex);
				contactSize = dgInt32 (m_world->m_contactBuffersSizeInBytes[m_threadIndex] / sizeof (dgContactPoint));
				contactBuffer = (dgContactPoint*) m_world->m_contactBuffers[m_threadIndex];
			}

			pair.m_contactBuffer = &contactBuffer[contactIndex];
			m_world->CalculateContacts (&pair, m_timestep, m_threadIndex);

			contactIndex += pair.m_contactCount;
			_ASSERTE (contactIndex < contactSize);
		}
	}
}



void dgBroadPhaseMaterialCallbackWorkerThread::ThreadExecute()
{
	dgCollidingPairCollector::dgPair * const pairs = m_pairs;

	dgInt32 step = m_step; 
	dgInt32 count = m_count;

	// now make all contact joints and perform callbacks and joint allocations allocation
	for (dgInt32 i = 0; i < count; i += step) {
		dgCollidingPairCollector::dgPair& pair = pairs[i];

		if (pair.m_contactCount || pair.m_isTrigger) {
			_ASSERTE (pair.m_contactCount <= (DG_CONSTRAINT_MAX_ROWS / 3));
			if (pair.m_isTrigger) {
				m_world->ProcessTriggers (&pair, m_timestep, m_threadIndex);
			} else {
				m_world->ProcessContacts (&pair, m_timestep, m_threadIndex);
			}

		} else if (pair.m_contact) {
			if (!pair.m_contactBuffer) {
				m_world->ProcessCachedContacts (pair.m_contact, pair.m_material, m_timestep, m_threadIndex);
			} else {
				pair.m_contact->m_maxDOF = 0;
			}
		}
	}
}




void dgBroadPhaseCollision::RayCast (
	 const dgVector& l0, 
	 const dgVector& l1, 
	 OnRayCastAction filter, 
	 OnRayPrecastAction prefilter, 
	 void* const userData) const
{
	dgVector ll0 (l0);
	dgVector ll1 (l1);
	dgVector segment (l1 - l0);
	dgFloat32 dist2 = segment % segment;

	if (filter && (dist2 > dgFloat32 (1.0e-8f)) && dgRayBoxClip (ll0, ll1, m_appMinBox, m_appMaxBox)) {
		dgLineBox line;	

		line.m_l0 = l0;
		line.m_l1 = l1;
		if (line.m_l0.m_x <= line.m_l1.m_x) {
			line.m_boxL0.m_x = line.m_l0.m_x;
			line.m_boxL1.m_x = line.m_l1.m_x;
		} else {
			line.m_boxL0.m_x = line.m_l1.m_x;
			line.m_boxL1.m_x = line.m_l0.m_x;
		}

		if (line.m_l0.m_y <= line.m_l1.m_y) {
			line.m_boxL0.m_y = line.m_l0.m_y;
			line.m_boxL1.m_y = line.m_l1.m_y;
		} else {
			line.m_boxL0.m_y = line.m_l1.m_y;
			line.m_boxL1.m_y = line.m_l0.m_y;
		}

		if (line.m_l0.m_z <= line.m_l1.m_z) {
			line.m_boxL0.m_z = line.m_l0.m_z;
			line.m_boxL1.m_z = line.m_l1.m_z;
		} else {
			line.m_boxL0.m_z = line.m_l1.m_z;
			line.m_boxL1.m_z = line.m_l0.m_z;
		}

		dgFloat32 minT = dgFloat32 (1.1f);

		dgVector rayP0 (l0 - m_min);
		dgVector rayP1 (l1 - m_min);

		dgFloat32 xx0 = GetMin (rayP0.m_x, rayP1.m_x) - dgFloat32 (1.0e-3f);
		dgFloat32 zz0 = GetMin (rayP0.m_z, rayP1.m_z) - dgFloat32 (1.0e-3f);
		dgFloat32 xx1 = GetMax (rayP0.m_x, rayP1.m_x) + dgFloat32 (1.0e-3f);
		dgFloat32 zz1 = GetMax (rayP0.m_z, rayP1.m_z) + dgFloat32 (1.0e-3f);
		dgFloat32 yy0 = GetMin (rayP0.m_y, rayP1.m_y) - dgFloat32 (1.0e-3f);
		dgFloat32 yy1 = GetMax (rayP0.m_y, rayP1.m_y) + dgFloat32 (1.0e-3f);


		dgWorld* const me = (dgWorld*) this;
		for (dgInt32 i = 0; i < DG_OCTREE_MAX_DEPTH; i ++) {
			// the user data is the pointer to the collision geometry
			const dgBroadPhaseLayer& layer = m_layerMap[i];

			if (layer.GetCount()) {
				// calculate the ray bounding box
				dgFloat32 scale = layer.m_cellSize;
				dgFloat32 invScale = layer.m_invCellSize;
				dgFloat32 x0 = scale * dgFloor (xx0 * invScale);
				dgFloat32 y0 = scale * dgFloor (yy0 * invScale);
				dgFloat32 z0 = scale * dgFloor (zz0 * invScale);
				dgFloat32 x1 = scale * dgFloor (xx1 * invScale) + scale;
				dgFloat32 y1 = scale * dgFloor (yy1 * invScale) + scale;
				dgFloat32 z1 = scale * dgFloor (zz1 * invScale) + scale;

				dgVector boxP0 (GetMax (x0, dgFloat32 (0.0f)), GetMax (y0, dgFloat32 (0.0f)), GetMax (z0, dgFloat32 (0.0f)), dgFloat32 (0.0f));
				dgVector boxP1 (GetMin (x1, m_boxSize.m_x),    GetMin (y1, m_boxSize.m_y),    GetMin (z1, m_boxSize.m_z),    dgFloat32 (0.0f));

				dgVector dq (rayP1.m_x - rayP0.m_x, rayP1.m_y - rayP0.m_y, rayP1.m_z - rayP0.m_z, dgFloat32 (0.0f));


				// make sure the line segment crosses the original segment box
				dgVector p0 (rayP0);
				dgVector p1 (rayP1);

				// clip the line against the bounding box
				if (dgRayBoxClip (p0, p1, boxP0, boxP1)) {
					dgVector dp (p1 - p0);
					dgInt32 ix0 = dgFastInt (p0.m_x * invScale);
					dgInt32 iz0 = dgFastInt (p0.m_z * invScale);


					// implement a 3ddda line algorithm 

					dgInt32 xInc = 0;
					dgFloat32 stepX = dgFloat32 (0.0f);
					dgFloat32 tx = dgFloat32 (1.0e10f);

					if (dp.m_x > dgFloat32 (0.0f)) {
						xInc = 1;
						dgFloat32 val = dgFloat32 (1.0f) / dp.m_x;
						stepX = scale * val;
						tx = (scale * (ix0 + dgFloat32 (1.0f)) - p0.m_x) * val;
					} else if (dp.m_x < dgFloat32 (0.0f)) {
						xInc = -1;
						dgFloat32 val = dgFloat32 (-1.0f) / dp.m_x;
						stepX = scale * val;
						tx = -(scale * ix0 - p0.m_x) * val;
					}

					dgInt32 zInc = 0;
					dgFloat32 stepZ = dgFloat32 (0.0f);
					dgFloat32 tz = dgFloat32 (1.0e10f);
					if (dp.m_z > dgFloat32 (0.0f)) {
						zInc = 1;
						dgFloat32 val = dgFloat32 (1.0f) / dp.m_z;
						stepZ = scale * val;
						tz = (scale * (iz0 + dgFloat32 (1.0f)) - p0.m_z) * val;
					} else if (dp.m_z < dgFloat32 (0.0f)) {
						zInc = -1;
						dgFloat32 val = dgFloat32 (-1.0f) / dp.m_z;
						stepZ = scale * val;
						tz = -(scale * iz0 - p0.m_z) * val;
					}

					dgFloat32 txAcc = tx;
					dgFloat32 tzAcc = tz;
					dgInt32 xIndex0 = ix0;
					dgInt32 zIndex0 = iz0;

					// for each cell touched by the line
					do {
						dgBroadPhaseCell *const cell = layer.Find (xIndex0, zIndex0);
						if (cell) {
							//_ASSERTE (cell->m_count);
							_ASSERTE (cell->m_lastSortArray);
							if (!me->m_inUpdate) {
								if (!cell->m_lastSortArray->m_isSorted) {
									cell->m_lastSortArray->Sort();
								}
							}
							minT = cell->m_lastSortArray->RayCast (minT, line, filter, prefilter, userData);
						}
						if (txAcc < tzAcc) {
							xIndex0 += xInc;
							tx = txAcc;
							txAcc += stepX;
						} else {
							zIndex0 += zInc;
							tz = tzAcc;
							tzAcc += stepZ;
						}
					} while ((tx <= dgFloat32 (1.0f)) || (tz <= dgFloat32 (1.0f)));
				}
			}
		}
	}
}



void dgBroadPhaseCollision::UpdateBodyBroadphase(dgBody* const body, dgInt32 threadIndex)
{
	if (!body->m_isInWorld) {
		if (dgOverlapTest (body->m_minAABB, body->m_maxAABB, m_appMinBox, m_appMaxBox)) {
//			dgBroadPhaseCell *cell;
//			cell = body->m_collisionCell.m_cell;
//			_ASSERTE (cell);
			Remove (body);
			Add (body); 
			body->m_isInWorld = true;
			body->m_sleeping = false;
			body->m_equilibrium = false;
		}
	}

	if (body->m_isInWorld) {

		dgWorld* const me = (dgWorld*) this;
		if ((body->m_minAABB.m_x > m_appMinBox.m_x) && (body->m_minAABB.m_y > m_appMinBox.m_y) && (body->m_minAABB.m_z > m_appMinBox.m_z) && 
			(body->m_maxAABB.m_x < m_appMaxBox.m_x) && (body->m_maxAABB.m_y < m_appMaxBox.m_y) && (body->m_maxAABB.m_z < m_appMaxBox.m_z)) {  
			dgFloat32 x0 = body->m_minAABB.m_x - m_min.m_x;
			//dgFloat32 y0 = body->m_minAABB.m_y - m_min.m_y;
			dgFloat32 z0 = body->m_minAABB.m_z - m_min.m_z;

			dgFloat32 x1 = body->m_maxAABB.m_x - m_min.m_x;
			//dgFloat32 y1 = body->m_maxAABB.m_y - m_min.m_y;
			dgFloat32 z1 = body->m_maxAABB.m_z - m_min.m_z;

			for (dgInt32 layer = DG_OCTREE_MAX_DEPTH - 1; layer >= 0; layer --) {
				dgFloat32 cellScale = m_layerMap[layer].m_invCellSize;
				dgInt32 ix0 = dgFastInt (x0 * cellScale);
				dgInt32 ix1 = dgFastInt (x1 * cellScale);
				if (ix1 == ix0) {
					dgInt32 iz0 = dgFastInt (z0 * cellScale);
					dgInt32 iz1 = dgFastInt (z1 * cellScale);
					if (iz1 == iz0) {
						if (!body->m_spawnnedFromCallback) {
							me->dgGetUserLock();
						}

						dgBroadPhaseCell* const newCell = m_layerMap[layer].FindCreate (ix0,  iz0);
						newCell->m_active = 1;
						dgBroadPhaseCell* const cell = body->m_collisionCell.m_cell;
						if (newCell != cell) {
							cell->Remove(body);
							if (!cell->m_count) {
								dgBroadPhaseLayer::dgTreeNode* const node = m_layerMap[dgInt32(cell->m_layerIndex)].GetNodeFromInfo(*cell);
								_ASSERTE (node);
								m_layerMap[int(cell->m_layerIndex)].Remove(node);
							}
							newCell->Add (body); 
						}
						newCell->m_lastSortArray->m_isSorted = 0;
						body->m_isInWorld = true;
						if (!body->m_spawnnedFromCallback) {
							me->dgReleasedUserLock();
						}
						return;
					}
				}
			}
		}

		if (dgOverlapTest (body->m_minAABB, body->m_maxAABB, m_appMinBox, m_appMaxBox)) {
			
			if (!body->m_spawnnedFromCallback) {
				me->dgGetUserLock();
			}
			dgBroadPhaseCell* const newCell = m_layerMap[0].FindCreate (0, 0);
			dgBroadPhaseCell* const cell = body->m_collisionCell.m_cell;
			if (newCell != cell) {
				cell->Remove(body);
				if (!cell->m_count) {
					dgBroadPhaseLayer::dgTreeNode* const node = m_layerMap[dgInt32(cell->m_layerIndex)].GetNodeFromInfo(*cell);
					_ASSERTE (node);
					m_layerMap[dgInt32(cell->m_layerIndex)].Remove(node);
				}
				newCell->Add (body); 
			}
			newCell->m_lastSortArray->m_isSorted = 0;
 			body->m_isInWorld = true;
			if (!body->m_spawnnedFromCallback) {
				me->dgReleasedUserLock();
			}
			return ;
		}

		{
  			

			body->m_sleeping = true;
			body->m_isInWorld = false;
			body->m_equilibrium = true;
			//bool inWorldFlag = body->m_isInWorld;

			if (!body->m_spawnnedFromCallback) {
				me->dgGetUserLock();
			}
			dgBroadPhaseCell* const cell = body->m_collisionCell.m_cell;
			cell->Remove(body);
			if (!cell->m_count) {
				dgBroadPhaseLayer::dgTreeNode* const node = m_layerMap[dgInt32(cell->m_layerIndex)].GetNodeFromInfo(*cell);
				_ASSERTE (node);
				m_layerMap[dgInt32(cell->m_layerIndex)].Remove(node);
			}
			m_inactiveList.Add (body); 
			if (!body->m_spawnnedFromCallback) {
				me->dgReleasedUserLock();
			}

			if (me->m_leavingWorldNotify) {
				me->m_leavingWorldNotify (body, threadIndex);
			}
		}
	}
}
/*
void dgBroadPhaseCollision::ForceAndtorque (void** const userParamArray, dgInt32 threadID)
{
	dgInt32 count  = dgInt32 (dgInt64 (userParamArray[1]));
	if (count > 8) {
		dgInt32 middle = count / 2;
		dgWorld* const me = (dgWorld*)userParamArray[4];
		userParamArray[1] = (void*)middle;
		me->QueueJob (ForceAndtorque, userParamArray, 5);

		dgBody* const bodies = ((dgBody*)userParamArray[0]) + middle;
		userParamArray[0] = bodies;
		userParamArray[1] = (void*) (count - middle);
		me->QueueJob (ForceAndtorque, userParamArray, 5);
	} else {
		for (dgInt32 i = 0; i < count; i ++) {

		}
	}
	
//	m_userParamArray[3] = (void*)skipForceUpdate;
//	m_userParamArray[4] = (void*)&timestep;
}
*/

dgUnsigned32 dgBroadPhaseCollision::UpdateContactsBroadPhaseBegin(dgFloat32 timestep, bool collisioUpdateOnly, dgUnsigned32 ticksBase)
{
	union {
		dgCellPair cellArray[1024];
		dgBody* bodyArray[1024];
	};
	dgInt32 chunkSizes[DG_MAXIMUN_THREADS];
	dgCollidingPairCollector::dgThreadPairCache pairCaches[DG_MAXIMUN_THREADS];

	dgWorld* const me = (dgWorld*) this;
	me->m_broadPhaseLru = me->m_broadPhaseLru + 1;
	const dgBodyMasterList& masterList = *me;
	dgInt32 threadCounts = dgInt32 (me->m_numberOfTheads);
	dgInt32 skipForceUpdate = collisioUpdateOnly ? 1 : 0;

	dgInt32 cellsBodyCount = 0;
	_ASSERTE (masterList.GetFirst()->GetInfo().GetBody() == me->GetSentinelBody());
	for (dgBodyMasterList::dgListNode* node = masterList.GetFirst()->GetNext(); node; node = node->GetNext()) { 
		dgBody* const body = node->GetInfo().GetBody();

		if (body->m_invMass.m_w == dgFloat32(0.0f)) {

			if (body->m_collision->GetCollisionPrimityType () == m_nullCollision) {
				if (body->m_collisionCell.m_cell != &m_inactiveList) {
					Remove (body);
					m_inactiveList.Add (body); 
				}
			}
			body->m_sleeping = true;
			body->m_autoSleep = true;
			body->m_equilibrium = true;
			body->m_solverInContinueCollision = false;

		} else {

			bodyArray[cellsBodyCount] = body;
			cellsBodyCount ++;
			if (cellsBodyCount >= dgInt32 ((sizeof (cellArray) / sizeof (dgBody*)))) {

				if (threadCounts > 1) {
					_ASSERTE (0);
					me->m_threadsManager.CalculateChunkSizes(cellsBodyCount, chunkSizes);
					for (dgInt32 threadIndex = 0; threadIndex < threadCounts; threadIndex ++) {
						m_applyExtForces[threadIndex].m_step = threadCounts;
						m_applyExtForces[threadIndex].m_skipForceUpdate = skipForceUpdate;
						m_applyExtForces[threadIndex].m_count = chunkSizes[threadIndex] * threadCounts;
						m_applyExtForces[threadIndex].m_bodies = &bodyArray[threadIndex];
						m_applyExtForces[threadIndex].m_threadIndex = threadIndex;
						m_applyExtForces[threadIndex].m_timeStep = timestep;
						m_applyExtForces[threadIndex].m_world = me;
						me->m_threadsManager.SubmitJob(&m_applyExtForces[threadIndex]);
					}
					me->m_threadsManager.SynchronizationBarrier ();
				} else {
					_ASSERTE (0);
					m_applyExtForces[0].m_step = 1;

					m_applyExtForces[0].m_count = cellsBodyCount;
					m_applyExtForces[0].m_timeStep = timestep;
					m_applyExtForces[0].m_bodies = &bodyArray[0];
					m_applyExtForces[0].m_threadIndex = 0;
					m_applyExtForces[0].m_skipForceUpdate = skipForceUpdate;
					m_applyExtForces[0].m_world = me;
					m_applyExtForces[0].ThreadExecute();
				}
				cellsBodyCount = 0;
			}
		}
	}



	if (threadCounts > 1) {
		me->m_threadsManager.CalculateChunkSizes(cellsBodyCount, chunkSizes);
		for (dgInt32 threadIndex = 0; threadIndex < threadCounts; threadIndex ++) {
			m_applyExtForces[threadIndex].m_step = threadCounts;
			m_applyExtForces[threadIndex].m_skipForceUpdate = skipForceUpdate;
			m_applyExtForces[threadIndex].m_count = chunkSizes[threadIndex] * threadCounts;
			m_applyExtForces[threadIndex].m_bodies = &bodyArray[threadIndex];
			m_applyExtForces[threadIndex].m_threadIndex = threadIndex;
			m_applyExtForces[threadIndex].m_timeStep = timestep;
			m_applyExtForces[threadIndex].m_world = me;
			me->m_threadsManager.SubmitJob(&m_applyExtForces[threadIndex]);
		}
		me->m_threadsManager.SynchronizationBarrier ();
	} else {
		m_applyExtForces[0].m_step = 1;
		m_applyExtForces[0].m_count = cellsBodyCount;
		m_applyExtForces[0].m_bodies = &bodyArray[0];
		m_applyExtForces[0].m_timeStep = timestep;
		m_applyExtForces[0].m_threadIndex = 0;
		m_applyExtForces[0].m_skipForceUpdate = skipForceUpdate;
		m_applyExtForces[0].m_world = me;
		m_applyExtForces[0].ThreadExecute();
	}

//	void* m_userParamArray[DG_MAX_THREADS_HIVE_PARAMETERS];
//	m_userParamArray[0] = bodyArray;
//	m_userParamArray[1] = (void*)cellsBodyCount;
//	m_userParamArray[2] = (void*)skipForceUpdate;
//	m_userParamArray[3] = (void*)&timestep;
//	m_userParamArray[4] = me;
//	me->QueueJob (ForceAndtorque, &m_userParamArray[0], 5);


	dgUnsigned32 ticks = me->m_getPerformanceCount();
	me->m_perfomanceCounters[m_forceCallback] = ticks - ticksBase;
	

	dgCollidingPairCollector& contactPair = *me;
//	contactPair.m_count = 0;
	contactPair.Init ();
	contactPair.SetCaches (pairCaches);

	dgInt32 cellsPairsCount = 0;
	for (dgInt32 i = 0; i < DG_OCTREE_MAX_DEPTH; i ++) {
		dgBroadPhaseLayer::Iterator iter (m_layerMap[i]);
		for (iter.Begin(); iter; iter ++) {
			dgBroadPhaseCell& cell = iter.GetNode()->GetInfo();
			_ASSERTE (cell.m_count);
			if (cell.m_active) {
				cell.Sort();
			}
		}
	}


	for (dgInt32 j = DG_OCTREE_MAX_DEPTH - 1; j > 0; j --) {
		dgBroadPhaseLayer::Iterator iter (m_layerMap[j]);
		for (iter.Begin(); iter; iter ++) {
			dgInt32 ix0;
			dgInt32 iz0;
			dgBroadPhaseCell* const cell0 = &iter.GetNode()->GetInfo();
			m_layerMap[j].KeyToIndex (dgInt32 (iter.GetNode()->GetKey()), ix0, iz0);

			for (dgInt32 i = j - 1; i >= 0; i --) {
				ix0 >>= 1;
				iz0 >>= 1;
				dgBroadPhaseCell* const cell1 = m_layerMap[i].Find(ix0, iz0);
				if (cell1) {
					if (cell0->m_active | cell1->m_active) {
						cellArray[cellsPairsCount].m_cell_A = cell1;
						cellArray[cellsPairsCount].m_cell_B = cell0;
						cellsPairsCount ++;
						if (cellsPairsCount >= dgInt32 (sizeof (cellArray) / sizeof (cellArray[0]))) {
							if (threadCounts > 1) {
								me->m_threadsManager.CalculateChunkSizes(cellsPairsCount, chunkSizes);
								for (dgInt32 threadIndex = 0; threadIndex < threadCounts; threadIndex ++) {
									m_cellPairsWorkerThreads[threadIndex].m_step = threadCounts;
									m_cellPairsWorkerThreads[threadIndex].m_count = chunkSizes[threadIndex] * threadCounts;
									m_cellPairsWorkerThreads[threadIndex].m_pairs = &cellArray[threadIndex];
									m_cellPairsWorkerThreads[threadIndex].m_threadIndex = threadIndex;
									m_cellPairsWorkerThreads[threadIndex].m_world = me;
									me->m_threadsManager.SubmitJob(&m_cellPairsWorkerThreads[threadIndex]);
								}
								me->m_threadsManager.SynchronizationBarrier ();
							} else {
								m_cellPairsWorkerThreads[0].m_step = 1;
								m_cellPairsWorkerThreads[0].m_count = cellsPairsCount;
								m_cellPairsWorkerThreads[0].m_pairs = &cellArray[0];
								m_cellPairsWorkerThreads[0].m_threadIndex = 0;
								m_cellPairsWorkerThreads[0].m_world = me;
								m_cellPairsWorkerThreads[0].ThreadExecute();
							}
							cellsPairsCount = 0;
						}
					}
				}
			}
		}
	}


	for (dgInt32 i = 0; i < DG_OCTREE_MAX_DEPTH; i ++) {
		dgBroadPhaseLayer::Iterator iter (m_layerMap[i]);
		for (iter.Begin(); iter; iter ++) {
			dgBroadPhaseCell& cell = iter.GetNode()->GetInfo();
			_ASSERTE (cell.m_count);
			if (cell.m_active) {
				cellArray[cellsPairsCount].m_cell_A = &cell;
				cellArray[cellsPairsCount].m_cell_B = NULL;
				cellsPairsCount ++;
				if (cellsPairsCount >= dgInt32 (sizeof (cellArray) / sizeof (cellArray[0]))) {

					if (threadCounts > 1) {
						me->m_threadsManager.CalculateChunkSizes(cellsPairsCount, chunkSizes);
						for (dgInt32 threadIndex = 0; threadIndex < threadCounts; threadIndex ++) {
							m_cellPairsWorkerThreads[threadIndex].m_step = threadCounts;
							m_cellPairsWorkerThreads[threadIndex].m_count = chunkSizes[threadIndex] * threadCounts;
							m_cellPairsWorkerThreads[threadIndex].m_pairs = &cellArray[threadIndex];
							m_cellPairsWorkerThreads[threadIndex].m_threadIndex = threadIndex;
							m_cellPairsWorkerThreads[threadIndex].m_world = me;
							me->m_threadsManager.SubmitJob(&m_cellPairsWorkerThreads[threadIndex]);
						}
						me->m_threadsManager.SynchronizationBarrier ();
					} else {
						m_cellPairsWorkerThreads[0].m_step = 1;
						m_cellPairsWorkerThreads[0].m_count = cellsPairsCount;
						m_cellPairsWorkerThreads[0].m_pairs = &cellArray[0];
						m_cellPairsWorkerThreads[0].m_threadIndex = 0;
						m_cellPairsWorkerThreads[0].m_world = me;
						m_cellPairsWorkerThreads[0].ThreadExecute();
					}
					cellsPairsCount = 0;
				}
			}
			cell.m_active = 0;
		}
	}

	if (threadCounts > 1) {
		me->m_threadsManager.CalculateChunkSizes(cellsPairsCount, chunkSizes);
		for (dgInt32 threadIndex = 0; threadIndex < threadCounts; threadIndex ++) {
			m_cellPairsWorkerThreads[threadIndex].m_step = threadCounts;
			m_cellPairsWorkerThreads[threadIndex].m_count = chunkSizes[threadIndex] * threadCounts;
			m_cellPairsWorkerThreads[threadIndex].m_pairs = &cellArray[threadIndex];
			m_cellPairsWorkerThreads[threadIndex].m_threadIndex = threadIndex;
			m_cellPairsWorkerThreads[threadIndex].m_world = me;
			me->m_threadsManager.SubmitJob(&m_cellPairsWorkerThreads[threadIndex]);
		}
		me->m_threadsManager.SynchronizationBarrier ();
	} else {
		m_cellPairsWorkerThreads[0].m_step = 1;
		m_cellPairsWorkerThreads[0].m_count = cellsPairsCount;
		m_cellPairsWorkerThreads[0].m_pairs = &cellArray[0];
		m_cellPairsWorkerThreads[0].m_threadIndex = 0;
		m_cellPairsWorkerThreads[0].m_world = me;
		m_cellPairsWorkerThreads[0].ThreadExecute();
	}

	for (dgInt32 i = 0; i < threadCounts; i ++) {
		if (pairCaches[i].m_count) {
			contactPair.FlushChache (&pairCaches[i]);
		}
	}

	ticksBase = me->m_getPerformanceCount(); 
	me->m_perfomanceCounters[m_broadPhaceTicks] = ticksBase - ticks;
	return ticksBase;
}


void dgBroadPhaseCollision::UpdateContactsBroadPhaseEnd (dgFloat32 timestep)
{
	// delete all non used contacts
	dgInt32 count = 0;
	dgWorld* const me = (dgWorld*) this;
	dgInt32 lru = dgInt32 (me->m_broadPhaseLru);

	dgCollidingPairCollector& contactPair = *me;
	dgContact** const deadContacs = (dgContact**) contactPair.m_pairs;
	dgActiveContacts& contactList = *me;

	for (dgActiveContacts::dgListNode* contactNode = contactList.GetFirst(); contactNode; contactNode = contactNode->GetNext()) {
		dgContact* const contact = contactNode->GetInfo();
		if ((contact->m_broadphaseLru != lru) || (contact->GetCount() == 0)) {
// note this is in observation (to prevent bodies from not going to sleep  inside triggers	
//			if (! (contact->m_body0->m_sleeping & contact->m_body1->m_sleeping) ) {
			const dgBody* const body0 = contact->m_body0;
			const dgBody* const body1 = contact->m_body1;
			if (! ((body0->m_sleeping | body0->m_equilibrium) & (body1->m_sleeping | body1->m_equilibrium)) ) {
				deadContacs[count] = contact;
				count ++;
			}
		}
	}

	for (dgInt32 i = 0; i < count; i ++) {
		dgContact* const contact = deadContacs[i];
		me->DestroyConstraint (contact);
	}
}



void dgBroadPhaseCollision::UpdateContactsSimd (dgFloat32 timestep, bool collisioUpdate)
{
#ifdef DG_BUILD_SIMD_CODE
	dgWorld* const me = (dgWorld*) this;
	dgUnsigned32 ticks = me->m_getPerformanceCount();
	dgUnsigned32 narrowTicks = UpdateContactsBroadPhaseBegin(timestep, collisioUpdate, ticks);

	// calculate or update new contacts
	dgCollidingPairCollector& contactPair = *me;

	dgInt32 count = contactPair.m_count;
	dgCollidingPairCollector::dgPair* const pairs = contactPair.m_pairs;
	dgInt32 threadCounts = dgInt32 (me->m_numberOfTheads);

	if (threadCounts > 1) {
		dgInt32 chunkSizes[DG_MAXIMUN_THREADS];
		me->m_threadsManager.CalculateChunkSizes(count, chunkSizes);
		for (dgInt32 threadIndex = 0; threadIndex < threadCounts; threadIndex ++) {
			m_calculateContactsWorkerThreads[threadIndex].m_useSimd = 1;
			m_calculateContactsWorkerThreads[threadIndex].m_step = threadCounts;
			m_calculateContactsWorkerThreads[threadIndex].m_count = chunkSizes[threadIndex] * threadCounts;
			m_calculateContactsWorkerThreads[threadIndex].m_threadIndex = threadIndex;
			m_calculateContactsWorkerThreads[threadIndex].m_timestep = timestep;		
			m_calculateContactsWorkerThreads[threadIndex].m_world = me;
			me->m_threadsManager.SubmitJob(&m_calculateContactsWorkerThreads[threadIndex]);
		}
		me->m_threadsManager.SynchronizationBarrier ();

		// material callback and create contact joints 
		for (dgInt32 threadIndex = 0; threadIndex < threadCounts; threadIndex ++) {
			m_materialCallbackWorkerThreads[threadIndex].m_step = threadCounts;
			m_materialCallbackWorkerThreads[threadIndex].m_useSimd = 0;
			m_materialCallbackWorkerThreads[threadIndex].m_count = chunkSizes[threadIndex] * threadCounts;;
			m_materialCallbackWorkerThreads[threadIndex].m_pairs = &pairs[threadIndex];
			m_materialCallbackWorkerThreads[threadIndex].m_threadIndex = threadIndex;
			m_materialCallbackWorkerThreads[threadIndex].m_timestep = timestep;		
			m_materialCallbackWorkerThreads[threadIndex].m_world = me;
			me->m_threadsManager.SubmitJob(&m_materialCallbackWorkerThreads[threadIndex]);
		}
		me->m_threadsManager.SynchronizationBarrier ();

	} else {
		m_calculateContactsWorkerThreads[0].m_step = 1;
		m_calculateContactsWorkerThreads[0].m_useSimd = 1;
		m_calculateContactsWorkerThreads[0].m_count = count;
		m_calculateContactsWorkerThreads[0].m_threadIndex = 0;
		m_calculateContactsWorkerThreads[0].m_timestep = timestep;		
		m_calculateContactsWorkerThreads[0].m_world = me;
		m_calculateContactsWorkerThreads[0].ThreadExecute();

		// material callback and create contact joints 
		m_materialCallbackWorkerThreads[0].m_step = 1;
		m_materialCallbackWorkerThreads[0].m_useSimd = 0;
		m_materialCallbackWorkerThreads[0].m_count = count;
		m_materialCallbackWorkerThreads[0].m_pairs = &pairs[0];
		m_materialCallbackWorkerThreads[0].m_threadIndex = 0;
		m_materialCallbackWorkerThreads[0].m_timestep = timestep;		
		m_materialCallbackWorkerThreads[0].m_world = me;
		m_materialCallbackWorkerThreads[0].ThreadExecute();

	}

	UpdateContactsBroadPhaseEnd(timestep);

	dgUnsigned32 endTicks = me->m_getPerformanceCount();
	me->m_perfomanceCounters[m_narrowPhaseTicks] = endTicks - narrowTicks;
	me->m_perfomanceCounters[m_collisionTicks] = endTicks - ticks;

#endif

}


void dgBroadPhaseCollision::UpdateContacts (dgFloat32 timestep, bool collisioUpdate)
{
//	dgUnsigned32 endTicks;

	dgWorld* const me = (dgWorld*) this;
	dgUnsigned32 ticks = me->m_getPerformanceCount();
	dgUnsigned32 narrowTicks = UpdateContactsBroadPhaseBegin(timestep, collisioUpdate, ticks);

	// calculate or update new contacts
	dgCollidingPairCollector& contactPair = *me;

	dgInt32 count = contactPair.m_count;
	dgCollidingPairCollector::dgPair * const pairs = contactPair.m_pairs;
	dgInt32 threadCounts = dgInt32 (me->m_numberOfTheads);
	if (threadCounts > 1) {
		dgInt32 chunkSizes[DG_MAXIMUN_THREADS];
		me->m_threadsManager.CalculateChunkSizes(count, chunkSizes);
		for (dgInt32 threadIndex = 0; threadIndex < threadCounts; threadIndex ++) {
			m_calculateContactsWorkerThreads[threadIndex].m_useSimd = 0;
			m_calculateContactsWorkerThreads[threadIndex].m_step = threadCounts;
			m_calculateContactsWorkerThreads[threadIndex].m_count = chunkSizes[threadIndex] * threadCounts;
			m_calculateContactsWorkerThreads[threadIndex].m_threadIndex = threadIndex;
			m_calculateContactsWorkerThreads[threadIndex].m_timestep = timestep;		
			m_calculateContactsWorkerThreads[threadIndex].m_world = me;
			me->m_threadsManager.SubmitJob(&m_calculateContactsWorkerThreads[threadIndex]);
		}
		me->m_threadsManager.SynchronizationBarrier ();

		for (dgInt32 threadIndex = 0; threadIndex < threadCounts; threadIndex ++) {
			m_materialCallbackWorkerThreads[threadIndex].m_step = threadCounts;
			m_materialCallbackWorkerThreads[threadIndex].m_useSimd = 0;
			m_materialCallbackWorkerThreads[threadIndex].m_count = chunkSizes[threadIndex] * threadCounts;;
			m_materialCallbackWorkerThreads[threadIndex].m_pairs = &pairs[threadIndex];
			m_materialCallbackWorkerThreads[threadIndex].m_threadIndex = threadIndex;
			m_materialCallbackWorkerThreads[threadIndex].m_timestep = timestep;		
			m_materialCallbackWorkerThreads[threadIndex].m_world = me;
			me->m_threadsManager.SubmitJob(&m_materialCallbackWorkerThreads[threadIndex]);
		}
		me->m_threadsManager.SynchronizationBarrier ();


	} else {

		// calculate contacts
		m_calculateContactsWorkerThreads[0].m_step = 1;
		m_calculateContactsWorkerThreads[0].m_useSimd = 0;
		m_calculateContactsWorkerThreads[0].m_count = count;
		m_calculateContactsWorkerThreads[0].m_threadIndex = 0;
		m_calculateContactsWorkerThreads[0].m_timestep = timestep;		
		m_calculateContactsWorkerThreads[0].m_world = me;
		m_calculateContactsWorkerThreads[0].ThreadExecute();

		// material callback and create contact joints 
		m_materialCallbackWorkerThreads[0].m_step = 1;
		m_materialCallbackWorkerThreads[0].m_useSimd = 0;
		m_materialCallbackWorkerThreads[0].m_count = count;
		m_materialCallbackWorkerThreads[0].m_pairs = &pairs[0];
		m_materialCallbackWorkerThreads[0].m_threadIndex = 0;
		m_materialCallbackWorkerThreads[0].m_timestep = timestep;		
		m_materialCallbackWorkerThreads[0].m_world = me;
		m_materialCallbackWorkerThreads[0].ThreadExecute();
	}

	UpdateContactsBroadPhaseEnd(timestep);

	dgUnsigned32 endTicks = me->m_getPerformanceCount();
	me->m_perfomanceCounters[m_narrowPhaseTicks] = endTicks - narrowTicks;
	me->m_perfomanceCounters[m_collisionTicks] = endTicks - ticks;
}