Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 2433 insertions, 0 deletions

diff --git a/APEX_1.4/shared/general/HACD/src/dgPolyhedra.cpp b/APEX_1.4/shared/general/HACD/src/dgPolyhedra.cpp
new file mode 100644
index 00000000..5e43af37
--- /dev/null
+++ b/APEX_1.4/shared/general/HACD/src/dgPolyhedra.cpp
@@ -0,0 +1,2433 @@
+/* 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 "dgTypes.h"
+#include "dgHeap.h"
+#include "dgStack.h"
+#include "dgSphere.h"
+#include "dgPolyhedra.h"
+#include "dgConvexHull3d.h"
+#include "dgSmallDeterminant.h"
+#include <string.h>
+
+
+//#define DG_MIN_EDGE_ASPECT_RATIO  double (0.02f)
+
+class dgDiagonalEdge
+{
+	public:
+	dgDiagonalEdge (dgEdge* const edge)
+		:m_i0(edge->m_incidentVertex), m_i1(edge->m_twin->m_incidentVertex)
+	{
+	}
+	int32_t m_i0;
+	int32_t m_i1;
+};
+
+
+struct dgEdgeCollapseEdgeHandle
+{
+	dgEdgeCollapseEdgeHandle (dgEdge* const newEdge)
+		:m_inList(false), m_edge(newEdge)
+	{
+	}
+
+	dgEdgeCollapseEdgeHandle (const dgEdgeCollapseEdgeHandle &dataHandle)
+		:m_inList(true), m_edge(dataHandle.m_edge)
+	{
+		dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *)IntToPointer (m_edge->m_userData);
+		if (handle) {
+			HACD_ASSERT (handle != this);
+			handle->m_edge = NULL;
+		}
+		m_edge->m_userData = uint64_t (PointerToInt(this));
+	}
+
+	~dgEdgeCollapseEdgeHandle ()
+	{
+		if (m_inList) {
+			if (m_edge) {
+				dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *)IntToPointer (m_edge->m_userData);
+				if (handle == this) {
+					m_edge->m_userData = PointerToInt (NULL);
+				}
+			}
+		}
+		m_edge = NULL;
+	}
+
+	uint32_t m_inList;
+	dgEdge* m_edge;
+};
+
+
+class dgVertexCollapseVertexMetric
+{
+	public:
+	double elem[10];
+
+	dgVertexCollapseVertexMetric (const dgBigPlane &plane) 
+	{
+		elem[0] = plane.m_x * plane.m_x;  
+		elem[1] = plane.m_y * plane.m_y;  
+		elem[2] = plane.m_z * plane.m_z;  
+		elem[3] = plane.m_w * plane.m_w;  
+		elem[4] = double (2.0) * plane.m_x * plane.m_y;  
+		elem[5] = double (2.0) * plane.m_x * plane.m_z;  
+		elem[6] = double (2.0) * plane.m_x * plane.m_w;  
+		elem[7] = double (2.0) * plane.m_y * plane.m_z;  
+		elem[8] = double (2.0) * plane.m_y * plane.m_w;  
+		elem[9] = double (2.0) * plane.m_z * plane.m_w;  
+	}
+
+	void Clear ()
+	{
+		memset (elem, 0, 10 * sizeof (double));
+	}
+
+	void Accumulate (const dgVertexCollapseVertexMetric& p) 
+	{
+		elem[0] += p.elem[0]; 
+		elem[1] += p.elem[1]; 
+		elem[2] += p.elem[2]; 
+		elem[3] += p.elem[3]; 
+		elem[4] += p.elem[4]; 
+		elem[5] += p.elem[5]; 
+		elem[6] += p.elem[6]; 
+		elem[7] += p.elem[7]; 
+		elem[8] += p.elem[8]; 
+		elem[9] += p.elem[9]; 
+	}
+
+	void Accumulate (const dgBigPlane& plane) 
+	{
+		elem[0] += plane.m_x * plane.m_x;  
+		elem[1] += plane.m_y * plane.m_y;  
+		elem[2] += plane.m_z * plane.m_z;  
+		elem[3] += plane.m_w * plane.m_w;  
+
+		elem[4] += double (2.0f) * plane.m_x * plane.m_y;  
+		elem[5] += double (2.0f) * plane.m_x * plane.m_z;  
+		elem[7] += double (2.0f) * plane.m_y * plane.m_z;  
+
+		elem[6] += double (2.0f) * plane.m_x * plane.m_w;  
+		elem[8] += double (2.0f) * plane.m_y * plane.m_w;  
+		elem[9] += double (2.0f) * plane.m_z * plane.m_w;  
+	}
+
+
+	double Evalue (const dgVector &p) const 
+	{
+		double acc = elem[0] * p.m_x * p.m_x + elem[1] * p.m_y * p.m_y + elem[2] * p.m_z * p.m_z + 
+						elem[4] * p.m_x * p.m_y + elem[5] * p.m_x * p.m_z + elem[7] * p.m_y * p.m_z + 
+						elem[6] * p.m_x + elem[8] * p.m_y + elem[9] * p.m_z + elem[3];  
+		return fabs (acc);
+	}
+};
+
+
+
+dgPolyhedra::dgPolyhedra (void)
+	:dgTree <dgEdge, int64_t>()
+	,m_baseMark(0)
+	,m_edgeMark(0)
+	,m_faceSecuence(0)
+{
+}
+
+dgPolyhedra::dgPolyhedra (const dgPolyhedra &polyhedra)
+	:dgTree <dgEdge, int64_t>()
+	,m_baseMark(0)
+	,m_edgeMark(0)
+	,m_faceSecuence(0)
+{
+	dgStack<int32_t> indexPool (1024 * 16);
+	dgStack<uint64_t> userPool (1024 * 16);
+	int32_t* const index = &indexPool[0];
+	uint64_t* const user = &userPool[0];
+
+	BeginFace ();
+	Iterator iter(polyhedra);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_incidentFace < 0) {
+			continue;
+		}
+
+		if (!FindEdge(edge->m_incidentVertex, edge->m_twin->m_incidentVertex))	{
+			int32_t indexCount = 0;
+			dgEdge* ptr = edge;
+			do {
+				user[indexCount] = ptr->m_userData;
+				index[indexCount] = ptr->m_incidentVertex;
+				indexCount ++;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+
+			dgEdge* const face = AddFace (indexCount, index, (int64_t*) user);
+			ptr = face;
+			do {
+				ptr->m_incidentFace = edge->m_incidentFace;
+				ptr = ptr->m_next;
+			} while (ptr != face);
+		}
+	}
+	EndFace();
+
+	m_faceSecuence = polyhedra.m_faceSecuence;
+
+#ifdef __ENABLE_SANITY_CHECK 
+	HACD_ASSERT (SanityCheck());
+#endif
+}
+
+dgPolyhedra::~dgPolyhedra ()
+{
+}
+
+
+int32_t dgPolyhedra::GetFaceCount() const
+{
+	Iterator iter (*this);
+	int32_t count = 0;
+	int32_t mark = IncLRU();
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_mark == mark) {
+			continue;
+		}
+
+		if (edge->m_incidentFace < 0) {
+			continue;
+		}
+
+		count ++;
+		dgEdge* ptr = edge;
+		do {
+			ptr->m_mark = mark;
+			ptr = ptr->m_next;
+		} while (ptr != edge);
+	}
+	return count;
+}
+
+
+
+dgEdge* dgPolyhedra::AddFace ( int32_t count, const int32_t* const index, const int64_t* const userdata)
+{
+	class IntersectionFilter
+	{
+		public:
+		IntersectionFilter ()
+		{
+			m_count = 0;
+		}
+
+		bool Insert (int32_t /*dummy*/, int64_t value)
+		{
+			int32_t i;				
+			for (i = 0 ; i < m_count; i ++) {
+				if (m_array[i] == value) {
+					return false;
+				}
+			}
+			m_array[i] = value;
+			m_count ++;
+			return true;
+		}
+
+		int32_t m_count;
+		int64_t m_array[2048];
+	};
+
+	IntersectionFilter selfIntersectingFaceFilter;
+
+	int32_t dummyValues = 0;
+	int32_t i0 = index[count-1];
+	for (int32_t i = 0; i < count; i ++) {
+		int32_t i1 = index[i];
+		dgPairKey code0 (i0, i1);
+
+		if (!selfIntersectingFaceFilter.Insert (dummyValues, code0.GetVal())) {
+			return NULL;
+		}
+
+		dgPairKey code1 (i1, i0);
+		if (!selfIntersectingFaceFilter.Insert (dummyValues, code1.GetVal())) {
+			return NULL;
+		}
+
+
+		if (i0 == i1) {
+			return NULL;
+		}
+		if (FindEdge (i0, i1)) {
+			return NULL;
+		}
+		i0 = i1;
+	}
+
+	m_faceSecuence ++;
+
+	i0 = index[count-1];
+	int32_t i1 = index[0];
+	uint64_t udata0 = 0;
+	uint64_t udata1 = 0;
+	if (userdata) {
+		udata0 = uint64_t (userdata[count-1]);
+		udata1 = uint64_t (userdata[0]);
+	} 
+
+	bool state;
+	dgPairKey code (i0, i1);
+	dgEdge tmpEdge (i0, m_faceSecuence, udata0);
+	dgTreeNode* node = Insert (tmpEdge, code.GetVal(), state); 
+	HACD_ASSERT (!state);
+	dgEdge* edge0 = &node->GetInfo();
+	dgEdge* const first = edge0;
+
+	for (int32_t i = 1; i < count; i ++) {
+		i0 = i1;
+		i1 = index[i];
+		udata0 = udata1;
+		udata1 = uint64_t (userdata ? userdata[i] : 0);
+
+		dgPairKey code (i0, i1);
+		dgEdge tmpEdge (i0, m_faceSecuence, udata0);
+		node = Insert (tmpEdge, code.GetVal(), state); 
+		HACD_ASSERT (!state);
+
+		dgEdge* const edge1 = &node->GetInfo();
+		edge0->m_next = edge1;
+		edge1->m_prev = edge0;
+		edge0 = edge1;
+	}
+
+	first->m_prev = edge0;
+	edge0->m_next = first;
+
+	return first->m_next;
+}
+
+
+void dgPolyhedra::EndFace ()
+{
+	dgPolyhedra::Iterator iter (*this);
+
+	// Connect all twin edge
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (!edge->m_twin) {
+			edge->m_twin = FindEdge (edge->m_next->m_incidentVertex, edge->m_incidentVertex);
+			if (edge->m_twin) {
+				edge->m_twin->m_twin = edge; 
+			}
+		}
+	}
+
+#ifdef __ENABLE_SANITY_CHECK 
+	HACD_ASSERT (SanityCheck());
+#endif
+	dgStack<dgEdge*> edgeArrayPool(GetCount() * 2 + 256);
+
+	int32_t edgeCount = 0;
+	dgEdge** const edgeArray = &edgeArrayPool[0];
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (!edge->m_twin) {
+			bool state;
+			dgPolyhedra::dgPairKey code (edge->m_next->m_incidentVertex, edge->m_incidentVertex);
+			dgEdge tmpEdge (edge->m_next->m_incidentVertex, -1);
+			tmpEdge.m_incidentFace = -1; 
+			dgPolyhedra::dgTreeNode* const node = Insert (tmpEdge, code.GetVal(), state); 
+			HACD_ASSERT (!state);
+			edge->m_twin = &node->GetInfo();
+			edge->m_twin->m_twin = edge; 
+			edgeArray[edgeCount] = edge->m_twin;
+			edgeCount ++;
+		}
+	}
+
+	for (int32_t i = 0; i < edgeCount; i ++) {
+		dgEdge* const edge = edgeArray[i];
+		HACD_ASSERT (!edge->m_prev);
+		dgEdge *ptr = edge->m_twin;
+		for (; ptr->m_next; ptr = ptr->m_next->m_twin){}
+		ptr->m_next = edge;
+		edge->m_prev = ptr;
+	}
+
+#ifdef __ENABLE_SANITY_CHECK 
+	HACD_ASSERT (SanityCheck ());
+#endif
+}
+
+
+void dgPolyhedra::DeleteFace(dgEdge* const face)
+{
+	dgEdge* edgeList[1024 * 16];
+
+	if (face->m_incidentFace > 0) {
+		int32_t count = 0;
+		dgEdge* ptr = face;
+		do {
+			ptr->m_incidentFace = -1;
+			int32_t i = 0;
+			for (; i < count; i ++) {
+				if ((edgeList[i] == ptr) || (edgeList[i]->m_twin == ptr)) {
+					break;
+				}
+			}
+			if (i == count) {
+				edgeList[count] = ptr;
+				count ++;
+			}
+			ptr = ptr->m_next;
+		} while (ptr != face);
+
+
+		for (int32_t i = 0; i < count; i ++) {
+			dgEdge* const ptr = edgeList[i];
+			if (ptr->m_twin->m_incidentFace < 0) {
+				DeleteEdge (ptr);
+			}
+		}
+	}
+}
+
+
+
+dgBigVector dgPolyhedra::FaceNormal (dgEdge* const face, const double* const pool, int32_t strideInBytes) const
+{
+	int32_t stride = strideInBytes / (int32_t)sizeof (double);
+	dgEdge* edge = face;
+	dgBigVector p0 (&pool[edge->m_incidentVertex * stride]);
+	edge = edge->m_next;
+	dgBigVector p1 (&pool[edge->m_incidentVertex * stride]);
+	dgBigVector e1 (p1 - p0);
+
+	dgBigVector normal (float (0.0f), float (0.0f), float (0.0f), float (0.0f));
+	for (edge = edge->m_next; edge != face; edge = edge->m_next) {
+		dgBigVector p2 (&pool[edge->m_incidentVertex * stride]);
+		dgBigVector e2 (p2 - p0);
+		normal += e1 * e2;
+		e1 = e2;
+	} 
+	return normal;
+}
+
+
+dgEdge* dgPolyhedra::AddHalfEdge (int32_t v0, int32_t v1)
+{
+	if (v0 != v1) {
+		dgPairKey pairKey (v0, v1);
+		dgEdge tmpEdge (v0, -1);
+
+		dgTreeNode* node = Insert (tmpEdge, pairKey.GetVal()); 
+		return node ? &node->GetInfo() : NULL;
+	} else {
+		return NULL;
+	}
+}
+
+
+void dgPolyhedra::DeleteEdge (dgEdge* const edge)
+{
+	dgEdge *const twin = edge->m_twin;
+
+	edge->m_prev->m_next = twin->m_next;
+	twin->m_next->m_prev = edge->m_prev;
+	edge->m_next->m_prev = twin->m_prev;
+	twin->m_prev->m_next = edge->m_next;
+
+	dgTreeNode *const nodeA = GetNodeFromInfo (*edge);
+	dgTreeNode *const nodeB = GetNodeFromInfo (*twin);
+
+	HACD_ASSERT (&nodeA->GetInfo() == edge);
+	HACD_ASSERT (&nodeB->GetInfo() == twin);
+
+	Remove (nodeA);
+	Remove (nodeB);
+}
+
+
+dgEdge* dgPolyhedra::SpliteEdge (int32_t newIndex,	dgEdge* const edge)
+{
+	dgEdge* const edge00 = edge->m_prev;
+	dgEdge* const edge01 = edge->m_next;
+	dgEdge* const twin00 = edge->m_twin->m_next;
+	dgEdge* const twin01 = edge->m_twin->m_prev;
+
+	int32_t i0 = edge->m_incidentVertex;
+	int32_t i1 = edge->m_twin->m_incidentVertex;
+
+	int32_t f0 = edge->m_incidentFace;
+	int32_t f1 = edge->m_twin->m_incidentFace;
+
+	DeleteEdge (edge);
+
+	dgEdge* const edge0 = AddHalfEdge (i0, newIndex);
+	dgEdge* const edge1 = AddHalfEdge (newIndex, i1);
+
+	dgEdge* const twin0 = AddHalfEdge (newIndex, i0);
+	dgEdge* const twin1 = AddHalfEdge (i1, newIndex);
+	HACD_ASSERT (edge0);
+	HACD_ASSERT (edge1);
+	HACD_ASSERT (twin0);
+	HACD_ASSERT (twin1);
+
+	edge0->m_twin = twin0;
+	twin0->m_twin = edge0;
+
+	edge1->m_twin = twin1;
+	twin1->m_twin = edge1;
+
+	edge0->m_next = edge1;
+	edge1->m_prev = edge0;
+
+	twin1->m_next = twin0;
+	twin0->m_prev = twin1;
+
+	edge0->m_prev = edge00;
+	edge00 ->m_next = edge0;
+
+	edge1->m_next = edge01;
+	edge01->m_prev = edge1;
+
+	twin0->m_next = twin00;
+	twin00->m_prev = twin0;
+
+	twin1->m_prev = twin01;
+	twin01->m_next = twin1;
+
+	edge0->m_incidentFace = f0;
+	edge1->m_incidentFace = f0;
+
+	twin0->m_incidentFace = f1;
+	twin1->m_incidentFace = f1;
+
+#ifdef __ENABLE_SANITY_CHECK 
+	//	HACD_ASSERT (SanityCheck ());
+#endif
+
+	return edge0;
+}
+
+
+
+bool dgPolyhedra::FlipEdge (dgEdge* const edge)
+{
+	//	dgTreeNode *node;
+	if (edge->m_next->m_next->m_next != edge) {
+		return false;
+	}
+
+	if (edge->m_twin->m_next->m_next->m_next != edge->m_twin) {
+		return false;
+	}
+
+	if (FindEdge(edge->m_prev->m_incidentVertex, edge->m_twin->m_prev->m_incidentVertex)) {
+		return false;
+	}
+
+	dgEdge *const prevEdge = edge->m_prev;
+	dgEdge *const prevTwin = edge->m_twin->m_prev;
+
+	dgPairKey edgeKey (prevTwin->m_incidentVertex, prevEdge->m_incidentVertex);
+	dgPairKey twinKey (prevEdge->m_incidentVertex, prevTwin->m_incidentVertex);
+
+	ReplaceKey (GetNodeFromInfo (*edge), edgeKey.GetVal());
+	//	HACD_ASSERT (node);
+
+	ReplaceKey (GetNodeFromInfo (*edge->m_twin), twinKey.GetVal());
+	//	HACD_ASSERT (node);
+
+	edge->m_incidentVertex = prevTwin->m_incidentVertex;
+	edge->m_twin->m_incidentVertex = prevEdge->m_incidentVertex;
+
+	edge->m_userData = prevTwin->m_userData;
+	edge->m_twin->m_userData = prevEdge->m_userData;
+
+	prevEdge->m_next = edge->m_twin->m_next;
+	prevTwin->m_prev->m_prev = edge->m_prev;
+
+	prevTwin->m_next = edge->m_next;
+	prevEdge->m_prev->m_prev = edge->m_twin->m_prev;
+
+	edge->m_prev = prevTwin->m_prev;
+	edge->m_next = prevEdge;
+
+	edge->m_twin->m_prev = prevEdge->m_prev;
+	edge->m_twin->m_next = prevTwin;
+
+	prevTwin->m_prev->m_next = edge;
+	prevTwin->m_prev = edge->m_twin;
+
+	prevEdge->m_prev->m_next = edge->m_twin;
+	prevEdge->m_prev = edge;
+
+	edge->m_next->m_incidentFace = edge->m_incidentFace;
+	edge->m_prev->m_incidentFace = edge->m_incidentFace;
+
+	edge->m_twin->m_next->m_incidentFace = edge->m_twin->m_incidentFace;
+	edge->m_twin->m_prev->m_incidentFace = edge->m_twin->m_incidentFace;
+
+
+#ifdef __ENABLE_SANITY_CHECK 
+	HACD_ASSERT (SanityCheck ());
+#endif
+
+	return true;
+}
+
+
+
+bool dgPolyhedra::GetConectedSurface (dgPolyhedra &polyhedra) const
+{
+	if (!GetCount()) {
+		return false;
+	}
+
+	dgEdge* edge = NULL;
+	Iterator iter(*this);
+	for (iter.Begin (); iter; iter ++) {
+		edge = &(*iter);
+		if ((edge->m_mark < m_baseMark) && (edge->m_incidentFace > 0)) {
+			break;
+		}
+	}
+
+	if (!iter) {
+		return false;
+	}
+
+	int32_t faceIndex[4096];
+	int64_t faceDataIndex[4096];
+	dgStack<dgEdge*> stackPool (GetCount()); 
+	dgEdge** const stack = &stackPool[0];
+
+	int32_t mark = IncLRU();
+
+	polyhedra.BeginFace ();
+	stack[0] = edge;
+	int32_t index = 1;
+	while (index) {
+		index --;
+		dgEdge* const edge = stack[index];
+
+		if (edge->m_mark == mark) {
+			continue;
+		}
+
+		int32_t count = 0;
+		dgEdge* ptr = edge;
+		do {
+			ptr->m_mark = mark;
+			faceIndex[count] = ptr->m_incidentVertex;
+			faceDataIndex[count] = int64_t (ptr->m_userData);
+			count ++;
+			HACD_ASSERT (count <  int32_t ((sizeof (faceIndex)/sizeof(faceIndex[0]))));
+
+			if ((ptr->m_twin->m_incidentFace > 0) && (ptr->m_twin->m_mark != mark)) {
+				stack[index] = ptr->m_twin;
+				index ++;
+				HACD_ASSERT (index < GetCount());
+			}
+
+			ptr = ptr->m_next;
+		} while (ptr != edge);
+
+		polyhedra.AddFace (count, &faceIndex[0], &faceDataIndex[0]);
+	}
+
+	polyhedra.EndFace ();
+
+	return true;
+}
+
+
+void dgPolyhedra::ChangeEdgeIncidentVertex (dgEdge* const edge, int32_t newIndex)
+{
+	dgEdge* ptr = edge;
+	do {
+		dgTreeNode* node = GetNodeFromInfo(*ptr);
+		dgPairKey Key0 (newIndex, ptr->m_twin->m_incidentVertex);
+		ReplaceKey (node, Key0.GetVal());
+
+		node = GetNodeFromInfo(*ptr->m_twin);
+		dgPairKey Key1 (ptr->m_twin->m_incidentVertex, newIndex);
+		ReplaceKey (node, Key1.GetVal());
+
+		ptr->m_incidentVertex = newIndex;
+
+		ptr = ptr->m_twin->m_next;
+	} while (ptr != edge);
+}
+
+
+void dgPolyhedra::DeleteDegenerateFaces (const double* const pool, int32_t strideInBytes, double area)
+{
+	if (!GetCount()) {
+		return;
+	}
+
+#ifdef __ENABLE_SANITY_CHECK 
+	HACD_ASSERT (SanityCheck ());
+#endif
+	dgStack <dgPolyhedra::dgTreeNode*> faceArrayPool(GetCount() / 2 + 100);
+
+	int32_t count = 0;
+	dgPolyhedra::dgTreeNode** const faceArray = &faceArrayPool[0];
+	int32_t mark = IncLRU();
+	Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+
+		if ((edge->m_mark != mark) && (edge->m_incidentFace > 0)) {
+			faceArray[count] = iter.GetNode();
+			count ++;
+			dgEdge* ptr = edge;
+			do	{
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+		}
+	}
+
+	double area2 = area * area;
+	area2 *= double (4.0f);
+
+	for (int32_t i = 0; i < count; i ++) {
+		dgPolyhedra::dgTreeNode* const faceNode = faceArray[i];
+		dgEdge* const edge = &faceNode->GetInfo();
+
+		dgBigVector normal (FaceNormal (edge, pool, strideInBytes));
+
+		double faceArea = normal % normal;
+		if (faceArea < area2) {
+			DeleteFace (edge);
+		}
+	}
+
+#ifdef __ENABLE_SANITY_CHECK 
+	mark = IncLRU();
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if ((edge->m_mark != mark) && (edge->m_incidentFace > 0)) {
+			//HACD_ASSERT (edge->m_next->m_next->m_next == edge);
+			dgEdge* ptr = edge;
+			do	{
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+
+			dgBigVector normal (FaceNormal (edge, pool, strideInBytes));
+
+			double faceArea = normal % normal;
+			HACD_ASSERT (faceArea >= area2);
+		}
+	}
+	HACD_ASSERT (SanityCheck ());
+#endif
+}
+
+
+static void NormalizeVertex (int32_t count, dgBigVector* const dst, const double* const src, int32_t stride)
+{
+//	dgBigVector min;
+//	dgBigVector max;
+//	GetMinMax (min, max, src, count, int32_t (stride * sizeof (double)));
+//	dgBigVector centre (float (0.0f), float (0.0f), float (0.0f), float (0.0f));
+	for (int32_t i = 0; i < count; i ++) {
+//		dst[i].m_x = centre.m_x + src[i * stride + 0];
+//		dst[i].m_y = centre.m_y + src[i * stride + 1];
+//		dst[i].m_z = centre.m_z + src[i * stride + 2];
+		dst[i].m_x = src[i * stride + 0];
+		dst[i].m_y = src[i * stride + 1];
+		dst[i].m_z = src[i * stride + 2];
+		dst[i].m_w= double (0.0f);
+	}
+}
+
+static dgBigPlane EdgePlane (int32_t i0, int32_t i1, int32_t i2, const dgBigVector* const pool)
+{
+	const dgBigVector& p0 = pool[i0];
+	const dgBigVector& p1 = pool[i1];
+	const dgBigVector& p2 = pool[i2];
+
+	dgBigPlane plane (p0, p1, p2);
+	double mag = sqrt (plane % plane);
+	if (mag < double (1.0e-12f)) {
+		mag = double (1.0e-12f);
+	}
+	mag = double (1.0f) / mag;
+
+	plane.m_x *= mag;
+	plane.m_y *= mag;
+	plane.m_z *= mag;
+	plane.m_w *= mag;
+
+	return plane;
+}
+
+
+static dgBigPlane UnboundedLoopPlane (int32_t i0, int32_t i1, int32_t i2, const dgBigVector* const pool)
+{
+	const dgBigVector p0 = pool[i0];
+	const dgBigVector p1 = pool[i1];
+	const dgBigVector p2 = pool[i2];
+	dgBigVector E0 (p1 - p0); 
+	dgBigVector E1 (p2 - p0); 
+
+	dgBigVector N ((E0 * E1) * E0); 
+	double dist = - (N % p0);
+	dgBigPlane plane (N, dist);
+
+	double mag = sqrt (plane % plane);
+	if (mag < double (1.0e-12f)) {
+		mag = double (1.0e-12f);
+	}
+	mag = double (10.0f) / mag;
+
+	plane.m_x *= mag;
+	plane.m_y *= mag;
+	plane.m_z *= mag;
+	plane.m_w *= mag;
+
+	return plane;
+}
+
+
+static void CalculateAllMetrics (const dgPolyhedra* const polyhedra, dgVertexCollapseVertexMetric* const table, const dgBigVector* const pool)
+{
+	int32_t edgeMark = polyhedra->IncLRU();
+	dgPolyhedra::Iterator iter (*polyhedra);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+
+		HACD_ASSERT (edge);
+		if (edge->m_mark != edgeMark) {
+
+			if (edge->m_incidentFace > 0) {
+				int32_t i0 = edge->m_incidentVertex;
+				int32_t i1 = edge->m_next->m_incidentVertex;
+				int32_t i2 = edge->m_prev->m_incidentVertex;
+
+				dgBigPlane constrainPlane (EdgePlane (i0, i1, i2, pool));
+				dgVertexCollapseVertexMetric tmp (constrainPlane);
+
+				dgEdge* ptr = edge;
+				do {
+					ptr->m_mark = edgeMark;
+					i0 = ptr->m_incidentVertex;
+					table[i0].Accumulate(tmp);
+
+					ptr = ptr->m_next;
+				} while (ptr != edge);
+
+			} else {
+				HACD_ASSERT (edge->m_twin->m_incidentFace > 0);
+				int32_t i0 = edge->m_twin->m_incidentVertex;
+				int32_t i1 = edge->m_twin->m_next->m_incidentVertex;
+				int32_t i2 = edge->m_twin->m_prev->m_incidentVertex;
+
+				edge->m_mark = edgeMark;
+				dgBigPlane constrainPlane (UnboundedLoopPlane (i0, i1, i2, pool));
+				dgVertexCollapseVertexMetric tmp (constrainPlane);
+
+				i0 = edge->m_incidentVertex;
+				table[i0].Accumulate(tmp);
+
+				i0 = edge->m_twin->m_incidentVertex;
+				table[i0].Accumulate(tmp);
+			}
+		}
+	}
+}
+
+
+double dgPolyhedra::EdgePenalty (const dgBigVector* const pool, dgEdge* const edge) const
+{
+	int32_t i0 = edge->m_incidentVertex;
+	int32_t i1 = edge->m_next->m_incidentVertex;
+
+	const dgBigVector& p0 = pool[i0];
+	const dgBigVector& p1 = pool[i1];
+	dgBigVector dp (p1 - p0);
+
+	double dot = dp % dp;
+	if (dot < double(1.0e-6f)) {
+		return double (-1.0f);
+	}
+
+	if ((edge->m_incidentFace > 0) && (edge->m_twin->m_incidentFace > 0)) {
+		dgBigVector edgeNormal (FaceNormal (edge, &pool[0].m_x, sizeof (dgBigVector)));
+		dgBigVector twinNormal (FaceNormal (edge->m_twin, &pool[0].m_x, sizeof (dgBigVector)));
+
+		double mag0 = edgeNormal % edgeNormal;
+		double mag1 = twinNormal % twinNormal;
+		if ((mag0 < double (1.0e-24f)) || (mag1 < double (1.0e-24f))) {
+			return double (-1.0f);
+		}
+
+		edgeNormal = edgeNormal.Scale (double (1.0f) / sqrt(mag0));
+		twinNormal = twinNormal.Scale (double (1.0f) / sqrt(mag1));
+
+		dot = edgeNormal % twinNormal;
+		if (dot < double (-0.9f)) {
+			return float (-1.0f);
+		}
+
+		dgEdge* ptr = edge;
+		do {
+			if ((ptr->m_incidentFace <= 0) || (ptr->m_twin->m_incidentFace <= 0)){
+				dgEdge* const adj = edge->m_twin;
+				ptr = edge;
+				do {
+					if ((ptr->m_incidentFace <= 0) || (ptr->m_twin->m_incidentFace <= 0)){
+						return float (-1.0);
+					}
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != adj);
+			}
+			ptr = ptr->m_twin->m_next;
+		} while (ptr != edge);
+	}
+
+	int32_t faceA = edge->m_incidentFace;
+	int32_t faceB = edge->m_twin->m_incidentFace;
+
+	i0 = edge->m_twin->m_incidentVertex;
+	dgBigVector p (pool[i0].m_x, pool[i0].m_y, pool[i0].m_z, float (0.0f));
+
+	bool penalty = false;
+	dgEdge* ptr = edge;
+	do {
+		dgEdge* const adj = ptr->m_twin;
+
+		int32_t face = adj->m_incidentFace;
+		if ((face != faceB) && (face != faceA) && (face >= 0) && (adj->m_next->m_incidentFace == face) && (adj->m_prev->m_incidentFace == face)){
+
+			int32_t i0 = adj->m_next->m_incidentVertex;
+			const dgBigVector& p0 = pool[i0];
+
+			int32_t i1 = adj->m_incidentVertex;
+			const dgBigVector& p1 = pool[i1];
+
+			int32_t i2 = adj->m_prev->m_incidentVertex;
+			const dgBigVector& p2 = pool[i2];
+
+			dgBigVector n0 ((p1 - p0) * (p2 - p0));
+			dgBigVector n1 ((p1 - p) * (p2 - p));
+
+//			double mag0 = n0 % n0;
+//			HACD_ASSERT (mag0 > double(1.0e-16f));
+//			mag0 = sqrt (mag0);
+
+//			double mag1 = n1 % n1;
+//			mag1 = sqrt (mag1);
+
+			double dot = n0 % n1;
+			if (dot < double (0.0f)) {
+//			if (dot <= (mag0 * mag1 * float (0.707f)) || (mag0 > (double(16.0f) * mag1))) {
+				penalty = true;
+				break;
+			}
+		}
+
+		ptr = ptr->m_twin->m_next;
+	} while (ptr != edge);
+
+	double aspect = float (-1.0f);
+	if (!penalty) {
+		int32_t i0 = edge->m_twin->m_incidentVertex;
+		dgBigVector p0 (pool[i0]);
+
+		aspect = float (1.0f);
+		for (dgEdge* ptr = edge->m_twin->m_next->m_twin->m_next; ptr != edge; ptr = ptr->m_twin->m_next) {
+			if (ptr->m_incidentFace > 0) {
+				int32_t i0 = ptr->m_next->m_incidentVertex;
+				const dgBigVector& p1 = pool[i0];
+
+				int32_t i1 = ptr->m_prev->m_incidentVertex;
+				const dgBigVector& p2 = pool[i1];
+
+				dgBigVector e0 (p1 - p0);
+				dgBigVector e1 (p2 - p1);
+				dgBigVector e2 (p0 - p2);
+
+				double mag0 = e0 % e0;
+				double mag1 = e1 % e1;
+				double mag2 = e2 % e2;
+				double maxMag = GetMax (mag0, mag1, mag2);
+				double minMag = GetMin (mag0, mag1, mag2);
+				double ratio = minMag / maxMag;
+
+				if (ratio < aspect) {
+					aspect = ratio;
+				}
+			}
+		}
+		aspect = sqrt (aspect);
+		//aspect = 1.0f;
+	}
+
+	return aspect;
+}
+
+static void CalculateVertexMetrics (dgVertexCollapseVertexMetric table[], const dgBigVector* const pool, dgEdge* const edge)
+{
+	int32_t i0 = edge->m_incidentVertex;
+
+//	const dgBigVector& p0 = pool[i0];
+	table[i0].Clear ();
+	dgEdge* ptr = edge;
+	do {
+
+		if (ptr->m_incidentFace > 0) {
+			int32_t i1 = ptr->m_next->m_incidentVertex;
+			int32_t i2 = ptr->m_prev->m_incidentVertex;
+			dgBigPlane constrainPlane (EdgePlane (i0, i1, i2, pool));
+			table[i0].Accumulate (constrainPlane);
+
+		} else {
+			int32_t i1 = ptr->m_twin->m_incidentVertex;
+			int32_t i2 = ptr->m_twin->m_prev->m_incidentVertex;
+			dgBigPlane constrainPlane (UnboundedLoopPlane (i0, i1, i2, pool));
+			table[i0].Accumulate (constrainPlane);
+
+			i1 = ptr->m_prev->m_incidentVertex;
+			i2 = ptr->m_prev->m_twin->m_prev->m_incidentVertex;
+			constrainPlane = UnboundedLoopPlane (i0, i1, i2, pool);
+			table[i0].Accumulate (constrainPlane);
+		}
+
+		ptr = ptr->m_twin->m_next;
+	} while (ptr != edge);
+}
+
+static void RemoveHalfEdge (dgPolyhedra* const polyhedra, dgEdge* const edge)
+{
+	dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *) IntToPointer (edge->m_userData);
+	if (handle) { 
+		handle->m_edge = NULL;
+	}
+
+	dgPolyhedra::dgTreeNode* const node = polyhedra->GetNodeFromInfo(*edge);
+	HACD_ASSERT (node);
+	polyhedra->Remove (node);
+}
+
+
+static dgEdge* CollapseEdge(dgPolyhedra* const polyhedra, dgEdge* const edge)
+{
+	int32_t v0 = edge->m_incidentVertex;
+	int32_t v1 = edge->m_twin->m_incidentVertex;
+
+#ifdef __ENABLE_SANITY_CHECK 
+	dgPolyhedra::dgPairKey TwinKey (v1, v0);
+	dgPolyhedra::dgTreeNode* const node = polyhedra->Find (TwinKey.GetVal());
+	dgEdge* const twin1 = node ? &node->GetInfo() : NULL;
+	HACD_ASSERT (twin1);
+	HACD_ASSERT (edge->m_twin == twin1);
+	HACD_ASSERT (twin1->m_twin == edge);
+	HACD_ASSERT (edge->m_incidentFace != 0);
+	HACD_ASSERT (twin1->m_incidentFace != 0);
+#endif
+
+
+	dgEdge* retEdge = edge->m_twin->m_prev->m_twin;
+	if (retEdge	== edge->m_twin->m_next) {
+		return NULL;
+	}
+	if (retEdge	== edge->m_twin) {
+		return NULL;
+	}
+	if (retEdge	== edge->m_next) {
+		retEdge = edge->m_prev->m_twin;
+		if (retEdge	== edge->m_twin->m_next) {
+			return NULL;
+		}
+		if (retEdge	== edge->m_twin) {
+			return NULL;
+		}
+	}
+
+	dgEdge* lastEdge = NULL;
+	dgEdge* firstEdge = NULL;
+	if ((edge->m_incidentFace >= 0)	&& (edge->m_twin->m_incidentFace >= 0)) {	
+		lastEdge = edge->m_prev->m_twin;
+		firstEdge = edge->m_twin->m_next->m_twin->m_next;
+	} else if (edge->m_twin->m_incidentFace >= 0) {
+		firstEdge = edge->m_twin->m_next->m_twin->m_next;
+		lastEdge = edge;
+	} else {
+		lastEdge = edge->m_prev->m_twin;
+		firstEdge = edge->m_twin->m_next;
+	}
+
+	for (dgEdge* ptr = firstEdge; ptr != lastEdge; ptr = ptr->m_twin->m_next) {
+		dgEdge* badEdge = polyhedra->FindEdge (edge->m_twin->m_incidentVertex, ptr->m_twin->m_incidentVertex);
+		if (badEdge) {
+			return NULL;
+		}
+	} 
+
+	dgEdge* const twin = edge->m_twin;
+	if (twin->m_next == twin->m_prev->m_prev) {
+		twin->m_prev->m_twin->m_twin = twin->m_next->m_twin;
+		twin->m_next->m_twin->m_twin = twin->m_prev->m_twin;
+
+		RemoveHalfEdge (polyhedra, twin->m_prev);
+		RemoveHalfEdge (polyhedra, twin->m_next);
+	} else {
+		twin->m_next->m_prev = twin->m_prev;
+		twin->m_prev->m_next = twin->m_next;
+	}
+
+	if (edge->m_next == edge->m_prev->m_prev) {
+		edge->m_next->m_twin->m_twin = edge->m_prev->m_twin;
+		edge->m_prev->m_twin->m_twin = edge->m_next->m_twin;
+		RemoveHalfEdge (polyhedra, edge->m_next);
+		RemoveHalfEdge (polyhedra, edge->m_prev);
+	} else {
+		edge->m_next->m_prev = edge->m_prev;
+		edge->m_prev->m_next = edge->m_next;
+	}
+
+	HACD_ASSERT (twin->m_twin->m_incidentVertex == v0);
+	HACD_ASSERT (edge->m_twin->m_incidentVertex == v1);
+	RemoveHalfEdge (polyhedra, twin);
+	RemoveHalfEdge (polyhedra, edge);
+
+	dgEdge* ptr = retEdge;
+	do {
+		dgPolyhedra::dgPairKey pairKey (v0, ptr->m_twin->m_incidentVertex);
+
+		dgPolyhedra::dgTreeNode* node = polyhedra->Find (pairKey.GetVal());
+		if (node) {
+			if (&node->GetInfo() == ptr) {
+				dgPolyhedra::dgPairKey key (v1, ptr->m_twin->m_incidentVertex);
+				ptr->m_incidentVertex = v1;
+				node = polyhedra->ReplaceKey (node, key.GetVal());
+				HACD_ASSERT (node);
+			} 
+		}
+
+		dgPolyhedra::dgPairKey TwinKey (ptr->m_twin->m_incidentVertex, v0);
+		node = polyhedra->Find (TwinKey.GetVal());
+		if (node) {
+			if (&node->GetInfo() == ptr->m_twin) {
+				dgPolyhedra::dgPairKey key (ptr->m_twin->m_incidentVertex, v1);
+				node = polyhedra->ReplaceKey (node, key.GetVal());
+				HACD_ASSERT (node);
+			}
+		}
+
+		ptr = ptr->m_twin->m_next;
+	} while (ptr != retEdge);
+
+	return retEdge;
+}
+
+
+
+void dgPolyhedra::Optimize (const double* const array, int32_t strideInBytes, double tol)
+{
+	dgList <dgEdgeCollapseEdgeHandle>::dgListNode *handleNodePtr;
+
+	int32_t stride = int32_t (strideInBytes / sizeof (double));
+
+#ifdef __ENABLE_SANITY_CHECK 
+	HACD_ASSERT (SanityCheck ());
+#endif
+
+	int32_t edgeCount = GetEdgeCount() * 4 + 1024 * 16;
+	int32_t maxVertexIndex = GetLastVertexIndex();
+
+	dgStack<dgBigVector> vertexPool (maxVertexIndex); 
+	dgStack<dgVertexCollapseVertexMetric> vertexMetrics (maxVertexIndex + 512); 
+
+	dgList <dgEdgeCollapseEdgeHandle> edgeHandleList;
+	dgStack<char> heapPool (2 * edgeCount * int32_t (sizeof (double) + sizeof (dgEdgeCollapseEdgeHandle*) + sizeof (int32_t))); 
+	dgDownHeap<dgList <dgEdgeCollapseEdgeHandle>::dgListNode* , double> bigHeapArray(&heapPool[0], heapPool.GetSizeInBytes());
+
+	NormalizeVertex (maxVertexIndex, &vertexPool[0], array, stride);
+	memset (&vertexMetrics[0], 0, maxVertexIndex * sizeof (dgVertexCollapseVertexMetric));
+	CalculateAllMetrics (this, &vertexMetrics[0], &vertexPool[0]);
+
+
+	double tol2 = tol * tol;
+	Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+
+		edge->m_userData = 0;
+		int32_t index0 = edge->m_incidentVertex;
+		int32_t index1 = edge->m_twin->m_incidentVertex;
+
+		dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+		dgVector p	(&vertexPool[index1].m_x);
+		double cost = metric.Evalue (p); 
+		if (cost < tol2) {
+			cost = EdgePenalty (&vertexPool[0], edge);
+
+			if (cost > double (0.0f)) {
+				dgEdgeCollapseEdgeHandle handle (edge);
+				handleNodePtr = edgeHandleList.Addtop (handle);
+				bigHeapArray.Push (handleNodePtr, cost);
+			}
+		}
+	}
+
+
+	while (bigHeapArray.GetCount()) {
+		handleNodePtr = bigHeapArray[0];
+
+		dgEdge* edge = handleNodePtr->GetInfo().m_edge;
+		bigHeapArray.Pop();
+		edgeHandleList.Remove (handleNodePtr);
+
+		if (edge) {
+			CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], edge);
+
+			int32_t index0 = edge->m_incidentVertex;
+			int32_t index1 = edge->m_twin->m_incidentVertex;
+			dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+			dgBigVector p (vertexPool[index1]);
+
+			if ((metric.Evalue (p) < tol2) && (EdgePenalty (&vertexPool[0], edge)  > double (0.0f))) {
+
+#ifdef __ENABLE_SANITY_CHECK 
+				HACD_ASSERT (SanityCheck ());
+#endif
+
+				edge = CollapseEdge(this, edge);
+
+#ifdef __ENABLE_SANITY_CHECK 
+				HACD_ASSERT (SanityCheck ());
+#endif
+				if (edge) {
+					// Update vertex metrics
+					CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], edge);
+
+					// Update metrics for all surrounding vertex
+					dgEdge* ptr = edge;
+					do {
+						CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], ptr->m_twin);
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge);
+
+					// calculate edge cost of all incident edges
+					int32_t mark = IncLRU();
+					ptr = edge;
+					do {
+						HACD_ASSERT (ptr->m_mark != mark);
+						ptr->m_mark = mark;
+
+						index0 = ptr->m_incidentVertex;
+						index1 = ptr->m_twin->m_incidentVertex;
+
+						dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+						dgBigVector p (vertexPool[index1]);
+
+						double cost = float (-1.0f);
+						if (metric.Evalue (p) < tol2) {
+							cost = EdgePenalty (&vertexPool[0], ptr);
+						}
+
+						if (cost  > double (0.0f)) {
+							dgEdgeCollapseEdgeHandle handle (ptr);
+							handleNodePtr = edgeHandleList.Addtop (handle);
+							bigHeapArray.Push (handleNodePtr, cost);
+						} else {
+							dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle*)IntToPointer (ptr->m_userData);
+							if (handle) {
+								handle->m_edge = NULL;
+							}
+							ptr->m_userData = uint32_t (NULL);
+
+						}
+
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge);
+
+
+					// calculate edge cost of all incident edges to a surrounding vertex
+					ptr = edge;
+					do {
+						dgEdge* const incidentEdge = ptr->m_twin;		
+
+						dgEdge* ptr1 = incidentEdge;
+						do {
+							index0 = ptr1->m_incidentVertex;
+							index1 = ptr1->m_twin->m_incidentVertex;
+
+							if (ptr1->m_mark != mark) {
+								ptr1->m_mark = mark;
+								dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
+								dgBigVector p (vertexPool[index1]);
+
+								double cost = float (-1.0f);
+								if (metric.Evalue (p) < tol2) {
+									cost = EdgePenalty (&vertexPool[0], ptr1);
+								}
+
+								if (cost  > double (0.0f)) {
+									HACD_ASSERT (cost > double(0.0f));
+									dgEdgeCollapseEdgeHandle handle (ptr1);
+									handleNodePtr = edgeHandleList.Addtop (handle);
+									bigHeapArray.Push (handleNodePtr, cost);
+								} else {
+									dgEdgeCollapseEdgeHandle *handle;
+									handle = (dgEdgeCollapseEdgeHandle*)IntToPointer (ptr1->m_userData);
+									if (handle) {
+										handle->m_edge = NULL;
+									}
+									ptr1->m_userData = uint32_t (NULL);
+
+								}
+							}
+
+							if (ptr1->m_twin->m_mark != mark) {
+								ptr1->m_twin->m_mark = mark;
+								dgVertexCollapseVertexMetric &metric = vertexMetrics[index1];
+								dgBigVector p (vertexPool[index0]);
+
+								double cost = float (-1.0f);
+								if (metric.Evalue (p) < tol2) {
+									cost = EdgePenalty (&vertexPool[0], ptr1->m_twin);
+								}
+
+								if (cost  > double (0.0f)) {
+									HACD_ASSERT (cost > double(0.0f));
+									dgEdgeCollapseEdgeHandle handle (ptr1->m_twin);
+									handleNodePtr = edgeHandleList.Addtop (handle);
+									bigHeapArray.Push (handleNodePtr, cost);
+								} else {
+									dgEdgeCollapseEdgeHandle *handle;
+									handle = (dgEdgeCollapseEdgeHandle*) IntToPointer (ptr1->m_twin->m_userData);
+									if (handle) {
+										handle->m_edge = NULL;
+									}
+									ptr1->m_twin->m_userData = uint32_t (NULL);
+
+								}
+							}
+
+							ptr1 = ptr1->m_twin->m_next;
+						} while (ptr1 != incidentEdge);
+
+						ptr = ptr->m_twin->m_next;
+					} while (ptr != edge);
+				}
+			}
+		}
+	}
+}
+
+
+dgEdge* dgPolyhedra::FindEarTip (dgEdge* const face, const double* const pool, int32_t stride, dgDownHeap<dgEdge*, double>& heap, const dgBigVector &normal) const
+{
+	dgEdge* ptr = face;
+	dgBigVector p0 (&pool[ptr->m_prev->m_incidentVertex * stride]);
+	dgBigVector p1 (&pool[ptr->m_incidentVertex * stride]);
+	dgBigVector d0 (p1 - p0);
+	double f = sqrt (d0 % d0);
+	if (f < double (1.0e-10f)) {
+		f = double (1.0e-10f);
+	}
+	d0 = d0.Scale (double (1.0f) / f);
+
+	double minAngle = float (10.0f);
+	do {
+		dgBigVector p2 (&pool [ptr->m_next->m_incidentVertex * stride]);
+		dgBigVector d1 (p2 - p1);
+		float f = dgSqrt (d1 % d1);
+		if (f < float (1.0e-10f)) {
+			f = float (1.0e-10f);
+		}
+		d1 = d1.Scale (float (1.0f) / f);
+		dgBigVector n (d0 * d1);
+
+		double angle = normal %  n;
+		if (angle >= double (0.0f)) {
+			heap.Push (ptr, angle);
+		}
+
+		if (angle < minAngle) {
+			minAngle = angle;
+		}
+
+		d0 = d1;
+		p1 = p2;
+		ptr = ptr->m_next;
+	} while (ptr != face);
+
+	if (minAngle > float (0.1f)) {
+		return heap[0];
+	}
+
+	dgEdge* ear = NULL;
+	while (heap.GetCount()) {
+		ear = heap[0];
+		heap.Pop();
+
+		if (FindEdge (ear->m_prev->m_incidentVertex, ear->m_next->m_incidentVertex)) {
+			continue;
+		}
+
+		dgBigVector p0 (&pool [ear->m_prev->m_incidentVertex * stride]);
+		dgBigVector p1 (&pool [ear->m_incidentVertex * stride]);
+		dgBigVector p2 (&pool [ear->m_next->m_incidentVertex * stride]);
+
+		dgBigVector p10 (p1 - p0);
+		dgBigVector p21 (p2 - p1);
+		dgBigVector p02 (p0 - p2);
+
+		for (ptr = ear->m_next->m_next; ptr != ear->m_prev; ptr = ptr->m_next) {
+			dgBigVector p (&pool [ptr->m_incidentVertex * stride]);
+
+			double side = ((p - p0) * p10) % normal;
+			if (side < double (0.05f)) {
+				side = ((p - p1) * p21) % normal;
+				if (side < double (0.05f)) {
+					side = ((p - p2) * p02) % normal;
+					if (side < float (0.05f)) {
+						break;
+					}
+				}
+			}
+		}
+
+		if (ptr == ear->m_prev) {
+			break;
+		}
+	}
+
+	return ear;
+}
+
+
+
+
+
+//dgEdge* TriangulateFace (dgPolyhedra& polyhedra, dgEdge* face, const float* const pool, int32_t stride, dgDownHeap<dgEdge*, float>& heap, dgVector* const faceNormalOut)
+dgEdge* dgPolyhedra::TriangulateFace (dgEdge* face, const double* const pool, int32_t stride, dgDownHeap<dgEdge*, double>& heap, dgBigVector* const faceNormalOut)
+{
+#ifdef _DEBUG
+	dgEdge* perimeter [1024 * 16]; 
+	dgEdge* ptr = face;
+	int32_t perimeterCount = 0;
+	do {
+		perimeter[perimeterCount] = ptr;
+		perimeterCount ++;
+		HACD_ASSERT (perimeterCount < int32_t (sizeof (perimeter) / sizeof (perimeter[0])));
+		ptr = ptr->m_next;
+	} while (ptr != face);
+	perimeter[perimeterCount] = face;
+	HACD_ASSERT ((perimeterCount + 1) < int32_t (sizeof (perimeter) / sizeof (perimeter[0])));
+#endif
+	dgBigVector normal (FaceNormal (face, pool, int32_t (stride * sizeof (double))));
+
+	double dot = normal % normal;
+	if (dot < double (1.0e-12f)) {
+		if (faceNormalOut) {
+			*faceNormalOut = dgBigVector (float (0.0f), float (0.0f), float (0.0f), float (0.0f)); 
+		}
+		return face;
+	}
+	normal = normal.Scale (double (1.0f) / sqrt (dot));
+	if (faceNormalOut) {
+		*faceNormalOut = normal;
+	}
+
+
+	while (face->m_next->m_next->m_next != face) {
+		dgEdge* const ear = FindEarTip (face, pool, stride, heap, normal); 
+		if (!ear) {
+			return face;
+		}
+		if ((face == ear)	|| (face == ear->m_prev)) {
+			face = ear->m_prev->m_prev;
+		}
+		dgEdge* const edge = AddHalfEdge (ear->m_next->m_incidentVertex, ear->m_prev->m_incidentVertex);
+		if (!edge) {
+			return face;
+		}
+		dgEdge* const twin = AddHalfEdge (ear->m_prev->m_incidentVertex, ear->m_next->m_incidentVertex);
+		if (!twin) {
+			return face;
+		}
+		HACD_ASSERT (twin);
+
+
+		edge->m_mark = ear->m_mark;
+		edge->m_userData = ear->m_next->m_userData;
+		edge->m_incidentFace = ear->m_incidentFace;
+
+		twin->m_mark = ear->m_mark;
+		twin->m_userData = ear->m_prev->m_userData;
+		twin->m_incidentFace = ear->m_incidentFace;
+
+		edge->m_twin = twin;
+		twin->m_twin = edge;
+
+		twin->m_prev = ear->m_prev->m_prev;
+		twin->m_next = ear->m_next;
+		ear->m_prev->m_prev->m_next = twin;
+		ear->m_next->m_prev = twin;
+
+		edge->m_next = ear->m_prev;
+		edge->m_prev = ear;
+		ear->m_prev->m_prev = edge;
+		ear->m_next = edge;
+
+		heap.Flush ();
+	}
+	return NULL;
+}
+
+
+void dgPolyhedra::MarkAdjacentCoplanarFaces (dgPolyhedra& polyhedraOut, dgEdge* const face, const double* const pool, int32_t strideInBytes)
+{
+	const double normalDeviation = double (0.9999f);
+	const double distanceFromPlane = double (1.0f / 128.0f);
+
+	int32_t faceIndex[1024 * 4];
+	dgEdge* stack[1024 * 4];
+	dgEdge* deleteEdge[1024 * 4];
+
+	int32_t deleteCount = 1;
+	deleteEdge[0] = face;
+	int32_t stride = int32_t (strideInBytes / sizeof (double));
+
+	HACD_ASSERT (face->m_incidentFace > 0);
+
+	dgBigVector normalAverage (FaceNormal (face, pool, strideInBytes));
+	double dot = normalAverage % normalAverage;
+	if (dot > double (1.0e-12f)) {
+		int32_t testPointsCount = 1;
+		dot = double (1.0f) / sqrt (dot);
+		dgBigVector normal (normalAverage.Scale (dot));
+
+		dgBigVector averageTestPoint (&pool[face->m_incidentVertex * stride]);
+		dgBigPlane testPlane(normal, - (averageTestPoint % normal));
+
+		polyhedraOut.BeginFace();
+
+		IncLRU();
+		int32_t faceMark = IncLRU();
+
+		int32_t faceIndexCount = 0;
+		dgEdge* ptr = face;
+		do {
+			ptr->m_mark = faceMark;
+			faceIndex[faceIndexCount] = ptr->m_incidentVertex;
+			faceIndexCount ++;
+			HACD_ASSERT (faceIndexCount < int32_t (sizeof (faceIndex) / sizeof (faceIndex[0])));
+			ptr = ptr ->m_next;
+		} while (ptr != face);
+		polyhedraOut.AddFace(faceIndexCount, faceIndex);
+
+		int32_t index = 1;
+		deleteCount = 0;
+		stack[0] = face;
+		while (index) 
+		{
+			index --;
+			dgEdge* const face = stack[index];
+			deleteEdge[deleteCount] = face;
+			deleteCount ++;
+			HACD_ASSERT (deleteCount < int32_t (sizeof (deleteEdge) / sizeof (deleteEdge[0])));
+// TODO:JWR Temporarily commented out...			HACD_ASSERT (face->m_next->m_next->m_next == face);
+
+			dgEdge* edge = face;
+			do 
+			{
+				dgEdge* const ptr = edge->m_twin;
+				if (ptr->m_incidentFace > 0) 
+				{
+					if (ptr->m_mark != faceMark) 
+					{
+						dgEdge* ptr1 = ptr;
+						faceIndexCount = 0;
+						do 
+						{
+							ptr1->m_mark = faceMark;
+							faceIndex[faceIndexCount] = ptr1->m_incidentVertex;
+							HACD_ASSERT (faceIndexCount < int32_t (sizeof (faceIndex) / sizeof (faceIndex[0])));
+							faceIndexCount ++;
+							ptr1 = ptr1 ->m_next;
+						} while (ptr1 != ptr);
+
+						dgBigVector normal1 (FaceNormal (ptr, pool, strideInBytes));
+						dot = normal1 % normal1;
+						if (dot < double (1.0e-12f)) {
+							deleteEdge[deleteCount] = ptr;
+							deleteCount ++;
+							HACD_ASSERT (deleteCount < int32_t (sizeof (deleteEdge) / sizeof (deleteEdge[0])));
+						} else {
+							//normal1 = normal1.Scale (double (1.0f) / sqrt (dot));
+							dgBigVector testNormal (normal1.Scale (double (1.0f) / sqrt (dot)));
+							dot = normal % testNormal;
+							if (dot >= normalDeviation) {
+								dgBigVector testPoint (&pool[ptr->m_prev->m_incidentVertex * stride]);
+								double dist = fabs (testPlane.Evalue (testPoint));
+								if (dist < distanceFromPlane) {
+									testPointsCount ++;
+
+									averageTestPoint += testPoint;
+									testPoint = averageTestPoint.Scale (double (1.0f) / double(testPointsCount));
+
+									normalAverage += normal1;
+									testNormal = normalAverage.Scale (double (1.0f) / sqrt (normalAverage % normalAverage));
+									testPlane = dgBigPlane (testNormal, - (testPoint % testNormal));
+
+									polyhedraOut.AddFace(faceIndexCount, faceIndex);;
+									stack[index] = ptr;
+									index ++;
+									HACD_ASSERT (index < int32_t (sizeof (stack) / sizeof (stack[0])));
+								}
+							}
+						}
+					}
+				}
+
+				edge = edge->m_next;
+			} while (edge != face);
+		}
+		polyhedraOut.EndFace();
+	}
+
+	for (int32_t index = 0; index < deleteCount; index ++) {
+		DeleteFace (deleteEdge[index]);
+	}
+}
+
+
+void dgPolyhedra::RefineTriangulation (const double* const vertex, int32_t stride, dgBigVector* const normal, int32_t perimeterCount, dgEdge** const perimeter)
+{
+	dgList<dgDiagonalEdge> dignonals;
+
+	for (int32_t i = 1; i <= perimeterCount; i ++) {
+		dgEdge* const last = perimeter[i - 1];
+		for (dgEdge* ptr = perimeter[i]->m_prev; ptr != last; ptr = ptr->m_twin->m_prev) {
+			dgList<dgDiagonalEdge>::dgListNode* node = dignonals.GetFirst();
+			for (; node; node = node->GetNext()) {
+				const dgDiagonalEdge& key = node->GetInfo();
+				if (((key.m_i0 == ptr->m_incidentVertex) && (key.m_i1 == ptr->m_twin->m_incidentVertex)) ||
+					((key.m_i1 == ptr->m_incidentVertex) && (key.m_i0 == ptr->m_twin->m_incidentVertex))) {
+						break;
+				}
+			}
+			if (!node) {
+				dgDiagonalEdge key (ptr);
+				dignonals.Append(key);
+			}
+		}
+	}
+
+	dgEdge* const face = perimeter[0];
+	int32_t i0 = face->m_incidentVertex * stride;
+	int32_t i1 = face->m_next->m_incidentVertex * stride;
+	dgBigVector p0 (vertex[i0], vertex[i0 + 1], vertex[i0 + 2], float (0.0f));
+	dgBigVector p1 (vertex[i1], vertex[i1 + 1], vertex[i1 + 2], float (0.0f));
+
+	dgBigVector p1p0 (p1 - p0);
+	double mag2 = p1p0 % p1p0;
+	for (dgEdge* ptr = face->m_next->m_next; mag2 < float (1.0e-12f); ptr = ptr->m_next) {
+		int32_t i1 = ptr->m_incidentVertex * stride;
+		dgBigVector p1 (vertex[i1], vertex[i1 + 1], vertex[i1 + 2], float (0.0f));
+		p1p0 = p1 - p0;
+		mag2 = p1p0 % p1p0;
+	}
+
+	dgMatrix matrix (dgGetIdentityMatrix());
+	matrix.m_posit = p0;
+	matrix.m_front = dgVector (p1p0.Scale (double (1.0f) / sqrt (mag2)));
+	matrix.m_right = dgVector (normal->Scale (double (1.0f) / sqrt (*normal % *normal)));
+	matrix.m_up = matrix.m_right * matrix.m_front;
+	matrix = matrix.Inverse();
+	matrix.m_posit.m_w = float (1.0f);
+
+	int32_t maxCount = dignonals.GetCount() * dignonals.GetCount();
+	while (dignonals.GetCount() && maxCount) {
+		maxCount --;
+		dgList<dgDiagonalEdge>::dgListNode* const node = dignonals.GetFirst();
+		dgDiagonalEdge key (node->GetInfo());
+		dignonals.Remove(node);
+		dgEdge* const edge = FindEdge(key.m_i0, key.m_i1);
+		if (edge) {
+			int32_t i0 = edge->m_incidentVertex * stride;
+			int32_t i1 = edge->m_next->m_incidentVertex * stride;
+			int32_t i2 = edge->m_next->m_next->m_incidentVertex * stride;
+			int32_t i3 = edge->m_twin->m_prev->m_incidentVertex * stride;
+
+			dgBigVector p0 (vertex[i0], vertex[i0 + 1], vertex[i0 + 2], double (0.0f));
+			dgBigVector p1 (vertex[i1], vertex[i1 + 1], vertex[i1 + 2], double (0.0f));
+			dgBigVector p2 (vertex[i2], vertex[i2 + 1], vertex[i2 + 2], double (0.0f));
+			dgBigVector p3 (vertex[i3], vertex[i3 + 1], vertex[i3 + 2], double (0.0f));
+
+			p0 = matrix.TransformVector(p0);
+			p1 = matrix.TransformVector(p1);
+			p2 = matrix.TransformVector(p2);
+			p3 = matrix.TransformVector(p3);
+
+			double circleTest[3][3];
+			circleTest[0][0] = p0[0] - p3[0];
+			circleTest[0][1] = p0[1] - p3[1];
+			circleTest[0][2] = circleTest[0][0] * circleTest[0][0] + circleTest[0][1] * circleTest[0][1];
+
+			circleTest[1][0] = p1[0] - p3[0];
+			circleTest[1][1] = p1[1] - p3[1];
+			circleTest[1][2] = circleTest[1][0] * circleTest[1][0] + circleTest[1][1] * circleTest[1][1];
+
+			circleTest[2][0] = p2[0] - p3[0];
+			circleTest[2][1] = p2[1] - p3[1];
+			circleTest[2][2] = circleTest[2][0] * circleTest[2][0] + circleTest[2][1] * circleTest[2][1];
+
+			double error;
+			double det = Determinant3x3 (circleTest, &error);
+			if (det < float (0.0f)) {
+				dgEdge* frontFace0 = edge->m_prev;
+				dgEdge* backFace0 = edge->m_twin->m_prev;
+
+				FlipEdge(edge);
+
+				if (perimeterCount > 4) {
+					dgEdge* backFace1 = backFace0->m_next;
+					dgEdge* frontFace1 = frontFace0->m_next;
+					for (int32_t i = 0; i < perimeterCount; i ++) {
+						if (frontFace0 == perimeter[i]) {
+							frontFace0 = NULL;
+						}
+						if (frontFace1 == perimeter[i]) {
+							frontFace1 = NULL;
+						}
+
+						if (backFace0 == perimeter[i]) {
+							backFace0 = NULL;
+						}
+						if (backFace1 == perimeter[i]) {
+							backFace1 = NULL;
+						}
+					}
+
+					if (backFace0 && (backFace0->m_incidentFace > 0) && (backFace0->m_twin->m_incidentFace > 0)) {
+						dgDiagonalEdge key (backFace0);
+						dignonals.Append(key);
+					}
+					if (backFace1 && (backFace1->m_incidentFace > 0) && (backFace1->m_twin->m_incidentFace > 0)) {
+						dgDiagonalEdge key (backFace1);
+						dignonals.Append(key);
+					}
+
+					if (frontFace0 && (frontFace0->m_incidentFace > 0) && (frontFace0->m_twin->m_incidentFace > 0)) {
+						dgDiagonalEdge key (frontFace0);
+						dignonals.Append(key);
+					}
+
+					if (frontFace1 && (frontFace1->m_incidentFace > 0) && (frontFace1->m_twin->m_incidentFace > 0)) {
+						dgDiagonalEdge key (frontFace1);
+						dignonals.Append(key);
+					}
+				}
+			}
+		}
+	}
+}
+
+
+void dgPolyhedra::RefineTriangulation (const double* const vertex, int32_t stride)
+{
+	dgEdge* edgePerimeters[1024 * 16];
+	int32_t perimeterCount = 0;
+
+	dgPolyhedra::Iterator iter (*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_incidentFace < 0) {
+			dgEdge* ptr = edge;
+			do {
+				edgePerimeters[perimeterCount] = ptr->m_twin;
+				perimeterCount ++;
+				HACD_ASSERT (perimeterCount < int32_t (sizeof (edgePerimeters) / sizeof (edgePerimeters[0])));
+				ptr = ptr->m_prev;
+			} while (ptr != edge);
+			break;
+		}
+	}
+	HACD_ASSERT (perimeterCount);
+	HACD_ASSERT (perimeterCount < int32_t (sizeof (edgePerimeters) / sizeof (edgePerimeters[0])));
+	edgePerimeters[perimeterCount] = edgePerimeters[0];
+
+	dgBigVector normal (FaceNormal(edgePerimeters[0], vertex, int32_t (stride * sizeof (double))));
+	if ((normal % normal) > float (1.0e-12f)) {
+		RefineTriangulation (vertex, stride, &normal, perimeterCount, edgePerimeters);
+	}
+}
+
+
+void dgPolyhedra::OptimizeTriangulation (const double* const vertex, int32_t strideInBytes)
+{
+	int32_t polygon[1024 * 8];
+	int32_t stride = int32_t (strideInBytes / sizeof (double));
+
+	dgPolyhedra leftOver;
+	dgPolyhedra buildConvex;
+
+	buildConvex.BeginFace();
+	dgPolyhedra::Iterator iter (*this);
+
+	for (iter.Begin(); iter; ) {
+		dgEdge* const edge = &(*iter);
+		iter++;
+
+		if (edge->m_incidentFace > 0) {
+			dgPolyhedra flatFace;
+			MarkAdjacentCoplanarFaces (flatFace, edge, vertex, strideInBytes);
+			//HACD_ASSERT (flatFace.GetCount());
+
+			if (flatFace.GetCount()) {
+				//flatFace.Triangulate (vertex, strideInBytes, &leftOver);
+				//HACD_ASSERT (!leftOver.GetCount());
+				flatFace.RefineTriangulation (vertex, stride);
+
+				int32_t mark = flatFace.IncLRU();
+				dgPolyhedra::Iterator iter (flatFace);
+				for (iter.Begin(); iter; iter ++) {
+					dgEdge* const edge = &(*iter);
+					if (edge->m_mark != mark) {
+						if (edge->m_incidentFace > 0) {
+							dgEdge* ptr = edge;
+							int32_t vertexCount = 0;
+							do {
+								polygon[vertexCount] = ptr->m_incidentVertex;				
+								vertexCount ++;
+								HACD_ASSERT (vertexCount < int32_t (sizeof (polygon) / sizeof (polygon[0])));
+								ptr->m_mark = mark;
+								ptr = ptr->m_next;
+							} while (ptr != edge);
+							if (vertexCount >= 3) {
+								buildConvex.AddFace (vertexCount, polygon);
+							}
+						}
+					}
+				}
+			}
+			iter.Begin();
+		}
+	}
+	buildConvex.EndFace();
+	HACD_ASSERT (GetCount() == 0);
+	SwapInfo(buildConvex);
+}
+
+
+void dgPolyhedra::Triangulate (const double* const /*vertex*/, int32_t /*strideInBytes*/, dgPolyhedra* const /*leftOver*/)
+{
+	int32_t count = GetCount() / 2;
+	dgStack<char> memPool (int32_t ((count + 512) * (2 * sizeof (double)))); 
+	dgDownHeap<dgEdge*, double> heap(&memPool[0], memPool.GetSizeInBytes());
+#if 0
+	int32_t stride = int32_t (strideInBytes / sizeof (double));
+	int32_t mark = IncLRU();
+	Iterator iter (*this);
+	for (iter.Begin(); iter; ) 
+	{
+		dgEdge* const thisEdge = &(*iter);
+		iter ++;
+
+		if (thisEdge->m_mark == mark) 
+		{
+			continue;
+		}
+		if (thisEdge->m_incidentFace < 0) 
+		{
+			continue;
+		}
+
+		count = 0;
+		dgEdge* ptr = thisEdge;
+		do 
+		{
+			count ++;
+			ptr->m_mark = mark;
+			ptr = ptr->m_next;
+		} while (ptr != thisEdge);
+
+		if (count > 3) 
+		{
+			dgEdge* const edge = TriangulateFace (thisEdge, vertex, stride, heap, NULL);
+			heap.Flush ();
+
+			if (edge) 
+			{
+				HACD_ASSERT (edge->m_incidentFace > 0);
+
+				if (leftOver) 
+				{
+					int32_t* const index = (int32_t *) &heap[0];
+					int64_t* const data = (int64_t *)&index[count];
+					int32_t i = 0;
+					dgEdge* ptr = edge;
+					do {
+						index[i] = ptr->m_incidentVertex;
+						data[i] = int64_t (ptr->m_userData);
+						i ++;
+						ptr = ptr->m_next;
+					} while (ptr != edge);
+					leftOver->AddFace(i, index, data);
+				} 
+				DeleteFace (edge);
+				iter.Begin();
+			}
+		}
+	}
+	OptimizeTriangulation (vertex, strideInBytes);
+	mark = IncLRU();
+	m_faceSecuence = 1;
+	for (iter.Begin(); iter; iter ++) 
+	{
+		dgEdge* edge = &(*iter);
+		if (edge->m_mark == mark) 
+		{
+			continue;
+		}
+		if (edge->m_incidentFace < 0) 
+		{
+			continue;
+		}
+		HACD_ASSERT (edge == edge->m_next->m_next->m_next);
+
+		for (int32_t i = 0; i < 3; i ++) 
+		{
+			edge->m_incidentFace = m_faceSecuence; 
+			edge->m_mark = mark;
+			edge = edge->m_next;
+		}
+		m_faceSecuence ++;
+	}
+#endif
+}
+
+
+static void RemoveColinearVertices (dgPolyhedra& flatFace, const double* const vertex, int32_t stride)
+{
+	dgEdge* edgePerimeters[1024];
+
+	int32_t perimeterCount = 0;
+	int32_t mark = flatFace.IncLRU();
+	dgPolyhedra::Iterator iter (flatFace);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if ((edge->m_incidentFace < 0) && (edge->m_mark != mark)) {
+			dgEdge* ptr = edge;
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_next;
+			} while (ptr != edge);
+			edgePerimeters[perimeterCount] = edge;
+			perimeterCount ++;
+			HACD_ASSERT (perimeterCount < int32_t (sizeof (edgePerimeters) / sizeof (edgePerimeters[0])));
+		}
+	}
+
+	for (int32_t i = 0; i < perimeterCount; i ++) {
+		dgEdge* edge = edgePerimeters[i];
+		dgEdge* ptr = edge;
+		dgVector p0 (&vertex[ptr->m_incidentVertex * stride]);
+		dgVector p1 (&vertex[ptr->m_next->m_incidentVertex * stride]);
+		dgVector e0 (p1 - p0) ;
+		e0 = e0.Scale (float (1.0f) / (dgSqrt (e0 % e0) + float (1.0e-12f)));
+		int32_t ignoreTest = 1;
+		do {
+			ignoreTest = 0;
+			dgVector p2 (&vertex[ptr->m_next->m_next->m_incidentVertex * stride]);
+			dgVector e1 (p2 - p1);
+			e1 = e1.Scale (float (1.0f) / (dgSqrt (e1 % e1) + float (1.0e-12f)));
+			float dot = e1 % e0;
+			if (dot > float (float (0.9999f))) {
+
+				for (dgEdge* interiorEdge = ptr->m_next->m_twin->m_next; interiorEdge != ptr->m_twin; interiorEdge = ptr->m_next->m_twin->m_next) {
+					flatFace.DeleteEdge (interiorEdge);
+				} 
+
+				if (ptr->m_twin->m_next->m_next->m_next == ptr->m_twin) {
+					HACD_ASSERT (ptr->m_twin->m_next->m_incidentFace > 0);
+					flatFace.DeleteEdge (ptr->m_twin->m_next);
+				}
+
+				HACD_ASSERT (ptr->m_next->m_twin->m_next->m_twin == ptr);
+				edge = ptr->m_next;
+
+				if (!flatFace.FindEdge (ptr->m_incidentVertex, edge->m_twin->m_incidentVertex) && 
+					!flatFace.FindEdge (edge->m_twin->m_incidentVertex, ptr->m_incidentVertex)) {
+						ptr->m_twin->m_prev = edge->m_twin->m_prev;
+						edge->m_twin->m_prev->m_next = ptr->m_twin;
+
+						edge->m_next->m_prev = ptr;
+						ptr->m_next = edge->m_next;
+
+						edge->m_next = edge->m_twin;
+						edge->m_prev = edge->m_twin;
+						edge->m_twin->m_next = edge;
+						edge->m_twin->m_prev = edge;
+						flatFace.DeleteEdge (edge);								
+						flatFace.ChangeEdgeIncidentVertex (ptr->m_twin, ptr->m_next->m_incidentVertex);
+
+						e1 = e0;
+						p1 = p2;
+						edge = ptr;
+						ignoreTest = 1;
+						continue;
+				}
+			}
+
+			e0 = e1;
+			p1 = p2;
+			ptr = ptr->m_next;
+		} while ((ptr != edge) || ignoreTest);
+	}
+}
+
+
+static int32_t GetInteriorDiagonals (dgPolyhedra& polyhedra, dgEdge** const diagonals, int32_t maxCount)
+{
+	int32_t count = 0;
+	int32_t mark = polyhedra.IncLRU();
+	dgPolyhedra::Iterator iter (polyhedra);
+	for (iter.Begin(); iter; iter++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_mark != mark) { 
+			if (edge->m_incidentFace > 0) {
+				if (edge->m_twin->m_incidentFace > 0) {
+					edge->m_twin->m_mark = mark;
+					if (count < maxCount){
+						diagonals[count] = edge;
+						count ++;
+					}
+					HACD_ASSERT (count <= maxCount);
+				}
+			}
+		}
+		edge->m_mark = mark;
+	}
+
+	return count;
+}
+
+static bool IsEssensialPointDiagonal (dgEdge* const diagonal, const dgBigVector& normal, const double* const pool, int32_t stride)
+{
+	double dot;
+	dgBigVector p0 (&pool[diagonal->m_incidentVertex * stride]);
+	dgBigVector p1 (&pool[diagonal->m_twin->m_next->m_twin->m_incidentVertex * stride]);
+	dgBigVector p2 (&pool[diagonal->m_prev->m_incidentVertex * stride]);
+
+	dgBigVector e1 (p1 - p0);
+	dot = e1 % e1;
+	if (dot < double (1.0e-12f)) {
+		return false;
+	}
+	e1 = e1.Scale (double (1.0f) / sqrt(dot));
+
+	dgBigVector e2 (p2 - p0);
+	dot = e2 % e2;
+	if (dot < double (1.0e-12f)) {
+		return false;
+	}
+	e2 = e2.Scale (double (1.0f) / sqrt(dot));
+
+	dgBigVector n1 (e1 * e2); 
+
+	dot = normal % n1;
+	//if (dot > double (float (0.1f)f)) {
+	//if (dot >= double (-1.0e-6f)) {
+	if (dot >= double (0.0f)) {
+		return false;
+	}
+	return true;
+}
+
+
+static bool IsEssensialDiagonal (dgEdge* const diagonal, const dgBigVector& normal, const double* const pool,  int32_t stride)
+{
+	return IsEssensialPointDiagonal (diagonal, normal, pool, stride) || IsEssensialPointDiagonal (diagonal->m_twin, normal, pool, stride); 
+}
+
+
+void dgPolyhedra::ConvexPartition (const double* const vertex, int32_t strideInBytes, dgPolyhedra* const leftOversOut)
+{
+	if (GetCount()) {
+		Triangulate (vertex, strideInBytes, leftOversOut);
+		DeleteDegenerateFaces (vertex, strideInBytes, float (1.0e-5f));
+		Optimize (vertex, strideInBytes, float (1.0e-4f));
+		DeleteDegenerateFaces (vertex, strideInBytes, float (1.0e-5f));
+
+		if (GetCount()) {
+			int32_t removeCount = 0;
+			int32_t stride = int32_t (strideInBytes / sizeof (double));
+
+			int32_t polygon[1024 * 8];
+			dgEdge* diagonalsPool[1024 * 8];
+			dgPolyhedra buildConvex;
+
+			buildConvex.BeginFace();
+			dgPolyhedra::Iterator iter (*this);
+			for (iter.Begin(); iter; ) {
+				dgEdge* edge = &(*iter);
+				iter++;
+				if (edge->m_incidentFace > 0) {
+
+					dgPolyhedra flatFace;
+					MarkAdjacentCoplanarFaces (flatFace, edge, vertex, strideInBytes);
+
+					if (flatFace.GetCount()) {
+						flatFace.RefineTriangulation (vertex, stride);
+						RemoveColinearVertices (flatFace, vertex, stride);
+
+						int32_t diagonalCount = GetInteriorDiagonals (flatFace, diagonalsPool, sizeof (diagonalsPool) / sizeof (diagonalsPool[0]));
+						if (diagonalCount) {
+							edge = &flatFace.GetRoot()->GetInfo();
+							if (edge->m_incidentFace < 0) {
+								edge = edge->m_twin;
+							}
+							HACD_ASSERT (edge->m_incidentFace > 0);
+
+							dgBigVector normal (FaceNormal (edge, vertex, strideInBytes));
+							normal = normal.Scale (double (1.0f) / sqrt (normal % normal));
+
+							edge = NULL;
+							dgPolyhedra::Iterator iter (flatFace);
+							for (iter.Begin(); iter; iter ++) {
+								edge = &(*iter);
+								if (edge->m_incidentFace < 0) {
+									break;
+								}
+							}
+							HACD_ASSERT (edge);
+
+							int32_t isConvex = 1;
+							dgEdge* ptr = edge;
+							int32_t mark = flatFace.IncLRU();
+
+							dgBigVector normal2 (normal);
+							dgBigVector p0 (&vertex[ptr->m_prev->m_incidentVertex * stride]);
+							dgBigVector p1 (&vertex[ptr->m_incidentVertex * stride]);
+							dgBigVector e0 (p1 - p0);
+							e0 = e0.Scale (float (1.0f) / (dgSqrt (e0 % e0) + float (1.0e-14f)));
+							do {
+								dgBigVector p2 (&vertex[ptr->m_next->m_incidentVertex * stride]);
+								dgBigVector e1 (p2 - p1);
+								e1 = e1.Scale (float (1.0f) / (sqrt (e1 % e1) + float (1.0e-14f)));
+								double dot = (e0 * e1) % normal2;
+								//if (dot > float (0.0f)) {
+								if (dot > float (5.0e-3f)) {
+									isConvex = 0;
+									break;
+								}
+								ptr->m_mark = mark;
+								e0 = e1;
+								p1 = p2;
+								ptr = ptr->m_next;
+							} while (ptr != edge);
+
+							if (isConvex) {
+								dgPolyhedra::Iterator iter (flatFace);
+								for (iter.Begin(); iter; iter ++) {
+									ptr = &(*iter);
+									if (ptr->m_incidentFace < 0) {
+										if (ptr->m_mark < mark) {
+											isConvex = 0;
+											break;
+										}
+									}
+								}
+							}
+
+							if (isConvex) {
+								if (diagonalCount > 2) {
+									int32_t count = 0;
+									ptr = edge;
+									do {
+										polygon[count] = ptr->m_incidentVertex;				
+										count ++;
+										HACD_ASSERT (count < int32_t (sizeof (polygon) / sizeof (polygon[0])));
+										ptr = ptr->m_next;
+									} while (ptr != edge);
+
+									for (int32_t i = 0; i < count - 1; i ++) {
+										for (int32_t j = i + 1; j < count; j ++) {
+											if (polygon[i] == polygon[j]) {
+												i = count;
+												isConvex = 0;
+												break ;
+											}
+										}
+									}
+								}
+							}
+
+							if (isConvex) {
+								for (int32_t j = 0; j < diagonalCount; j ++) {
+									dgEdge* const diagonal = diagonalsPool[j];
+									removeCount ++;
+									flatFace.DeleteEdge (diagonal);
+								}
+							} else {
+								for (int32_t j = 0; j < diagonalCount; j ++) {
+									dgEdge* const diagonal = diagonalsPool[j];
+									if (!IsEssensialDiagonal(diagonal, normal, vertex, stride)) {
+										removeCount ++;
+										flatFace.DeleteEdge (diagonal);
+									}
+								}
+							}
+						}
+
+						int32_t mark = flatFace.IncLRU();
+						dgPolyhedra::Iterator iter (flatFace);
+						for (iter.Begin(); iter; iter ++) {
+							dgEdge* const edge = &(*iter);
+							if (edge->m_mark != mark) {
+								if (edge->m_incidentFace > 0) {
+									dgEdge* ptr = edge;
+									int32_t diagonalCount = 0;
+									do {
+										polygon[diagonalCount] = ptr->m_incidentVertex;				
+										diagonalCount ++;
+										HACD_ASSERT (diagonalCount < int32_t (sizeof (polygon) / sizeof (polygon[0])));
+										ptr->m_mark = mark;
+										ptr = ptr->m_next;
+									} while (ptr != edge);
+									if (diagonalCount >= 3) {
+										buildConvex.AddFace (diagonalCount, polygon);
+									}
+								}
+							}
+						}
+					}
+					iter.Begin();
+				}
+			}
+
+			buildConvex.EndFace();
+			HACD_ASSERT (GetCount() == 0);
+			SwapInfo(buildConvex);
+		}
+	}
+}
+
+
+dgSphere dgPolyhedra::CalculateSphere (const double* const vertex, int32_t strideInBytes, const dgMatrix* const /*basis*/) const
+{
+/*
+		// this si a degenerate mesh of a flat plane calculate OOBB by discrete rotations
+		dgStack<int32_t> pool (GetCount() * 3 + 6); 
+		int32_t* const indexList = &pool[0]; 
+
+		dgMatrix axis (dgGetIdentityMatrix());
+		dgBigVector p0 (float ( 1.0e10f), float ( 1.0e10f), float ( 1.0e10f), float (0.0f));
+		dgBigVector p1 (float (-1.0e10f), float (-1.0e10f), float (-1.0e10f), float (0.0f));
+
+		int32_t stride = int32_t (strideInBytes / sizeof (double));
+		int32_t indexCount = 0;
+		int32_t mark = IncLRU();
+		dgPolyhedra::Iterator iter(*this);
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &(*iter);
+			if (edge->m_mark != mark) {
+				dgEdge *ptr = edge;
+				do {
+					ptr->m_mark = mark;
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != edge);
+				int32_t index = edge->m_incidentVertex;
+				indexList[indexCount + 6] = edge->m_incidentVertex;
+				dgBigVector point (vertex[index * stride + 0], vertex[index * stride + 1], vertex[index * stride + 2], float (0.0f));
+				for (int32_t i = 0; i < 3; i ++) {
+					if (point[i] < p0[i]) {
+						p0[i] = point[i];
+						indexList[i * 2 + 0] = index;
+					}
+					if (point[i] > p1[i]) {
+						p1[i] = point[i];
+						indexList[i * 2 + 1] = index;
+					}
+				}
+				indexCount ++;
+			}
+		}
+		indexCount += 6;
+
+
+		dgBigVector size (p1 - p0);
+		double volume = size.m_x * size.m_y * size.m_z;
+
+
+		for (float pitch = float (0.0f); pitch < float (90.0f); pitch += float (10.0f)) {
+			dgMatrix pitchMatrix (dgPitchMatrix(pitch * float (3.1416f) / float (180.0f)));
+			for (float yaw = float (0.0f); yaw  < float (90.0f); yaw  += float (10.0f)) {
+				dgMatrix yawMatrix (dgYawMatrix(yaw * float (3.1416f) / float (180.0f)));
+				for (float roll = float (0.0f); roll < float (90.0f); roll += float (10.0f)) {
+					int32_t tmpIndex[6];
+					dgMatrix rollMatrix (dgRollMatrix(roll * float (3.1416f) / float (180.0f)));
+					dgMatrix tmp (pitchMatrix * yawMatrix * rollMatrix);
+					dgBigVector q0 (float ( 1.0e10f), float ( 1.0e10f), float ( 1.0e10f), float (0.0f));
+					dgBigVector q1 (float (-1.0e10f), float (-1.0e10f), float (-1.0e10f), float (0.0f));
+
+					float volume1 = float (1.0e10f);
+					for (int32_t i = 0; i < indexCount; i ++) {
+						int32_t index = indexList[i];
+						dgBigVector point (vertex[index * stride + 0], vertex[index * stride + 1], vertex[index * stride + 2], float (0.0f));
+						point = tmp.UnrotateVector(point);
+
+						for (int32_t j = 0; j < 3; j ++) {
+							if (point[j] < q0[j]) {
+								q0[j] = point[j];
+								tmpIndex[j * 2 + 0] = index;
+							}
+							if (point[j] > q1[j]) {
+								q1[j] = point[j];
+								tmpIndex[j * 2 + 1] = index;
+							}
+						}
+
+
+						dgVector size1 (q1 - q0);
+						volume1 = size1.m_x * size1.m_y * size1.m_z;
+						if (volume1 >= volume) {
+							break;
+						}
+					}
+
+					if (volume1 < volume) {
+						p0 = q0;
+						p1 = q1;
+						axis = tmp;
+						volume = volume1;
+						memcpy (indexList, tmpIndex, sizeof (tmpIndex));
+					}
+				}
+			}
+		}
+
+		HACD_ASSERT (0);
+		dgSphere sphere (axis);
+		dgVector q0 (p0);
+		dgVector q1 (p1);
+		sphere.m_posit = axis.RotateVector((q1 + q0).Scale (float (0.5f)));
+		sphere.m_size = (q1 - q0).Scale (float (0.5f));
+		return sphere;
+*/
+
+	int32_t stride = int32_t (strideInBytes / sizeof (double));	
+
+	int32_t vertexCount = 0;
+	int32_t mark = IncLRU();
+	dgPolyhedra::Iterator iter(*this);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_mark != mark) {
+			dgEdge* ptr = edge;
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_twin->m_next;
+			} while (ptr != edge);
+			vertexCount ++;
+		}
+	}
+	HACD_ASSERT (vertexCount);
+
+	mark = IncLRU();
+	int32_t vertexCountIndex = 0;
+	dgStack<dgBigVector> pool (vertexCount);
+	for (iter.Begin(); iter; iter ++) {
+		dgEdge* const edge = &(*iter);
+		if (edge->m_mark != mark) {
+			dgEdge* ptr = edge;
+			do {
+				ptr->m_mark = mark;
+				ptr = ptr->m_twin->m_next;
+			} while (ptr != edge);
+			int32_t incidentVertex = edge->m_incidentVertex * stride;
+			pool[vertexCountIndex] = dgBigVector (vertex[incidentVertex + 0], vertex[incidentVertex + 1], vertex[incidentVertex + 2], float (0.0f));
+			vertexCountIndex ++;
+		}
+	}
+	HACD_ASSERT (vertexCountIndex <= vertexCount);
+
+	dgMatrix axis (dgGetIdentityMatrix());
+	dgSphere sphere (axis);
+	dgConvexHull3d convexHull (&pool[0].m_x, sizeof (dgBigVector), vertexCountIndex, 0.0f);
+	if (convexHull.GetCount()) {
+		dgStack<int32_t> triangleList (convexHull.GetCount() * 3); 				
+		int32_t trianglesCount = 0;
+		for (dgConvexHull3d::dgListNode* node = convexHull.GetFirst(); node; node = node->GetNext()) {
+			dgConvexHull3DFace* const face = &node->GetInfo();
+			triangleList[trianglesCount * 3 + 0] = face->m_index[0];
+			triangleList[trianglesCount * 3 + 1] = face->m_index[1];
+			triangleList[trianglesCount * 3 + 2] = face->m_index[2];
+			trianglesCount ++;
+			HACD_ASSERT ((trianglesCount * 3) <= triangleList.GetElementsCount());
+		}
+
+		dgVector* const dst = (dgVector*) &pool[0].m_x;
+		for (int32_t i = 0; i < convexHull.GetVertexCount(); i ++) {
+			dst[i] = convexHull.GetVertex(i);
+		}
+		sphere.SetDimensions (&dst[0].m_x, sizeof (dgVector), &triangleList[0], trianglesCount * 3, NULL);
+
+	} else if (vertexCountIndex >= 3) {
+		dgStack<int32_t> triangleList (GetCount() * 3 * 2); 
+		int32_t mark = IncLRU();
+		int32_t trianglesCount = 0;
+		for (iter.Begin(); iter; iter ++) {
+			dgEdge* const edge = &(*iter);
+			if (edge->m_mark != mark) {
+				dgEdge* ptr = edge;
+				do {
+					ptr->m_mark = mark;
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != edge);
+
+				ptr = edge->m_next->m_next;
+				do {
+					triangleList[trianglesCount * 3 + 0] = edge->m_incidentVertex;
+					triangleList[trianglesCount * 3 + 1] = ptr->m_prev->m_incidentVertex;
+					triangleList[trianglesCount * 3 + 2] = ptr->m_incidentVertex;
+					trianglesCount ++;
+					HACD_ASSERT ((trianglesCount * 3) <= triangleList.GetElementsCount());
+					ptr = ptr->m_twin->m_next;
+				} while (ptr != edge);
+
+				dgVector* const dst = (dgVector*) &pool[0].m_x;
+				for (int32_t i = 0; i < vertexCountIndex; i ++) {
+					dst[i] = pool[i];
+				}
+				sphere.SetDimensions (&dst[0].m_x, sizeof (dgVector), &triangleList[0], trianglesCount * 3, NULL);
+			}
+		}
+	}
+	return sphere;
+
+}