/* Copyright (c) <2003-2019> <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 "dgStdafx.h"
#include "dgHeap.h"
#include "dgStack.h"
#include "dgList.h"
#include "dgMatrix.h"
#include "dgAABBPolygonSoup.h"
#include "dgPolygonSoupBuilder.h"


#define DG_STACK_DEPTH 512


DG_MSC_VECTOR_ALIGNMENT
class dgAABBPolygonSoup::dgNodeBuilder: public dgAABBPolygonSoup::dgNode
{
	public:
	dgNodeBuilder (const dgVector& p0, const dgVector& p1)
		:dgNode()
		,m_left (NULL)
		,m_right (NULL)
		,m_parent (NULL)
		,m_indexBox0(0)
		,m_indexBox1(0)
		,m_enumeration(-1)
		,m_faceIndex(0)
		,m_indexCount(0)
		,m_faceIndices(NULL)
	{
		SetBox (p0, p1);
	}

	dgNodeBuilder (const dgVector* const vertexArray, dgInt32 faceIndex, dgInt32 indexCount, const dgInt32* const indexArray)
		:dgNode()
		,m_left (NULL)
		,m_right (NULL)
		,m_parent (NULL)
		,m_indexBox0(0)
		,m_indexBox1(0)
		,m_enumeration(-1)
		,m_faceIndex(faceIndex)
		,m_indexCount(indexCount)
		,m_faceIndices(indexArray)
	{
		dgVector minP ( dgFloat32 (1.0e15f)); 
		dgVector maxP (-dgFloat32 (1.0e15f)); 
		for (dgInt32 i = 0; i < indexCount; i ++) {
			dgInt32 index = indexArray[i];
			const dgVector& p (vertexArray[index]);
			minP = p.GetMin(minP); 
			maxP = p.GetMax(maxP); 
		}
		minP -= dgVector (dgFloat32 (1.0e-3f));
		maxP += dgVector (dgFloat32 (1.0e-3f));
		minP = minP & dgVector::m_triplexMask;
		maxP = maxP & dgVector::m_triplexMask;
		SetBox (minP, maxP);
	}

	dgNodeBuilder (dgNodeBuilder* const left, dgNodeBuilder* const right)
		:dgNode()
		,m_left(left)
		,m_right(right)
		,m_parent(NULL)
		,m_indexBox0(0)
		,m_indexBox1(0)
		,m_enumeration(-1)
		,m_faceIndex(0)
		,m_indexCount(0)
		,m_faceIndices(NULL)
	{
		m_left->m_parent = this;
		m_right->m_parent = this;

		dgVector p0 (left->m_p0.GetMin(right->m_p0));
		dgVector p1 (left->m_p1.GetMax(right->m_p1));
		SetBox(p0, p1);
	}

	void SetBox (const dgVector& p0, const dgVector& p1)
	{
		m_p0 = p0;
		m_p1 = p1;
		m_size = m_p1 - m_p0;
		m_origin = (m_p1 + m_p0) * dgVector::m_half;
		m_area = m_size.DotProduct(m_size.ShiftTripleRight()).m_x;
	}

	DG_INLINE static dgFloat32 CalculateSurfaceArea (dgNodeBuilder* const node0, dgNodeBuilder* const node1, dgVector& minBox, dgVector& maxBox)
	{
		minBox = node0->m_p0.GetMin(node1->m_p0);
		maxBox = node0->m_p1.GetMax(node1->m_p1);

		dgVector side0 ((maxBox - minBox) * dgVector::m_half);
		//dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f));
		dgVector side1 (side0.ShiftTripleLeft());
		return side0.DotProduct(side1).GetScalar();
	}

	dgVector m_p0;
	dgVector m_p1;
	dgVector m_size;
	dgVector m_origin;
	dgFloat32 m_area;
	
	dgNodeBuilder* m_left;
	dgNodeBuilder* m_right;
	dgNodeBuilder* m_parent;
	dgInt32 m_indexBox0;
	dgInt32 m_indexBox1;
	dgInt32 m_enumeration;
	dgInt32 m_faceIndex;
	dgInt32 m_indexCount;
	const dgInt32* m_faceIndices;
} DG_GCC_VECTOR_ALIGNMENT;



class dgAABBPolygonSoup::dgSpliteInfo
{
	public:
	dgSpliteInfo (dgNodeBuilder* const boxArray, dgInt32 boxCount)
	{
		dgVector minP ( dgFloat32 (1.0e15f)); 
		dgVector maxP (-dgFloat32 (1.0e15f)); 

		if (boxCount == 2) {
			m_axis = 1;
			for (dgInt32 i = 0; i < boxCount; i ++) {
				const dgNodeBuilder& box = boxArray[i];
				const dgVector& p0 = box.m_p0;
				const dgVector& p1 = box.m_p1;
				minP = minP.GetMin (p0); 
				maxP = maxP.GetMax (p1); 
			}

		} else {
			dgVector median (dgFloat32 (0.0f));
			dgVector varian (dgFloat32 (0.0f));
			for (dgInt32 i = 0; i < boxCount; i ++) {
				const dgNodeBuilder& box = boxArray[i];

				const dgVector& p0 = box.m_p0;
				const dgVector& p1 = box.m_p1;

				minP = minP.GetMin (p0); 
				maxP = maxP.GetMax (p1); 
				dgVector p (dgVector::m_half * (p0 + p1));

				median += p;
				varian += p * p;
			}

			varian = varian.Scale (dgFloat32 (boxCount)) - median * median;

			dgInt32 index = 0;
			dgFloat32 maxVarian = dgFloat32 (-1.0e10f);
			for (dgInt32 i = 0; i < 3; i ++) {
				if (varian[i] > maxVarian) {
					index = i;
					maxVarian = varian[i];
				}
			}

			dgVector center = median.Scale (dgFloat32 (1.0f) / dgFloat32 (boxCount));
			dgFloat32 test = center[index];
			dgInt32 i0 = 0;
			dgInt32 i1 = boxCount - 1;
			do {    
				for (; i0 <= i1; i0 ++) {
					const dgNodeBuilder& box = boxArray[i0];
					dgFloat32 val = (box.m_p0[index] + box.m_p1[index]) * dgFloat32 (0.5f);
					if (val > test) {
						break;
					}
				}

				for (; i1 >= i0; i1 --) {
					const dgNodeBuilder& box = boxArray[i1];
					dgFloat32 val = (box.m_p0[index] + box.m_p1[index]) * dgFloat32 (0.5f);
					if (val < test) {
						break;
					}
				}

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

			if (i0 > 0){
				i0 --;
			}
			if ((i0 + 1) >= boxCount) {
				i0 = boxCount - 2;
			}

			m_axis = i0 + 1;
		}

		dgAssert (maxP.m_x - minP.m_x >= dgFloat32 (0.0f));
		dgAssert (maxP.m_y - minP.m_y >= dgFloat32 (0.0f));
		dgAssert (maxP.m_z - minP.m_z >= dgFloat32 (0.0f));
		m_p0 = minP;
		m_p1 = maxP;
	}

	dgInt32 m_axis;
	dgVector m_p0;
	dgVector m_p1;
};



dgAABBPolygonSoup::dgAABBPolygonSoup ()
	:dgPolygonSoupDatabase()
	,m_nodesCount(0)
	,m_indexCount(0)
	,m_aabb(NULL)
	,m_indices(NULL)
{
}

dgAABBPolygonSoup::~dgAABBPolygonSoup ()
{
	if (m_aabb) {
		dgFreeStack (m_aabb);
		dgFreeStack (m_indices);
	}
}


void dgAABBPolygonSoup::ImproveNodeFitness (dgNodeBuilder* const node) const
{
	dgAssert (node->m_left);
	dgAssert (node->m_right);

	if (node->m_parent)	{
		dgAssert(node->m_parent->m_p0.m_w == dgFloat32(0.0f));
		dgAssert(node->m_parent->m_p1.m_w == dgFloat32(0.0f));

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

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

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

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

				if (parent->m_parent) {
					if (parent->m_parent->m_left == parent) {
						parent->m_parent->m_left = node;
					} else {
						dgAssert (parent->m_parent->m_right == parent);
						parent->m_parent->m_right = node;
					}
				}
				node->m_parent = parent->m_parent;
				parent->m_parent = node;
				node->m_right->m_parent = parent;
				parent->m_left = node->m_right;
				node->m_right = parent;
				parent->m_p0 = cost1P0;
				parent->m_p1 = cost1P1;		
				parent->m_area = cost1;
				parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half;
				parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half;

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

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

				parent->m_p0 = cost2P0;
				parent->m_p1 = cost2P1;		
				parent->m_area = cost2;
				parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half;
				parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half;
			}
		} else {
			dgFloat32 cost0 = node->m_area;

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

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

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

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

				parent->m_p0 = cost1P0;
				parent->m_p1 = cost1P1;		
				parent->m_area = cost1;
				parent->m_size = (parent->m_p1 - parent->m_p0) * dgVector::m_half;
				parent->m_origin = (parent->m_p1 + parent->m_p0) * dgVector::m_half;

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

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

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

dgFloat32 dgAABBPolygonSoup::CalculateFaceMaxSize (const dgVector* const vertex, dgInt32 indexCount, const dgInt32* const indexArray) const
{
	dgFloat32 maxSize = dgFloat32 (0.0f);
	dgInt32 index = indexArray[indexCount - 1];
	dgVector p0 (vertex[index]);
	for (dgInt32 i = 0; i < indexCount; i ++) {
		dgInt32 index1 = indexArray[i];
		dgVector p1 (vertex[index1]);

		dgVector dir (p1 - p0);
		dgAssert (dir.m_w == dgFloat32 (0.0f));
		dir = dir.Normalize();

		dgFloat32 maxVal = dgFloat32 (-1.0e10f);
		dgFloat32 minVal = dgFloat32 ( 1.0e10f);
		for (dgInt32 j = 0; j < indexCount; j ++) {
			dgInt32 index2 = indexArray[j];
			dgVector q (vertex[index2]);
			dgFloat32 val = dir.DotProduct(q).GetScalar();
			minVal = dgMin(minVal, val);
			maxVal = dgMax(maxVal, val);
		}

		dgFloat32 size = maxVal - minVal;
		maxSize = dgMax(maxSize, size);
		p0 = p1;
	}

	return dgFloor (maxSize + dgFloat32 (1.0f));
}

void dgAABBPolygonSoup::GetAABB (dgVector& p0, dgVector& p1) const
{
	if (m_aabb) { 
		GetNodeAABB (m_aabb, p0, p1);
	} else {
		p0 = dgVector::m_zero;
		p1 = dgVector::m_zero;
	}
}

void dgAABBPolygonSoup::CalculateAdjacendy ()
{
	dgVector p0;
	dgVector p1;
	GetAABB (p0, p1);
	dgFastAABBInfo box (p0, p1);
	ForAllSectors (box, dgVector (dgFloat32 (0.0f)), dgFloat32 (1.0f), CalculateAllFaceEdgeNormals, this);

	dgStack<dgTriplex> pool ((m_indexCount / 2) - 1);
	const dgTriplex* const vertexArray = (dgTriplex*)GetLocalVertexPool();
	dgInt32 normalCount = 0;
	for (dgInt32 i = 0; i < m_nodesCount; i ++) {
		const dgNode* const node = &m_aabb[i];
		if (node->m_left.IsLeaf()) {
			dgInt32 vCount = dgInt32 (node->m_left.GetCount());
			if (vCount) {
				dgInt32 index = dgInt32 (node->m_left.GetIndex());
				dgInt32* const face = &m_indices[index];

				dgInt32 j0 = 2 * (vCount + 1) - 1;
				dgVector normal (&vertexArray[face[vCount + 1]].m_x);
				normal = normal & dgVector::m_triplexMask;
				dgAssert (dgAbs (normal.DotProduct(normal).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f));
				dgVector q0 (&vertexArray[face[vCount - 1]].m_x);
				q0 = q0 & dgVector::m_triplexMask;
				for (dgInt32 j = 0; j < vCount; j ++) {
					dgInt32 j1 = vCount + 2 + j;
					dgVector q1 (&vertexArray[face[j]].m_x);
					q1 = q1 & dgVector::m_triplexMask;
					if (face[j0] == -1) {
						dgVector e (q1 - q0);
						dgVector n (e.CrossProduct(normal));
						n = n.Scale(dgFloat32 (1.0f) / dgSqrt (n.DotProduct(n).GetScalar()));
						dgAssert (dgAbs (n.DotProduct(n).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f));
						pool[normalCount].m_x = n.m_x;
						pool[normalCount].m_y = n.m_y;
						pool[normalCount].m_z = n.m_z;
						face[j0] = -normalCount - 1;
						normalCount ++;
					}
					q0 = q1;
					j0 = j1;
				}
			}
		}

		if (node->m_right.IsLeaf()) {
			dgInt32 vCount = dgInt32 (node->m_right.GetCount());
			if (vCount) {
				dgInt32 index = dgInt32 (node->m_right.GetIndex());
				dgInt32* const face = &m_indices[index];

				dgInt32 j0 = 2 * (vCount + 1) - 1;
				dgVector normal (&vertexArray[face[vCount + 1]].m_x);
				normal = normal & dgVector::m_triplexMask;
				dgAssert (dgAbs (normal.DotProduct(normal).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f));
				dgVector q0 (&vertexArray[face[vCount - 1]].m_x);
				q0 = q0 & dgVector::m_triplexMask;
				for (dgInt32 j = 0; j < vCount; j ++) {
					dgInt32 j1 = vCount + 2 + j;
					dgVector q1 (&vertexArray[face[j]].m_x);
					q1 = q1 & dgVector::m_triplexMask;
					if (face[j0] == -1) {
						dgVector e (q1 - q0);
						dgVector n (e.CrossProduct(normal));
						n = n.Scale(dgFloat32 (1.0f) / dgSqrt (n.DotProduct(n).GetScalar()));
						dgAssert (dgAbs (n.DotProduct(n).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f));
						pool[normalCount].m_x = n.m_x;
						pool[normalCount].m_y = n.m_y;
						pool[normalCount].m_z = n.m_z;
						face[j0] = -normalCount - 1;
						normalCount ++;
					}
					q0 = q1;
					j0 = j1;
				}
			}
		}
	}

	if (normalCount) {
		dgStack<dgInt32> indexArray (normalCount);
		dgInt32 newNormalCount = dgVertexListToIndexList (&pool[0].m_x, sizeof (dgTriplex), sizeof (dgTriplex), 0, normalCount, &indexArray[0], dgFloat32 (1.0e-6f));

		dgInt32 oldCount = GetVertexCount();
		dgTriplex* const vertexArray1 = (dgTriplex*) dgMallocStack (sizeof (dgTriplex) * (oldCount + newNormalCount));
		memcpy (vertexArray1, GetLocalVertexPool(), sizeof (dgTriplex) * oldCount);
		memcpy (&vertexArray1[oldCount], &pool[0].m_x, sizeof (dgTriplex) * newNormalCount);
		dgFreeStack(GetLocalVertexPool());

		m_localVertex = &vertexArray1[0].m_x;
		m_vertexCount = oldCount + newNormalCount;

		for (dgInt32 i = 0; i < m_nodesCount; i ++) {
			const dgNode* const node = &m_aabb[i];
			if (node->m_left.IsLeaf()) {
				dgInt32 vCount = dgInt32 (node->m_left.GetCount());
				dgInt32 index = dgInt32 (node->m_left.GetIndex());
				dgInt32* const face = &m_indices[index];
				for (dgInt32 j = 0; j < vCount; j ++) {
					if (face[vCount + 2 + j] < 0) {
						dgInt32 k = -1 - face[vCount + 2 + j];
						face[vCount + 2 + j] = indexArray[k] + oldCount;
					}
					#ifdef _DEBUG	
						dgVector normal (&vertexArray1[face[vCount + 2 + j]].m_x);
						normal = normal & dgVector::m_triplexMask;
						dgAssert (dgAbs (normal.DotProduct(normal).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f));
					#endif
				}
			}

			if (node->m_right.IsLeaf()) {
				dgInt32 vCount = dgInt32 (node->m_right.GetCount());
				dgInt32 index = dgInt32 (node->m_right.GetIndex());
				dgInt32* const face = &m_indices[index];
				for (dgInt32 j = 0; j < vCount; j ++) {
					if (face[vCount + 2 + j] < 0) {
						dgInt32 k = -1 - face[vCount + 2 + j];
						face[vCount + 2 + j] = indexArray[k] + oldCount;
					}

					#ifdef _DEBUG	
						dgVector normal (&vertexArray1[face[vCount + 2 + j]].m_x);
						normal = normal & dgVector::m_triplexMask;
						dgAssert (dgAbs (normal.DotProduct(normal).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-6f));
					#endif
				}
			}
		}
	}
}

dgIntersectStatus dgAABBPolygonSoup::CalculateAllFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance)
{
	dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));

	AdjacentdFace adjacentFaces;
	adjacentFaces.m_count = indexCount;
	adjacentFaces.m_index = (dgInt32*) indexArray;

	dgVector n (&polygon[indexArray[indexCount + 1] * stride]);
	dgVector p (&polygon[indexArray[0] * stride]);
	n = n & dgVector::m_triplexMask;
	p = p & dgVector::m_triplexMask;
	adjacentFaces.m_normal = dgPlane (n, - n.DotProduct(p).GetScalar());

	dgAssert (indexCount < dgInt32 (sizeof (adjacentFaces.m_edgeMap) / sizeof (adjacentFaces.m_edgeMap[0])));

	dgInt32 edgeIndex = indexCount - 1;
	dgInt32 i0 = indexArray[indexCount - 1];
	dgVector p0 ( dgFloat32 (1.0e15f),  dgFloat32 (1.0e15f),  dgFloat32 (1.0e15f), dgFloat32 (0.0f)); 
	dgVector p1 (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f)); 
	for (dgInt32 i = 0; i < indexCount; i ++) {
		dgInt32 i1 = indexArray[i];
		dgInt32 index = i1 * stride;
		dgVector point (&polygon[index]);
		point = point & dgVector::m_triplexMask;
		p0 = p0.GetMin(point);
		p1 = p1.GetMax(point);
		adjacentFaces.m_edgeMap[edgeIndex] = (dgInt64 (i1) << 32) + i0;
		edgeIndex = i;
		i0 = i1;
	}

	dgFloat32 padding = dgFloat32 (1.0f/16.0f);
	p0.m_x -= padding;
	p0.m_y -= padding;
	p0.m_z -= padding;
	p1.m_x += padding;
	p1.m_y += padding;
	p1.m_z += padding;

	dgAABBPolygonSoup* const me = (dgAABBPolygonSoup*) context;
	dgFastAABBInfo box (p0, p1);
	me->ForAllSectors (box, dgVector (dgFloat32 (0.0f)), dgFloat32 (1.0f), CalculateDisjointedFaceEdgeNormals, &adjacentFaces);
	return t_ContinueSearh;
}


dgIntersectStatus dgAABBPolygonSoup::CalculateDisjointedFaceEdgeNormals (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance)
{
	#define DG_WELDING_TOL (1.0e-2f)
	#define DG_WELDING_TOL2 (DG_WELDING_TOL * DG_WELDING_TOL)

	const AdjacentdFace& adjacentFace = *((AdjacentdFace*)context);

	if (adjacentFace.m_index != indexArray) {	
		dgInt32 adjacentCount = adjacentFace.m_count;
		dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));

		dgInt32 j0 = adjacentCount - 1;
		dgInt32 indexJ0 = adjacentFace.m_index[adjacentCount - 1];
		for (dgInt32 j = 0; j < adjacentCount; j ++) {
			dgInt32 indexJ1 = adjacentFace.m_index[j];
			dgBigVector q0 (dgVector(&polygon[indexJ1 * stride]) & dgVector::m_triplexMask);
			dgBigVector q1 (dgVector(&polygon[indexJ0 * stride]) & dgVector::m_triplexMask);
			dgBigVector q1q0 (q1 - q0);
			dgFloat64 q1q0Mag2 = q1q0.DotProduct(q1q0).GetScalar();

			dgInt32 indexI0 = indexArray[indexCount - 1];
			for (dgInt32 i = 0; i < indexCount; i ++) {
				dgInt32 indexI1 = indexArray[i];
				dgBigVector p0 (dgVector(&polygon[indexI0 * stride]) & dgVector::m_triplexMask);
				dgBigVector p1 (dgVector(&polygon[indexI1 * stride]) & dgVector::m_triplexMask);
				dgBigVector p1p0 (p1 - p0);
				dgFloat64 dot = p1p0.DotProduct(q1q0).GetScalar();
				if (dot > 0.0f) {
					dgFloat64 q1p0Mag2 = p1p0.DotProduct(p1p0).GetScalar();
					if ((dot * dot) >= (q1p0Mag2 * q1q0Mag2 * dgFloat64(0.99995f))) {
						dgFloat64 x0 = q0.DotProduct(q1q0).GetScalar();
						dgFloat64 x1 = q1.DotProduct(q1q0).GetScalar();
						dgFloat64 y0 = p0.DotProduct(q1q0).GetScalar();
						dgFloat64 y1 = p1.DotProduct(q1q0).GetScalar();
						dgAssert (x1 > x0);
						dgAssert (y1 > y0);
						if (!((y0 >= x1) || (y1 <= x0))) {
							dgFloat64 t = q1q0.DotProduct(p0 - q0).GetScalar() / q1q0Mag2;
							dgAssert (q1q0.m_w == dgFloat32 (0.0f));
							dgBigVector q (q0 + q1q0.Scale(t));
							dgBigVector dist (p0 - q);
							dgAssert (dist.m_w == dgFloat32 (0.0f));
							dgFloat64 err2 = dist.DotProduct(dist).GetScalar();
							if (err2 < DG_WELDING_TOL2) {
								dgFloat32 maxDist = dgFloat32 (0.0f);
								for (dgInt32 k = 0; k < indexCount; k ++) {
									dgVector r (&polygon[indexArray[k] * stride]);
									r = r & dgVector::m_triplexMask;
									dgFloat32 dist1 = adjacentFace.m_normal.Evalue(r);
									if (dgAbs (dist1) > dgAbs (maxDist)) {
										maxDist = dist1;
									}
								}

								if (adjacentFace.m_index[j0 + adjacentCount + 2] == -1) {
									if (maxDist < -dgFloat32 (1.0e-3f)) {
										adjacentFace.m_index[j0 + adjacentCount + 2] = indexArray[indexCount + 1];
									} else {
										adjacentFace.m_index[j0 + adjacentCount + 2] = adjacentFace.m_index[adjacentCount + 1];
									}
								} else {
									if (maxDist < -dgFloat32 (1.0e-3f)) {
										dgBigVector n0 (adjacentFace.m_normal[0], adjacentFace.m_normal[1], adjacentFace.m_normal[2], dgFloat64(0.0f));
										dgBigVector n1 (dgVector(&polygon[adjacentFace.m_index[j0 + adjacentCount + 2] * stride]) & dgVector::m_triplexMask);
										dgBigVector n2 (dgVector(&polygon[indexArray[indexCount + 1] * stride]) & dgVector::m_triplexMask);

										dgBigVector tilt0 (n0.CrossProduct(n1)); 
										dgBigVector tilt1 (n0.CrossProduct(n2)); 
										dgFloat64 dist0 (q1q0.DotProduct(tilt0).GetScalar());
										dgFloat64 dist1 (q1q0.DotProduct(tilt1).GetScalar());
										if (dist0 < dist1) {
											adjacentFace.m_index[j0 + adjacentCount + 2] = indexArray[indexCount + 1];
										}
									} else {
										adjacentFace.m_index[j0 + adjacentCount + 2] = adjacentFace.m_index[adjacentCount + 1];
									}
								}
								break;
							}
						}
					}
				}
				indexI0 = indexI1;
			}
			j0 = j;
			indexJ0 = indexJ1;
		}
	}
	return t_ContinueSearh;
}



dgAABBPolygonSoup::dgNodeBuilder* dgAABBPolygonSoup::BuildTopDown (dgNodeBuilder* const leafArray, dgInt32 firstBox, dgInt32 lastBox, dgNodeBuilder** const allocator) const
{
	dgAssert (firstBox >= 0);
	dgAssert (lastBox >= 0);

	if (lastBox == firstBox) {
		return &leafArray[firstBox];
	} else {
		dgSpliteInfo info (&leafArray[firstBox], lastBox - firstBox + 1);

		dgNodeBuilder* const parent = new (*allocator) dgNodeBuilder (info.m_p0, info.m_p1);
		*allocator = *allocator + 1;

		dgAssert (parent);
		parent->m_right = BuildTopDown (leafArray, firstBox + info.m_axis, lastBox, allocator);
		parent->m_right->m_parent = parent;

		parent->m_left = BuildTopDown (leafArray, firstBox, firstBox + info.m_axis - 1, allocator);
		parent->m_left->m_parent = parent;
		return parent;
	}
}

void dgAABBPolygonSoup::Create (const dgPolygonSoupDatabaseBuilder& builder, bool optimizedBuild)
{
	if (builder.m_faceCount == 0) {
		return;
	}
	dgAssert (builder.m_faceCount >= 1);
	m_strideInBytes = sizeof (dgTriplex);
	m_nodesCount = ((builder.m_faceCount - 1) < 1) ? 1 : builder.m_faceCount - 1;
	m_aabb = (dgNode*) dgMallocStack (sizeof (dgNode) * m_nodesCount);
	m_indexCount = builder.m_indexCount * 2 + builder.m_faceCount;
	if (builder.m_faceCount == 1) {
		m_indexCount *= 2;
	}
	m_indices = (dgInt32*) dgMallocStack (sizeof (dgInt32) * m_indexCount);
	dgStack<dgVector> tmpVertexArrayCont(builder.m_vertexCount + builder.m_normalCount + builder.m_faceCount * 2 + 4);

	dgVector* const tmpVertexArray = &tmpVertexArrayCont[0];
	for (dgInt32 i = 0; i < builder.m_vertexCount; i ++) {
		tmpVertexArray[i] = builder.m_vertexPoints[i];
	}

	for (dgInt32 i = 0; i < builder.m_normalCount; i ++) {
		tmpVertexArray[i + builder.m_vertexCount] = builder.m_normalPoints[i];
	}

	const dgInt32* const indices = &builder.m_vertexIndex[0];
	dgStack<dgNodeBuilder> constructor (builder.m_faceCount * 2 + 16); 

	dgInt32 polygonIndex = 0;
	dgInt32 allocatorIndex = 0;
	if (builder.m_faceCount == 1) {
		dgInt32 indexCount = builder.m_faceVertexCount[0] - 1;
		new (&constructor[allocatorIndex]) dgNodeBuilder (&tmpVertexArray[0], 0, indexCount, &indices[0]);
		allocatorIndex ++;
	}
	for (dgInt32 i = 0; i < builder.m_faceCount; i ++) {
		dgInt32 indexCount = builder.m_faceVertexCount[i] - 1;
		new (&constructor[allocatorIndex]) dgNodeBuilder (&tmpVertexArray[0], i, indexCount, &indices[polygonIndex]);
		allocatorIndex ++;
		polygonIndex += (indexCount + 1);
	}

	dgNodeBuilder* contructorAllocator = &constructor[allocatorIndex];
	dgNodeBuilder* root = BuildTopDown (&constructor[0], 0, allocatorIndex - 1, &contructorAllocator);

	dgAssert (root);
	if (root->m_left) {

		dgAssert (root->m_right);
		dgList<dgNodeBuilder*> list (builder.m_allocator);
		dgList<dgNodeBuilder*> stack (builder.m_allocator);
		stack.Append(root);
		while (stack.GetCount()) {
			dgList<dgNodeBuilder*>::dgListNode* const stackNode = stack.GetLast();
			dgNodeBuilder* const node = stackNode->GetInfo();
			stack.Remove(stackNode);

			if (node->m_left) {
				dgAssert (node->m_right);
				list.Append(node);
				stack.Append(node->m_right);
				stack.Append(node->m_left);
			} 
		}

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

			newCost = dgFloat32 (0.0f);
			for (dgList<dgNodeBuilder*>::dgListNode* listNode = list.GetFirst(); listNode; listNode = listNode->GetNext()) {
				dgNodeBuilder* const node = listNode->GetInfo();
				newCost += node->m_area;
			}
		} while (newCost < (prevCost * dgFloat32 (0.9999f)));

		root = list.GetLast()->GetInfo();
		while (root->m_parent) {
			root = root->m_parent;
		}
	}

	dgList<dgNodeBuilder*> list (builder.m_allocator);

	list.Append(root);
	dgInt32 nodeIndex = 0;
	while (list.GetCount())  {
		dgNodeBuilder* const node = list.GetFirst()->GetInfo();
		list.Remove(list.GetFirst());

		if (node->m_left) {
			node->m_enumeration = nodeIndex;
			nodeIndex ++;
			dgAssert (node->m_right);
			list.Append(node->m_left);
			list.Append(node->m_right);
		}
	}
	dgAssert(!list.GetCount());

	dgInt32 aabbBase = builder.m_vertexCount + builder.m_normalCount;


	dgVector* const aabbPoints = &tmpVertexArray[aabbBase];


	dgInt32 vertexIndex = 0;
	dgInt32 aabbNodeIndex = 0;
	list.Append(root);
	dgInt32 indexMap = 0;
	while (list.GetCount())  {

		dgNodeBuilder* const node = list.GetFirst()->GetInfo();
		list.Remove(list.GetFirst());

		if (node->m_enumeration >= 0) {
			dgAssert (node->m_left);
			dgAssert (node->m_right);
			dgNode& aabbNode = m_aabb[aabbNodeIndex];
			aabbNodeIndex ++;
			dgAssert (aabbNodeIndex <= m_nodesCount);

			if (node->m_parent) {
				if (node->m_parent->m_left == node) {
					m_aabb[node->m_parent->m_enumeration].m_left = dgNode::dgLeafNodePtr (dgUnsigned32 (&m_aabb[node->m_enumeration] - m_aabb));
				} else {
					dgAssert (node->m_parent->m_right == node);
					m_aabb[node->m_parent->m_enumeration].m_right = dgNode::dgLeafNodePtr (dgUnsigned32 (&m_aabb[node->m_enumeration] - m_aabb));
				}
			}

			aabbPoints[vertexIndex + 0] = node->m_p0;
			aabbPoints[vertexIndex + 1] = node->m_p1;

			aabbNode.m_indexBox0 = aabbBase + vertexIndex;
			aabbNode.m_indexBox1 = aabbBase + vertexIndex + 1;

			vertexIndex += 2;

		} else {
			dgAssert (!node->m_left);
			dgAssert (!node->m_right);

			if (node->m_parent) {
				if (node->m_parent->m_left == node) {
					m_aabb[node->m_parent->m_enumeration].m_left = dgNode::dgLeafNodePtr (dgUnsigned32(node->m_indexCount), dgUnsigned32(indexMap));
				} else {
					dgAssert (node->m_parent->m_right == node);
					m_aabb[node->m_parent->m_enumeration].m_right = dgNode::dgLeafNodePtr (dgUnsigned32(node->m_indexCount), dgUnsigned32(indexMap));
				}
			}

			// index format i0, i1, i2, ... , id, normal, e0Normal, e1Normal, e2Normal, ..., faceSize
			for (dgInt32 j = 0; j < node->m_indexCount; j ++) {
				m_indices[indexMap + j] = node->m_faceIndices[j];
				m_indices[indexMap + j + node->m_indexCount + 2] = -1;
			}

			// face attribute
			m_indices[indexMap + node->m_indexCount] = node->m_faceIndices[node->m_indexCount];
			// face normal
			m_indices[indexMap + node->m_indexCount + 1] = builder.m_vertexCount + builder.m_normalIndex[node->m_faceIndex];
			// face size
			m_indices[indexMap + node->m_indexCount * 2 + 2] = dgInt32 (CalculateFaceMaxSize (&tmpVertexArray[0], node->m_indexCount, node->m_faceIndices));

			indexMap += node->m_indexCount * 2 + 3;
		}

		if (node->m_left) {
			dgAssert (node->m_right);
			list.Append(node->m_left);
			list.Append(node->m_right);
		}
	}

	dgStack<dgInt32> indexArray (vertexIndex);
	dgInt32 aabbPointCount = dgVertexListToIndexList (&aabbPoints[0].m_x, sizeof (dgVector), sizeof (dgTriplex), 0, vertexIndex, &indexArray[0], dgFloat32 (1.0e-6f));

	m_vertexCount = aabbBase + aabbPointCount;
	m_localVertex = (dgFloat32*) dgMallocStack (sizeof (dgTriplex) * m_vertexCount);

	dgTriplex* const dstPoints = (dgTriplex*)m_localVertex;
	for (dgInt32 i = 0; i < m_vertexCount; i ++) {
		dstPoints[i].m_x = tmpVertexArray[i].m_x;
		dstPoints[i].m_y = tmpVertexArray[i].m_y;
		dstPoints[i].m_z = tmpVertexArray[i].m_z;
	}

	for (dgInt32 i = 0; i < m_nodesCount; i ++) {
		dgNode& box = m_aabb[i];

		dgInt32 j = box.m_indexBox0 - aabbBase;
		box.m_indexBox0 = indexArray[j] + aabbBase;

		j = box.m_indexBox1 - aabbBase;
		box.m_indexBox1 = indexArray[j] + aabbBase;
	}

	if (builder.m_faceCount == 1) {
		m_aabb[0].m_right = dgNode::dgLeafNodePtr (0, 0);
	}
//	CalculateAdjacendy();
}

void dgAABBPolygonSoup::Serialize (dgSerialize callback, void* const userData) const
{
	callback (userData, &m_vertexCount, sizeof (dgInt32));
	callback (userData, &m_indexCount, sizeof (dgInt32));
	callback (userData, &m_nodesCount, sizeof (dgInt32));
	callback (userData, &m_nodesCount, sizeof (dgInt32));
	if (m_aabb) {
		callback (userData,  m_localVertex, dgInt32 (sizeof (dgTriplex) * m_vertexCount));
		callback (userData,  m_indices, dgInt32 (sizeof (dgInt32) * m_indexCount));
		callback (userData, m_aabb, dgInt32 (sizeof (dgNode) * m_nodesCount));
	}
}

void dgAABBPolygonSoup::Deserialize (dgDeserialize callback, void* const userData, dgInt32 revisionNumber)
{
	m_strideInBytes = sizeof (dgTriplex);
	callback (userData, &m_vertexCount, sizeof (dgInt32));
	callback (userData, &m_indexCount, sizeof (dgInt32));
	callback (userData, &m_nodesCount, sizeof (dgInt32));
	callback (userData, &m_nodesCount, sizeof (dgInt32));

	if (m_vertexCount) {
		m_localVertex = (dgFloat32*) dgMallocStack (sizeof (dgTriplex) * m_vertexCount);
		m_indices = (dgInt32*) dgMallocStack (sizeof (dgInt32) * m_indexCount);
		m_aabb = (dgNode*) dgMallocStack (sizeof (dgNode) * m_nodesCount);

		callback (userData, m_localVertex, dgInt32 (sizeof (dgTriplex) * m_vertexCount));
		callback (userData, m_indices, dgInt32 (sizeof (dgInt32) * m_indexCount));
		callback (userData, m_aabb, dgInt32 (sizeof (dgNode) * m_nodesCount));
	} else {
		m_localVertex = NULL;
		m_indices = NULL;
		m_aabb = NULL;
	}
}


dgVector dgAABBPolygonSoup::ForAllSectorsSupportVectex (const dgVector& dir) const
{
	dgVector supportVertex (dgFloat32 (0.0f));
	if (m_aabb) {
		dgFloat32 aabbProjection[DG_STACK_DEPTH];
		const dgNode *stackPool[DG_STACK_DEPTH];

		dgInt32 stack = 1;
		stackPool[0] = m_aabb;
		aabbProjection[0] = dgFloat32 (1.0e10f);
		const dgTriplex* const boxArray = (dgTriplex*)m_localVertex;
		

		dgFloat32 maxProj = dgFloat32 (-1.0e20f); 
		dgInt32 ix = (dir[0] > dgFloat32 (0.0f)) ? 1 : 0;
		dgInt32 iy = (dir[1] > dgFloat32 (0.0f)) ? 1 : 0;
		dgInt32 iz = (dir[2] > dgFloat32 (0.0f)) ? 1 : 0;

		while (stack) {
			dgFloat32 boxSupportValue;

			stack--;
			boxSupportValue = aabbProjection[stack];
			if (boxSupportValue > maxProj) {
				dgFloat32 backSupportDist = dgFloat32 (0.0f);
				dgFloat32 frontSupportDist = dgFloat32 (0.0f);
				const dgNode* const me = stackPool[stack];
				if (me->m_left.IsLeaf()) {
					backSupportDist = dgFloat32 (-1.0e20f);
					dgInt32 index = dgInt32 (me->m_left.GetIndex());
					dgInt32 vCount = dgInt32 (me->m_left.GetCount());
					dgVector vertex (dgFloat32 (0.0f));
					for (dgInt32 j = 0; j < vCount; j ++) {
						dgInt32 i0 = m_indices[index + j] * dgInt32 (sizeof (dgTriplex) / sizeof (dgFloat32));
						dgVector p (&boxArray[i0].m_x);
						p = p & dgVector::m_triplexMask;
						dgFloat32 dist = p.DotProduct(dir).GetScalar();
						if (dist > backSupportDist) {
							backSupportDist = dist;
							vertex = p;
						}
					}

					if (backSupportDist > maxProj) {
						maxProj = backSupportDist;
						supportVertex = vertex; 
					}

				} else {
					dgVector box[2];
					const dgNode* const node = me->m_left.GetNode(m_aabb);
					box[0].m_x = boxArray[node->m_indexBox0].m_x;
					box[0].m_y = boxArray[node->m_indexBox0].m_y;
					box[0].m_z = boxArray[node->m_indexBox0].m_z;
					box[1].m_x = boxArray[node->m_indexBox1].m_x;
					box[1].m_y = boxArray[node->m_indexBox1].m_y;
					box[1].m_z = boxArray[node->m_indexBox1].m_z;

					dgVector supportPoint (box[ix].m_x, box[iy].m_y, box[iz].m_z, dgFloat32 (0.0));
					backSupportDist = supportPoint.DotProduct(dir).GetScalar();
				}

				if (me->m_right.IsLeaf()) {
					frontSupportDist = dgFloat32 (-1.0e20f);
					dgInt32 index = dgInt32 (me->m_right.GetIndex());
					dgInt32 vCount = dgInt32 (me->m_right.GetCount());
					dgVector vertex (dgFloat32 (0.0f));
					for (dgInt32 j = 0; j < vCount; j ++) {
						dgInt32 i0 = m_indices[index + j] * dgInt32 (sizeof (dgTriplex) / sizeof (dgFloat32));
						dgVector p (&boxArray[i0].m_x);
						p = p & dgVector::m_triplexMask;
						dgFloat32 dist = p.DotProduct(dir).GetScalar();
						if (dist > frontSupportDist) {
							frontSupportDist = dist;
							vertex = p;
						}
					}
					if (frontSupportDist > maxProj) {
						maxProj = frontSupportDist;
						supportVertex = vertex; 
					}

				} else {
					dgVector box[2];
					const dgNode* const node = me->m_right.GetNode(m_aabb);
					box[0].m_x = boxArray[node->m_indexBox0].m_x;
					box[0].m_y = boxArray[node->m_indexBox0].m_y;
					box[0].m_z = boxArray[node->m_indexBox0].m_z;
					box[1].m_x = boxArray[node->m_indexBox1].m_x;
					box[1].m_y = boxArray[node->m_indexBox1].m_y;
					box[1].m_z = boxArray[node->m_indexBox1].m_z;

					dgVector supportPoint (box[ix].m_x, box[iy].m_y, box[iz].m_z, dgFloat32 (0.0f));
					frontSupportDist = supportPoint.DotProduct(dir).GetScalar();
				}

				if (frontSupportDist >= backSupportDist) {
					if (!me->m_left.IsLeaf()) {
						aabbProjection[stack] = backSupportDist;
						stackPool[stack] = me->m_left.GetNode(m_aabb);
						stack++;
					}

					if (!me->m_right.IsLeaf()) {
						aabbProjection[stack] = frontSupportDist;
						stackPool[stack] = me->m_right.GetNode(m_aabb);
						stack++;
					}

				} else {

					if (!me->m_right.IsLeaf()) {
						aabbProjection[stack] = frontSupportDist;
						stackPool[stack] = me->m_right.GetNode(m_aabb);
						stack++;
					}

					if (!me->m_left.IsLeaf()) {
						aabbProjection[stack] = backSupportDist;
						stackPool[stack] = me->m_left.GetNode(m_aabb);
						stack++;
					}
				}
			}
		}
	}
	return supportVertex;
}


void dgAABBPolygonSoup::ForAllSectorsRayHit (const dgFastRayTest& raySrc, dgFloat32 maxParam, dgRayIntersectCallback callback, void* const context) const
{
	const dgNode *stackPool[DG_STACK_DEPTH];
	dgFloat32 distance[DG_STACK_DEPTH];
	dgFastRayTest ray (raySrc);

	dgInt32 stack = 1;
	const dgTriplex* const vertexArray = (dgTriplex*) m_localVertex;

	stackPool[0] = m_aabb;
	distance[0] = m_aabb->RayDistance(ray, vertexArray);
	while (stack) {
		stack --;
		dgFloat32 dist = distance[stack];
		if (dist > maxParam) {
			break;
		} else {
			const dgNode *const me = stackPool[stack];
			if (me->m_left.IsLeaf()) {
				dgInt32 vCount = dgInt32 (me->m_left.GetCount());
				if (vCount > 0) {
					dgInt32 index = dgInt32 (me->m_left.GetIndex());
					dgFloat32 param = callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), &m_indices[index], vCount);
					dgAssert (param >= dgFloat32 (0.0f));
					if (param < maxParam) {
						maxParam = param;
						if (maxParam == dgFloat32 (0.0f)) {
							break;
						}
					}
				}

			} else {
				const dgNode* const node = me->m_left.GetNode(m_aabb);
				dgFloat32 dist1 = node->RayDistance(ray, vertexArray);
				if (dist1 < maxParam) {
					dgInt32 j = stack;
					for ( ; j && (dist1 > distance[j - 1]); j --) {
						stackPool[j] = stackPool[j - 1];
						distance[j] = distance[j - 1];
					}
					dgAssert (stack < DG_STACK_DEPTH);
					stackPool[j] = node;
					distance[j] = dist1;
					stack++;
				}
			}

			if (me->m_right.IsLeaf()) {
				dgInt32 vCount = dgInt32 (me->m_right.GetCount());
				if (vCount > 0) {
					dgInt32 index = dgInt32 (me->m_right.GetIndex());
					dgFloat32 param = callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), &m_indices[index], vCount);
					dgAssert (param >= dgFloat32 (0.0f));
					if (param < maxParam) {
						maxParam = param;
						if (maxParam == dgFloat32 (0.0f)) {
							break;
						}
					}
				}

			} else {
				const dgNode* const node = me->m_right.GetNode(m_aabb);
				dgFloat32 dist1 = node->RayDistance(ray, vertexArray);
				if (dist1 < maxParam) {
					dgInt32 j = stack;
					for ( ; j && (dist1 > distance[j - 1]); j --) {
						stackPool[j] = stackPool[j - 1];
						distance[j] = distance[j - 1];
					}
					dgAssert (stack < DG_STACK_DEPTH);
					stackPool[j] = node;
					distance[j] = dist1;
					stack++;
				}
			}
		}
	}
}

void dgAABBPolygonSoup::ForAllSectors (const dgFastAABBInfo& obbAabbInfo, const dgVector& boxDistanceTravel, dgFloat32 m_maxT, dgAABBIntersectCallback callback, void* const context) const
{
	dgAssert (dgAbs(dgAbs(obbAabbInfo[0][0]) - obbAabbInfo.m_absDir[0][0]) < dgFloat32 (1.0e-4f));
	dgAssert (dgAbs(dgAbs(obbAabbInfo[1][1]) - obbAabbInfo.m_absDir[1][1]) < dgFloat32 (1.0e-4f));
	dgAssert (dgAbs(dgAbs(obbAabbInfo[2][2]) - obbAabbInfo.m_absDir[2][2]) < dgFloat32 (1.0e-4f));

	dgAssert (dgAbs(dgAbs(obbAabbInfo[0][1]) - obbAabbInfo.m_absDir[1][0]) < dgFloat32 (1.0e-4f));
	dgAssert (dgAbs(dgAbs(obbAabbInfo[0][2]) - obbAabbInfo.m_absDir[2][0]) < dgFloat32 (1.0e-4f));
	dgAssert (dgAbs(dgAbs(obbAabbInfo[1][2]) - obbAabbInfo.m_absDir[2][1]) < dgFloat32 (1.0e-4f));

	if (m_aabb) {
		dgFloat32 distance[DG_STACK_DEPTH];
		const dgNode* stackPool[DG_STACK_DEPTH];

		const dgInt32 stride = sizeof (dgTriplex) / sizeof (dgFloat32);
		const dgTriplex* const vertexArray = (dgTriplex*) m_localVertex;

		dgAssert (boxDistanceTravel.m_w == dgFloat32 (0.0f));
		if (boxDistanceTravel.DotProduct(boxDistanceTravel).GetScalar() < dgFloat32 (1.0e-8f)) {

			dgInt32 stack = 1;
			stackPool[0] = m_aabb;
			distance[0] = m_aabb->BoxPenetration(obbAabbInfo, vertexArray);
			if (distance[0] <= dgFloat32(0.0f)) {
				obbAabbInfo.m_separationDistance = dgMin(obbAabbInfo.m_separationDistance[0], -distance[0]);
			}
			while (stack) {
				stack --;
				dgFloat32 dist = distance[stack];
				if (dist > dgFloat32 (0.0f)) {
					const dgNode* const me = stackPool[stack];
					if (me->m_left.IsLeaf()) {
						dgInt32 index = dgInt32 (me->m_left.GetIndex());
						dgInt32 vCount = dgInt32 (me->m_left.GetCount());
						if (vCount > 0) {
							const dgInt32* const indices = &m_indices[index];
							dgInt32 normalIndex = indices[vCount + 1];
							dgVector faceNormal (&vertexArray[normalIndex].m_x);
							faceNormal = faceNormal & dgVector::m_triplexMask;
							dgFloat32 dist1 = obbAabbInfo.PolygonBoxDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x);
							if (dist1 > dgFloat32 (0.0f)) {
								obbAabbInfo.m_separationDistance = dgFloat32(0.0f);
								dgAssert (vCount >= 3);
								if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, dist1) == t_StopSearh) {
									return;
								}
							} else {
								obbAabbInfo.m_separationDistance = dgMin(obbAabbInfo.m_separationDistance[0], -dist1);
							}
						}

					} else {
						const dgNode* const node = me->m_left.GetNode(m_aabb);
						dgFloat32 dist1 = node->BoxPenetration(obbAabbInfo, vertexArray);
						if (dist1 > dgFloat32 (0.0f)) {
							dgInt32 j = stack;
							for ( ; j && (dist1 > distance[j - 1]); j --) {
								stackPool[j] = stackPool[j - 1];
								distance[j] = distance[j - 1];
							}
							dgAssert (stack < DG_STACK_DEPTH);
							stackPool[j] = node;
							distance[j] = dist1;
							stack++;
						} else {
							obbAabbInfo.m_separationDistance = dgMin(obbAabbInfo.m_separationDistance[0], -dist1);
						}
					}

					if (me->m_right.IsLeaf()) {
						dgInt32 index = dgInt32 (me->m_right.GetIndex());
						dgInt32 vCount = dgInt32 (me->m_right.GetCount());
						if (vCount > 0) {
							const dgInt32* const indices = &m_indices[index];
							dgInt32 normalIndex = indices[vCount + 1];
							dgVector faceNormal (&vertexArray[normalIndex].m_x);
							faceNormal = faceNormal & dgVector::m_triplexMask;
							dgFloat32 dist1 = obbAabbInfo.PolygonBoxDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x);
							if (dist1 > dgFloat32 (0.0f)) {
								dgAssert (vCount >= 3);
								obbAabbInfo.m_separationDistance = dgFloat32(0.0f);
								if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, dist1) == t_StopSearh) {
									return;
								}
							} else {
								obbAabbInfo.m_separationDistance = dgMin(obbAabbInfo.m_separationDistance[0], -dist1);
							}
						}

					} else {
						const dgNode* const node = me->m_right.GetNode(m_aabb);
						dgFloat32 dist1 = node->BoxPenetration(obbAabbInfo, vertexArray);
						if (dist1 > dgFloat32 (0.0f)) {
							dgInt32 j = stack;
							for ( ; j && (dist1 > distance[j - 1]); j --) {
								stackPool[j] = stackPool[j - 1];
								distance[j] = distance[j - 1];
							}
							dgAssert (stack < DG_STACK_DEPTH);
							stackPool[j] = node;
							distance[j] = dist1;
							stack++;
						} else {
							obbAabbInfo.m_separationDistance = dgMin(obbAabbInfo.m_separationDistance[0], -dist1);
						}
					}
				}
			}

		} else {
			dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), boxDistanceTravel);
			dgFastRayTest obbRay (dgVector (dgFloat32 (0.0f)), obbAabbInfo.UnrotateVector(boxDistanceTravel));
			dgInt32 stack = 1;
			stackPool[0] = m_aabb;
			distance [0] = m_aabb->BoxIntersect (ray, obbRay, obbAabbInfo, vertexArray);

			while (stack) {
				stack --;
				const dgFloat32 dist = distance[stack];
				const dgNode* const me = stackPool[stack];
				if (dist < dgFloat32 (1.0f)) {

					if (me->m_left.IsLeaf()) {
						dgInt32 index = dgInt32 (me->m_left.GetIndex());
						dgInt32 vCount = dgInt32 (me->m_left.GetCount());
						if (vCount > 0) {
							const dgInt32* const indices = &m_indices[index];
							dgInt32 normalIndex = indices[vCount + 1];
							dgVector faceNormal (&vertexArray[normalIndex].m_x);
							faceNormal = faceNormal & dgVector::m_triplexMask;
							dgFloat32 hitDistance = obbAabbInfo.PolygonBoxRayDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x, ray);
							if (hitDistance < dgFloat32 (1.0f)) {
								dgAssert (vCount >= 3);
								if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, hitDistance) == t_StopSearh) {
									return;
								}
							}
						}

					} else {
						const dgNode* const node = me->m_left.GetNode(m_aabb);
						dgFloat32 dist1 = node->BoxIntersect (ray, obbRay, obbAabbInfo, vertexArray);
						if (dist1 < dgFloat32 (1.0f)) {
							dgInt32 j = stack;
							for ( ; j && (dist1 > distance[j - 1]); j --) {
								stackPool[j] = stackPool[j - 1];
								distance[j] = distance[j - 1];
							}
							dgAssert (stack < DG_STACK_DEPTH);
							stackPool[j] = node;
							distance[j] = dist1;
							stack++;
						}
					}

					if (me->m_right.IsLeaf()) {
						dgInt32 index = dgInt32 (me->m_right.GetIndex());
						dgInt32 vCount = dgInt32 (me->m_right.GetCount());
						if (vCount > 0) {
							const dgInt32* const indices = &m_indices[index];
							dgInt32 normalIndex = indices[vCount + 1];
							dgVector faceNormal (&vertexArray[normalIndex].m_x);
							faceNormal = faceNormal & dgVector::m_triplexMask;
							dgFloat32 hitDistance = obbAabbInfo.PolygonBoxRayDistance (faceNormal, vCount, indices, stride, &vertexArray[0].m_x, ray);
							if (hitDistance < dgFloat32 (1.0f)) {
								dgAssert (vCount >= 3);
								if (callback(context, &vertexArray[0].m_x, sizeof (dgTriplex), indices, vCount, hitDistance) == t_StopSearh) {
									return;
								}
							}
						}

					} else {
						const dgNode* const node = me->m_right.GetNode(m_aabb);
						dgFloat32 dist1 = node->BoxIntersect (ray, obbRay, obbAabbInfo, vertexArray);
						if (dist1 < dgFloat32 (1.0f)) {
							dgInt32 j = stack;
							for ( ; j && (dist1 > distance[j - 1]); j --) {
								stackPool[j] = stackPool[j - 1];
								distance[j] = distance[j - 1];
							}
							dgAssert (stack < DG_STACK_DEPTH);
							stackPool[j] = node;
							distance[j] = dist1;
							stack ++;
						}
					}
				}
			}
		}
	}
}