/* 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 "dgBroadPhaseCollision.h"
#include "dgBody.h"
#include "dgCollisionConvex.h"
#include "dgContact.h"
#include "dgWorld.h"
#include "dgWorldDynamicUpdate.h"
#include "hpl1/engine/libraries/newton/core/dg.h"


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

dgSortArray::~dgSortArray() {
	NEWTON_ASSERT(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)];
	NEWTON_ASSERT(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);
		}
	}
	//  NEWTON_ASSERT ((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];
		NEWTON_ASSERT(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() {
	NEWTON_ASSERT(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;

	NEWTON_ASSERT(!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;

	NEWTON_ASSERT(m_count >= 0);
	NEWTON_ASSERT(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)) {
					NEWTON_ASSERT(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() {
	NEWTON_ASSERT(!GetCount());
}

void dgBroadPhaseLayer::Init(dgWorld *const world, dgFloat32 cellSize,
                             dgInt32 layerIndex) {
	NEWTON_ASSERT(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++) {
		NEWTON_ASSERT(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) {
	NEWTON_ASSERT(!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;

	NEWTON_ASSERT(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);
			NEWTON_ASSERT(node);
			m_layerMap[dgInt32(obtreeCell->m_layerIndex)].Remove(node);
		}
	}
}

void dgBroadPhaseCollision::UpdatePairs(dgBody *const body0,
                                        dgSortArray::dgListNode *const srcnode, dgInt32 axisX,
                                        dgInt32 threadIndex) {
	//  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)) {
				NEWTON_ASSERT(body0 != body1);
				if (OverlapTest(body0, body1)) {
					contactPair.AddPair(body0, body1, threadIndex);
				}
			}
		}
	}
}

void dgBroadPhaseCollision::UpdatePairs(dgBroadPhaseCell &cellA,
                                        dgBroadPhaseCell &cellB, dgInt32 threadIndex) {
	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 = ((const 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) {
	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, reinterpret_cast<const NewtonBody *>(body), reinterpret_cast<const NewtonCollision *>(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 pv0(info[i].m_point[0], info[i].m_point[1], info[i].m_point[2],
				             dgFloat32(0.0f));
				dgVector pv1(pv0 + dir);
				pv0 -= dir;

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

				dgVector l0(matrix.UntransformVector(pv0));
				dgVector l1(matrix.UntransformVector(pv1));
				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;
		}
	}

	NEWTON_ASSERT(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];
				NEWTON_ASSERT(
				    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];
				NEWTON_ASSERT(
				    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;
				NEWTON_ASSERT(body->m_invMass.m_w > dgFloat32(0.0f));

				NEWTON_ASSERT(
				    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;
				NEWTON_ASSERT(body->m_invMass.m_w > dgFloat32(0.0f));

				NEWTON_ASSERT(
				    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;
			NEWTON_ASSERT(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;
			NEWTON_ASSERT(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) {
			NEWTON_ASSERT(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) {
	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) {
							//NEWTON_ASSERT (cell->m_count);
							NEWTON_ASSERT(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;
			//          NEWTON_ASSERT (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);
								NEWTON_ASSERT(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);
					NEWTON_ASSERT(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);
				NEWTON_ASSERT(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;
	NEWTON_ASSERT(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(dgCellPair)))) {

				if (threadCounts > 1) {
					NEWTON_ASSERT(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 {
					NEWTON_ASSERT(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();
			NEWTON_ASSERT(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();
			NEWTON_ASSERT(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;
}