/* 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 "dgCollisionCompoundBreakable.h"
#include "dgCollisionConvexHull.h"
#include "dgMeshEffect.h"
#include "dgWorld.h"
#include "hpl1/engine/libraries/newton/core/dg.h"


#define DG_DYNAMINIC_ISLAND_COST 0x7fffffff

dgCollisionCompoundBreakable::dgCollisionConvexIntance::dgCollisionConvexIntance(
    dgCollisionConvex *convexChild,
    dgCollisionCompoundBreakable::dgDebriGraph::dgListNode *node,
    dgFloat32 density) : dgCollisionConvex(convexChild->GetAllocator(), 0, dgGetIdentityMatrix(),
	            m_convexConvexIntance) {
	m_treeNode = NULL;
	m_graphNode = node;
	m_myShape = convexChild;
	m_myShape->AddRef();
	SetOffsetMatrix(m_myShape->GetOffsetMatrix());

	((dgCollision *)convexChild)->CalculateInertia(m_inertia, m_volume);
	m_volume.m_w = ((dgCollision *)convexChild)->GetVolume();

	m_inertia = m_inertia.Scale(density * m_volume.m_w);
	m_inertia.m_w = density * m_volume.m_w;

	SetBreakImpulse(2500);

	m_vertexCount = 1;
}

dgCollisionCompoundBreakable::dgCollisionConvexIntance::dgCollisionConvexIntance(
    const dgCollisionConvexIntance &source,
    dgCollisionCompoundBreakable::dgDebriGraph::dgListNode *node) : dgCollisionConvex(source.GetAllocator(), 0, source.m_offset,
	            m_convexConvexIntance),
	m_inertia(source.m_inertia) {
	m_treeNode = NULL;
	m_graphNode = node;
	m_myShape = source.m_myShape;
	m_myShape->AddRef();
	m_vertexCount = 1;
	m_volume = source.m_volume;
	m_destructionImpulse = source.m_destructionImpulse;
}

dgCollisionCompoundBreakable::dgCollisionConvexIntance::dgCollisionConvexIntance(
    dgWorld *const world, dgDeserialize deserialization, void *const userData) : dgCollisionConvex(world, deserialization, userData) {
	m_graphNode = NULL;
	m_treeNode = NULL;
	m_vertexCount = 1;

	deserialization(userData, &m_volume, sizeof(m_volume));
	deserialization(userData, &m_inertia, sizeof(m_inertia));
	deserialization(userData, &m_destructionImpulse,
	                sizeof(m_destructionImpulse));

	m_myShape = new (world->GetAllocator()) dgCollisionConvexHull(world,
	        deserialization, userData);
}

dgCollisionCompoundBreakable::dgCollisionConvexIntance::~dgCollisionConvexIntance() {
	m_myShape->Release();
}

dgFloat32 dgCollisionCompoundBreakable::dgCollisionConvexIntance::GetVolume() const {
	return ((dgCollision *)m_myShape)->GetVolume();
}

bool dgCollisionCompoundBreakable::dgCollisionConvexIntance::OOBBTest(
    const dgMatrix &matrix, const dgCollisionConvex *const shape,
    void *const cacheOrder) const {
	return m_myShape->OOBBTest(matrix, shape, cacheOrder);
}

dgVector dgCollisionCompoundBreakable::dgCollisionConvexIntance::SupportVertex(
    const dgVector &dir) const {
	return m_myShape->SupportVertex(dir);
}

dgVector dgCollisionCompoundBreakable::dgCollisionConvexIntance::SupportVertexSimd(
    const dgVector &dir) const {
	return m_myShape->SupportVertexSimd(dir);
}

void dgCollisionCompoundBreakable::dgCollisionConvexIntance::CalcAABB(
    const dgMatrix &matrix, dgVector &p0, dgVector &p1) const {
	m_myShape->CalcAABB(matrix, p0, p1);
}

void dgCollisionCompoundBreakable::dgCollisionConvexIntance::CalcAABBSimd(
    const dgMatrix &matrix, dgVector &p0, dgVector &p1) const {
	m_myShape->CalcAABBSimd(matrix, p0, p1);
}

void dgCollisionCompoundBreakable::dgCollisionConvexIntance::DebugCollision(
    const dgMatrix &matrix, OnDebugCollisionMeshCallback callback,
    void *const userData) const {
	((dgCollision *)m_myShape)->DebugCollision(matrix, callback, userData);
}

dgFloat32 dgCollisionCompoundBreakable::dgCollisionConvexIntance::RayCast(
    const dgVector &localP0, const dgVector &localP1,
    dgContactPoint &contactOut, OnRayPrecastAction preFilter,
    const dgBody *const body, void *const userData) const {
	return m_myShape->RayCast(localP0, localP1, contactOut, preFilter, body,
	                          userData);
}

dgFloat32 dgCollisionCompoundBreakable::dgCollisionConvexIntance::RayCastSimd(
    const dgVector &localP0, const dgVector &localP1,
    dgContactPoint &contactOut, OnRayPrecastAction preFilter,
    const dgBody *const body, void *const userData) const {
	return m_myShape->RayCastSimd(localP0, localP1, contactOut, preFilter, body,
	                              userData);
}

dgVector dgCollisionCompoundBreakable::dgCollisionConvexIntance::CalculateVolumeIntegral(
    const dgMatrix &globalMatrix, GetBuoyancyPlane bouyancyPlane,
    void *const context) const {
	return ((dgCollision *)m_myShape)->CalculateVolumeIntegral(globalMatrix, bouyancyPlane, context);
}

dgInt32 dgCollisionCompoundBreakable::dgCollisionConvexIntance::CalculateSignature() const {
	return 0;
	//      return ((dgCollision*)m_myShape)->CalculateSignature ();
}

void dgCollisionCompoundBreakable::dgCollisionConvexIntance::SetCollisionBBox(
    const dgVector &p0, const dgVector &p1) {
	m_myShape->SetCollisionBBox(p0, p1);
}

dgFloat32 dgCollisionCompoundBreakable::dgCollisionConvexIntance::GetBoxMinRadius() const {
	return ((dgCollision *)m_myShape)->GetBoxMinRadius();
}

dgFloat32 dgCollisionCompoundBreakable::dgCollisionConvexIntance::GetBoxMaxRadius() const {
	return ((dgCollision *)m_myShape)->GetBoxMaxRadius();
}

dgInt32 dgCollisionCompoundBreakable::dgCollisionConvexIntance::CalculatePlaneIntersection(
    const dgVector &normal, const dgVector &point,
    dgVector *const contactsOut) const {
	return m_myShape->CalculatePlaneIntersection(normal, point, contactsOut);
}

dgInt32 dgCollisionCompoundBreakable::dgCollisionConvexIntance::CalculatePlaneIntersectionSimd(
    const dgVector &normal, const dgVector &point,
    dgVector *const contactsOut) const {
	return m_myShape->CalculatePlaneIntersectionSimd(normal, point, contactsOut);
}

void dgCollisionCompoundBreakable::dgCollisionConvexIntance::GetCollisionInfo(
    dgCollisionInfo *info) const {
	m_myShape->GetCollisionInfo(info);
}

void dgCollisionCompoundBreakable::dgCollisionConvexIntance::Serialize(
    dgSerialize callback, void *const userData) const {
	SerializeLow(callback, userData);

	callback(userData, &m_volume, sizeof(m_volume));
	callback(userData, &m_inertia, sizeof(m_inertia));
	callback(userData, &m_destructionImpulse, sizeof(m_destructionImpulse));

	m_myShape->Serialize(callback, userData);
}

void dgCollisionCompoundBreakable::dgCollisionConvexIntance::SetBreakImpulse(
    dgFloat32 force) {
	m_destructionImpulse = force;
}

dgFloat32 dgCollisionCompoundBreakable::dgCollisionConvexIntance::GetBreakImpulse() const {
	return m_destructionImpulse;
}

dgCollisionCompoundBreakable::dgVertexBuffer::dgVertexBuffer(dgInt32 vertsCount,
        dgMemoryAllocator *allocator) {
	m_allocator = allocator;
	m_vertexCount = vertsCount;
	m_uv = (dgFloat32 *)m_allocator->Malloc(
	           2 * vertsCount * dgInt32(sizeof(dgFloat32)));
	m_vertex = (dgFloat32 *)m_allocator->Malloc(
	               3 * vertsCount * dgInt32(sizeof(dgFloat32)));
	m_normal = (dgFloat32 *)m_allocator->Malloc(
	               3 * vertsCount * dgInt32(sizeof(dgFloat32)));
}

dgCollisionCompoundBreakable::dgVertexBuffer::~dgVertexBuffer() {
	m_allocator->Free(m_normal);
	m_allocator->Free(m_vertex);
	m_allocator->Free(m_uv);
}

dgCollisionCompoundBreakable::dgVertexBuffer::dgVertexBuffer(
    dgMemoryAllocator *const allocator, dgDeserialize callback,
    void *const userData) {
	m_allocator = allocator;
	callback(userData, &m_vertexCount, dgInt32(sizeof(dgInt32)));

	m_uv = (dgFloat32 *)m_allocator->Malloc(
	           2 * m_vertexCount * dgInt32(sizeof(dgFloat32)));
	m_vertex = (dgFloat32 *)m_allocator->Malloc(
	               3 * m_vertexCount * dgInt32(sizeof(dgFloat32)));
	m_normal = (dgFloat32 *)m_allocator->Malloc(
	               3 * m_vertexCount * dgInt32(sizeof(dgFloat32)));

	callback(userData, m_vertex,
	         size_t(3 * m_vertexCount * dgInt32(sizeof(dgFloat32))));
	callback(userData, m_normal,
	         size_t(3 * m_vertexCount * dgInt32(sizeof(dgFloat32))));
	callback(userData, m_uv,
	         size_t(2 * m_vertexCount * dgInt32(sizeof(dgFloat32))));
}

void dgCollisionCompoundBreakable::dgVertexBuffer::Serialize(
    dgSerialize callback, void *const userData) const {
	callback(userData, &m_vertexCount, dgInt32(sizeof(dgInt32)));
	callback(userData, m_vertex,
	         size_t(3 * m_vertexCount * dgInt32(sizeof(dgFloat32))));
	callback(userData, m_normal,
	         size_t(3 * m_vertexCount * dgInt32(sizeof(dgFloat32))));
	callback(userData, m_uv,
	         size_t(2 * m_vertexCount * dgInt32(sizeof(dgFloat32))));
}

void dgCollisionCompoundBreakable::dgVertexBuffer::GetVertexStreams(
    dgInt32 vertexStrideInByte, dgFloat32 *vertex, dgInt32 normalStrideInByte,
    dgFloat32 *normal, dgInt32 uvStrideInByte, dgFloat32 *uv) const {
	uvStrideInByte /= dgInt32(sizeof(dgFloat32));
	vertexStrideInByte /= dgInt32(sizeof(dgFloat32));
	normalStrideInByte /= dgInt32(sizeof(dgFloat32));
	for (dgInt32 i = 0; i < m_vertexCount; i++) {
		dgInt32 j = i * vertexStrideInByte;
		vertex[j + 0] = m_vertex[i * 3 + 0];
		vertex[j + 1] = m_vertex[i * 3 + 1];
		vertex[j + 2] = m_vertex[i * 3 + 2];

		j = i * normalStrideInByte;
		normal[j + 0] = m_normal[i * 3 + 0];
		normal[j + 1] = m_normal[i * 3 + 1];
		normal[j + 2] = m_normal[i * 3 + 2];

		j = i * uvStrideInByte;
		uv[j + 0] = m_uv[i * 2 + 0];
		uv[j + 1] = m_uv[i * 2 + 1];
	}
}

dgCollisionCompoundBreakable::dgSubMesh::dgSubMesh(
    dgMemoryAllocator *const allocator) {
	m_material = 0;
	m_faceCount = 0;
	m_indexes = NULL;
	m_faceOffset = 0;
	m_visibleFaces = 1;
	m_allocator = allocator;
}

dgCollisionCompoundBreakable::dgSubMesh::~dgSubMesh() {
	if (m_indexes) {
		//      m_allocator->Free (m_visibilityMap);
		m_allocator->Free(m_indexes);
	}
}

void dgCollisionCompoundBreakable::dgSubMesh::Serialize(dgSerialize callback,
        void *const userData) const {
	callback(userData, &m_material, dgInt32(sizeof(dgInt32)));
	callback(userData, &m_faceCount, dgInt32(sizeof(dgInt32)));
	callback(userData, &m_faceOffset, dgInt32(sizeof(dgInt32)));
	callback(userData, &m_visibleFaces, dgInt32(sizeof(dgInt32)));
	callback(userData, m_indexes,
	         size_t(3 * m_faceCount * dgInt32(sizeof(dgInt32))));
}

dgCollisionCompoundBreakable::dgMesh::dgMesh(dgMemoryAllocator *const allocator) : dgList<dgSubMesh>(allocator) {
	m_IsVisible = 1;
}

dgCollisionCompoundBreakable::dgMesh::~dgMesh() {
}

dgCollisionCompoundBreakable::dgMesh::dgMesh(dgMemoryAllocator *const allocator,
        dgDeserialize callback, void *const userData) : dgList<dgSubMesh>(allocator), dgRefCounter() {
	dgInt32 count;

	callback(userData, &m_IsVisible, dgInt32(sizeof(dgInt32)));
	callback(userData, &count, dgInt32(sizeof(dgInt32)));
	for (dgInt32 i = 0; i < count; i++) {
		dgInt32 material;
		dgInt32 faceCount;
		dgInt32 faceOffset;
		dgInt32 exteriorFaces;

		callback(userData, &material, dgInt32(sizeof(dgInt32)));
		callback(userData, &faceCount, dgInt32(sizeof(dgInt32)));
		callback(userData, &faceOffset, dgInt32(sizeof(dgInt32)));
		callback(userData, &exteriorFaces, dgInt32(sizeof(dgInt32)));
		dgSubMesh *const subMesh = AddgSubMesh(faceCount * 3, material);

		subMesh->m_faceOffset = faceOffset;
		subMesh->m_visibleFaces = exteriorFaces;

		//      callback (userData, subMesh->m_visibilityMap, faceCount * dgInt32 (sizeof (dgInt32)));
		callback(userData, subMesh->m_indexes,
		         size_t(3 * faceCount * sizeof(dgInt32)));
	}
}

void dgCollisionCompoundBreakable::dgMesh::Serialize(dgSerialize callback,
        void *const userData) const {
	dgInt32 count;
	count = GetCount();

	callback(userData, &m_IsVisible, dgInt32(sizeof(dgInt32)));
	callback(userData, &count, dgInt32(sizeof(dgInt32)));
	for (dgListNode *node = GetFirst(); node; node = node->GetNext()) {
		dgSubMesh &subMesh = node->GetInfo();
		subMesh.Serialize(callback, userData);
	}
}

dgCollisionCompoundBreakable::dgSubMesh *dgCollisionCompoundBreakable::dgMesh::AddgSubMesh(
    dgInt32 indexCount, dgInt32 material) {
	dgSubMesh tmp(GetAllocator());
	dgSubMesh &subMesh = Append(tmp)->GetInfo();

	subMesh.m_faceOffset = 0;
	subMesh.m_visibleFaces = 1;

	subMesh.m_material = material;
	subMesh.m_faceCount = indexCount / 3;

	subMesh.m_indexes = (dgInt32 *)subMesh.m_allocator->Malloc(
	                        indexCount * dgInt32(sizeof(dgInt32)));
	return &subMesh;
}

dgCollisionCompoundBreakable::dgDebriNodeInfo::dgDebriNodeInfo() {
	m_mesh = NULL;
	m_shape = NULL;
	memset(&m_commonData, 0, sizeof(m_commonData));
}

dgCollisionCompoundBreakable::dgDebriNodeInfo::~dgDebriNodeInfo() {
	if (m_shape) {
		m_shape->Release();
	}

	if (m_mesh) {
		m_mesh->Release();
	}
}

dgCollisionCompoundBreakable::dgSharedNodeMesh::dgSharedNodeMesh() {
}

dgCollisionCompoundBreakable::dgSharedNodeMesh::~dgSharedNodeMesh() {
}

dgCollisionCompoundBreakable::dgDebriGraph::dgDebriGraph(
    dgMemoryAllocator *const allocator) : dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>(allocator) {
}

dgCollisionCompoundBreakable::dgDebriGraph::dgDebriGraph(
    dgMemoryAllocator *const allocator, dgDeserialize callback,
    void *const userData) : dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>(allocator) {
	dgInt32 count;
	dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *n;

	callback(userData, &count, dgInt32(sizeof(dgInt32)));
	dgStack<dgListNode *> nodesMap(count);

	n = dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::AddNode();
	dgDebriNodeInfo &dataN = GetFirst()->GetInfo().m_nodeData;
	callback(userData, &dataN.m_commonData, sizeof(dataN.m_commonData));

	nodesMap[0] = n;
	for (dgInt32 i = 1; i < count; i++) {
		dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *node;

		node = dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::AddNode();
		dgDebriNodeInfo &data = node->GetInfo().m_nodeData;
		callback(userData, &data.m_commonData, sizeof(data.m_commonData));
		data.m_mesh = new (GetAllocator()) dgMesh(GetAllocator(), callback,
		        userData);
		nodesMap[i] = node;
	}

	for (dgInt32 i = 0; i < count - 1; i++) {
		dgInt32 edges;
		callback(userData, &edges, dgInt32(sizeof(dgInt32)));
		dgStack<dgInt32> pool(edges);
		callback(userData, &pool[0], size_t(edges * dgInt32(sizeof(dgInt32))));
		for (dgInt32 j = 0; j < edges; j++) {
			nodesMap[i]->GetInfo().AddEdge(nodesMap[pool[j]]);
		}
	}
}

dgCollisionCompoundBreakable::dgDebriGraph::dgDebriGraph(
    const dgDebriGraph &source) : dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>(source.GetAllocator()) {
	dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *newNode;
	dgTree<dgListNode *, dgListNode *> filter(GetAllocator());

	newNode = dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::AddNode();
	dgDebriNodeInfo &data = newNode->GetInfo().m_nodeData;
	dgDebriNodeInfo &srcData = source.GetFirst()->GetInfo().m_nodeData;

	data.m_commonData = srcData.m_commonData;

	filter.Insert(newNode, source.GetFirst());
	for (dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *node =
	            source.GetFirst()->GetNext();
	        node; node = node->GetNext())

	{
		newNode = dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::AddNode();

		dgDebriNodeInfo &sourceData = node->GetInfo().m_nodeData;
		dgDebriNodeInfo &destData = newNode->GetInfo().m_nodeData;

		destData.m_commonData = sourceData.m_commonData;
		destData.m_mesh = sourceData.m_mesh;
		destData.m_mesh->AddRef();

		filter.Insert(newNode, node);
	}

	for (dgListNode *node = source.GetFirst(); node; node = node->GetNext()) {
		dgListNode *myNode = filter.Find(node)->GetInfo();
		for (dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *edgeNode =
		            node->GetInfo().GetFirst();
		        edgeNode; edgeNode = edgeNode->GetNext()) {
			dgListNode *otherNode;
			otherNode = filter.Find(edgeNode->GetInfo().m_node)->GetInfo();
			myNode->GetInfo().AddEdge(otherNode);
		}
	}
}

dgCollisionCompoundBreakable::dgDebriGraph::~dgDebriGraph() {
}

void dgCollisionCompoundBreakable::dgDebriGraph::Serialize(dgSerialize callback,
        void *const userData) const {
	dgInt32 index;
	dgInt32 count;
	dgTree<dgInt32, dgListNode *> enumerator(GetAllocator());

	index = 1;
	count = GetCount();
	callback(userData, &count, dgInt32(sizeof(dgInt32)));
	dgDebriNodeInfo &data = GetFirst()->GetInfo().m_nodeData;

	dgDebriNodeInfo::PackedSaveData packedData(data.m_commonData);
	packedData.m_lru = 0;
	callback(userData, &packedData, sizeof(packedData));
	enumerator.Insert(dgInt32(0), GetFirst());

	for (dgListNode *node = GetFirst()->GetNext(); node; node = node->GetNext()) {
		dgDebriNodeInfo &dataN = node->GetInfo().m_nodeData;
		dgDebriNodeInfo::PackedSaveData packedDataN(dataN.m_commonData);
		packedDataN.m_lru = 0;
		callback(userData, &packedDataN, sizeof(packedDataN));

		dataN.m_mesh->Serialize(callback, userData);
		enumerator.Insert(index, node);
		index++;
	}

	for (dgListNode *node = GetFirst(); node != GetLast(); node = node->GetNext()) {
		dgInt32 countN;

		index = 0;
		countN = node->GetInfo().GetCount();
		callback(userData, &countN, dgInt32(sizeof(dgInt32)));
		dgStack<dgInt32> buffer(countN);
		dgInt32 *const pool = &buffer[0];
		for (dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *edgeNode =
		            node->GetInfo().GetFirst();
		        edgeNode; edgeNode = edgeNode->GetNext()) {
			pool[index] = enumerator.Find(edgeNode->GetInfo().m_node)->GetInfo();
			index++;
		}
		callback(userData, &pool[0], size_t(countN * sizeof(dgInt32)));
	}
}

void dgCollisionCompoundBreakable::dgDebriGraph::AddNode(
    dgFlatVertexArray &flatArray, dgMeshEffect *solid, dgInt32 clipperMaterial,
    dgInt32 id, dgFloat32 density, dgFloat32 padding) {
	NEWTON_ASSERT(0);
	/*
	 dgCollision* collision;

	 collision =  solid->CreateConvexCollision(dgFloat32 (0.005f), id, padding, dgGetIdentityMatrix());
	 NEWTON_ASSERT (collision);
	 if (collision) {
	 dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode* node;
	 dgInt32 vertexCount;
	 dgInt32 baseVertexCount;
	 dgCollision* collisionInstance;
	 dgMeshEffect::dgIndexArray* geometryHandle;

	 node =  dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::AddNode ();
	 dgDebriNodeInfo& data = node->GetInfo().m_nodeData;

	 collisionInstance = new (GetAllocator()) dgCollisionConvexIntance ((dgCollisionConvex*) collision, node, density);
	 collision->Release();

	 data.m_mesh = new (GetAllocator()) dgMesh(GetAllocator());
	 data.m_shape = (dgCollisionConvex*)collisionInstance;

	 vertexCount = solid->GetPropertiesCount() * 6;
	 dgStack<dgVector>vertex (vertexCount);
	 dgStack<dgVector>normal (vertexCount);
	 dgStack<dgVector>uv0 (vertexCount);
	 dgStack<dgVector>uv1 (vertexCount);

	 solid->GetVertexStreams (sizeof (dgVector), &vertex[0].m_x, sizeof (dgVector), &normal[0].m_x, sizeof (dgVector), &uv0[0].m_x, sizeof (dgVector), &uv1[0].m_x);

	 // extract the materials index array for mesh
	 geometryHandle = solid->MaterialGeometryBegin();

	 data.m_mesh->m_IsVisible = 0;
	 vertexCount = flatArray.m_count;
	 baseVertexCount = flatArray.m_count;
	 for (dgInt32 handle = solid->GetFirstMaterial (geometryHandle); handle != -1; handle = solid->GetNextMaterial (geometryHandle, handle)) {
	 dgInt32 isVisible;
	 dgInt32 material;
	 dgInt32 indexCount;
	 dgSubMesh* segment;

	 //         isVisible = 1;
	 material = solid->GetMaterialID (geometryHandle, handle);
	 //         if (material == clipperMaterial) {
	 //             isVisible = 0;
	 //         }
	 isVisible = (material != clipperMaterial) ? 1 : 0;
	 data.m_mesh->m_IsVisible |= isVisible;

	 indexCount = solid->GetMaterialIndexCount (geometryHandle, handle);
	 segment = data.m_mesh->AddgSubMesh(indexCount, material);
	 segment->m_visibleFaces = isVisible;

	 solid->GetMaterialGetIndexStream (geometryHandle, handle, segment->m_indexes);
	 for (dgInt32 i = 0; i < indexCount; i ++) {
	 dgInt32 j;
	 j = segment->m_indexes[i];
	 flatArray[vertexCount].m_point[0] = vertex[j].m_x;
	 flatArray[vertexCount].m_point[1] = vertex[j].m_y;
	 flatArray[vertexCount].m_point[2] = vertex[j].m_z;
	 flatArray[vertexCount].m_point[3] = normal[j].m_x;
	 flatArray[vertexCount].m_point[4] = normal[j].m_y;
	 flatArray[vertexCount].m_point[5] = normal[j].m_z;
	 flatArray[vertexCount].m_point[6] = uv0[j].m_x;
	 flatArray[vertexCount].m_point[7] = uv0[j].m_y;
	 flatArray[vertexCount].m_point[8] = uv1[j].m_x;
	 flatArray[vertexCount].m_point[9] = uv1[j].m_y;
	 segment->m_indexes[i] = vertexCount - baseVertexCount;
	 vertexCount ++;
	 //     NEWTON_ASSERT ((vertexCount - baseVertexCount) < dgInt32 (indexBuffer.GetElementsCount()));
	 }
	 }
	 solid->MaterialGeomteryEnd(geometryHandle);

	 dgStack<dgInt32> indexBuffer (vertexCount - baseVertexCount);
	 flatArray.m_count += dgVertexListToIndexList (&flatArray[baseVertexCount].m_point[0], sizeof (dgFlatVertex), sizeof (dgFlatVertex), 0, vertexCount - baseVertexCount, &indexBuffer[0], dgFloat32 (1.0e-6f));

	 for (dgMesh::dgListNode* meshSgement = data.m_mesh->GetFirst(); meshSgement; meshSgement = meshSgement->GetNext()) {
	 dgSubMesh* const subMesh = &meshSgement->GetInfo();
	 for (dgInt32 i = 0; i < subMesh->m_faceCount * 3; i ++) {
	 dgInt32 j;
	 j = subMesh->m_indexes[i];
	 subMesh->m_indexes[i] = baseVertexCount + indexBuffer[j];
	 }
	 }
	 }
	 */
}

void dgCollisionCompoundBreakable::dgDebriGraph::AddMeshes(
    dgFlatVertexArray &vertexArray, dgInt32 count,
    dgMeshEffect *const solidArray[], const dgInt32 *const idArray,
    const dgFloat32 *const densities, const dgInt32 *const internalFaceMaterial,
    dgFloat32 padding) {
	// padding = 0.0f;

	//dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *fixNode;

	/*fixNode =*/ dgGraph<dgDebriNodeInfo, dgSharedNodeMesh>::AddNode();
	for (dgInt32 i = 0; i < count; i++) {
		AddNode(vertexArray, (dgMeshEffect *)solidArray[i], internalFaceMaterial[i],
		        idArray[i], densities[i], padding);
	}
}

dgCollisionCompoundBreakable::dgCollisionCompoundBreakable(dgInt32 count,
        dgMeshEffect *const solidArray[], const dgInt32 *const idArray,
        const dgFloat32 *const densities, const dgInt32 *const internalFaceMaterial,
        dgInt32 debriiId, dgFloat32 collisionPadding, dgWorld *world) : dgCollisionCompound(world), m_conectivity(world->GetAllocator()), m_detachedIslands(world->GetAllocator()) {
	dgInt32 acc;
	dgInt32 indexCount;
	dgInt32 vertsCount;
	dgInt32 materialHitogram[256];
	dgInt32 faceOffsetHitogram[256];
	dgSubMesh *mainSegmenst[256];
	dgMesh *mainMesh;
	dgFlatVertexArray vertexArray(m_world->GetAllocator());

	m_lru = 0;
	m_lastIslandColor = 0;
	m_visibilityMapIndexCount = 0;
	m_vertexBuffer = NULL;
	m_visibilityMap = NULL;
	m_visibilityInderectMap = NULL;
	m_collsionId = m_compoundBreakable;
	m_rtti |= dgCollisionCompoundBreakable_RTTI;

	if (collisionPadding < dgFloat32(0.01f)) {
		collisionPadding = dgFloat32(0.01f);
	}
	m_conectivity.AddMeshes(vertexArray, count, solidArray, idArray, densities,
	                        internalFaceMaterial, collisionPadding);

	dgStack<dgCollisionConvex *> shapeArrayPool(m_conectivity.GetCount());
	dgCollisionConvex **const shapeArray = &shapeArrayPool[0];

	indexCount = 0;
	for (dgDebriGraph::dgListNode *node = m_conectivity.GetFirst()->GetNext();
	        node; node = node->GetNext()) {
		dgDebriNodeInfo &data = node->GetInfo().m_nodeData;
		shapeArray[indexCount] = data.m_shape;
		indexCount++;
	}

	if (indexCount) {
		m_root = BuildTree(indexCount, &shapeArray[0]);
	}
	Init(indexCount, &shapeArray[0]);

	dgStack<dgInt32> indexBuffer(vertexArray.m_count);
	vertsCount = dgVertexListToIndexList(&vertexArray[0].m_point[0],
	                                     sizeof(dgFlatVertex), sizeof(dgFlatVertex), 0, vertexArray.m_count,
	                                     &indexBuffer[0], dgFloat32(1.0e-6f));

	m_vertexBuffer = new (m_world->GetAllocator()) dgVertexBuffer(vertsCount,
	        m_world->GetAllocator());
	for (dgInt32 i = 0; i < vertsCount; i++) {
		m_vertexBuffer->m_vertex[i * 3 + 0] = vertexArray[i].m_point[0];
		m_vertexBuffer->m_vertex[i * 3 + 1] = vertexArray[i].m_point[1];
		m_vertexBuffer->m_vertex[i * 3 + 2] = vertexArray[i].m_point[2];
		m_vertexBuffer->m_normal[i * 3 + 0] = vertexArray[i].m_point[3];
		m_vertexBuffer->m_normal[i * 3 + 1] = vertexArray[i].m_point[4];
		m_vertexBuffer->m_normal[i * 3 + 2] = vertexArray[i].m_point[5];
		m_vertexBuffer->m_uv[i * 2 + 0] = vertexArray[i].m_point[6];
		m_vertexBuffer->m_uv[i * 2 + 1] = vertexArray[i].m_point[7];
	}

	memset(materialHitogram, 0, sizeof(materialHitogram));
	memset(faceOffsetHitogram, 0, sizeof(faceOffsetHitogram));
	memset(mainSegmenst, 0, sizeof(mainSegmenst));
	for (dgDebriGraph::dgListNode *node = m_conectivity.GetFirst()->GetNext();
	        node; node = node->GetNext()) {
		dgDebriNodeInfo &data = node->GetInfo().m_nodeData;
		for (dgMesh::dgListNode *meshSgement = data.m_mesh->GetFirst(); meshSgement;
		        meshSgement = meshSgement->GetNext()) {
			dgInt32 material;
			dgSubMesh *const subMesh = &meshSgement->GetInfo();
			material = subMesh->m_material;
			materialHitogram[material] += subMesh->m_faceCount;
		}
	}

	dgDebriNodeInfo &mainNodeData = m_conectivity.dgGraph<dgDebriNodeInfo,
	                 dgSharedNodeMesh>::AddNode()
	                 ->GetInfo()
	                 .m_nodeData;

	acc = 0;
	mainMesh = new (m_world->GetAllocator()) dgMesh(m_world->GetAllocator());
	mainNodeData.m_mesh = mainMesh;
	for (dgInt32 i = 0; i < 256; i++) {
		if (materialHitogram[i]) {
			dgSubMesh *segment;
			segment = mainMesh->AddgSubMesh(materialHitogram[i] * 3, i);
			segment->m_faceOffset = acc;
			segment->m_faceCount = 0;
			mainSegmenst[i] = segment;
		}
		faceOffsetHitogram[i] = acc;
		acc += materialHitogram[i];
	}

	m_visibilityMapIndexCount = acc;
	m_visibilityMap = (dgInt8 *)m_allocator->Malloc(
	                      dgInt32(acc * sizeof(dgInt8)));
	m_visibilityInderectMap = (dgInt32 *)m_allocator->Malloc(
	                              acc * dgInt32(sizeof(dgInt32)));
	acc = 0;

	for (dgDebriGraph::dgListNode *node = m_conectivity.GetFirst()->GetNext();
	        node != m_conectivity.GetLast(); node = node->GetNext()) {
		dgDebriNodeInfo &data = node->GetInfo().m_nodeData;

		for (dgMesh::dgListNode *nodeF = data.m_mesh->GetFirst(); nodeF;
		        nodeF = nodeF->GetNext()) {
			dgInt8 visbility;
			dgInt32 rootIndexCount;

			dgSubMesh &segment = nodeF->GetInfo();
			dgSubMesh &rootSegment = *mainSegmenst[segment.m_material];

			visbility = dgInt8(segment.m_visibleFaces);

			memset(&m_visibilityMap[acc], visbility, size_t(segment.m_faceCount));
			for (dgInt32 i = 0; i < segment.m_faceCount; i++) {
				m_visibilityInderectMap[faceOffsetHitogram[segment.m_material] + i] =
				    acc + i;
			}
			faceOffsetHitogram[segment.m_material] += segment.m_faceCount;

			rootIndexCount = rootSegment.m_faceCount * 3;
			for (dgInt32 i = 0; i < segment.m_faceCount * 3; i++) {
				dgInt32 j;

				j = segment.m_indexes[i];
				segment.m_indexes[i] = indexBuffer[j];
				rootSegment.m_indexes[rootIndexCount] = indexBuffer[j];
				rootIndexCount++;
			}
			rootSegment.m_faceCount = rootIndexCount / 3;

			segment.m_faceOffset = acc;
			//          NEWTON_ASSERT (acc == segment.m_faceOffset);
			acc += segment.m_faceCount;
		}
	}
	LinkNodes();

	dgMatrix matrix(dgGetIdentityMatrix());
	for (dgDebriGraph::dgListNode *node = m_conectivity.GetFirst()->GetNext();
	        node != m_conectivity.GetLast(); node = node->GetNext()) {
		dgInt32 stack;
		dgCollisionConvexIntance *myShape;
		dgNodeBase *pool[DG_COMPOUND_STACK_DEPTH];

		myShape = (dgCollisionConvexIntance *)node->GetInfo().m_nodeData.m_shape;
		pool[0] = m_root;
		stack = 1;

		dgVector p0(myShape->m_treeNode->m_p0);
		dgVector p1(myShape->m_treeNode->m_p1);

		p0 -= dgVector(dgFloat32(1.0e-2f), dgFloat32(1.0e-2f), dgFloat32(1.0e-2f),
		               dgFloat32(0.0f));
		p1 += dgVector(dgFloat32(1.0e-2f), dgFloat32(1.0e-2f), dgFloat32(1.0e-2f),
		               dgFloat32(0.0f));

		while (stack) {
			dgNodeBase *treeNode;
			stack--;
			treeNode = pool[stack];
			if (dgOverlapTest(p0, p1, treeNode->m_p0, treeNode->m_p1)) {
				if (treeNode->m_type == m_leaf) {
					dgCollisionConvexIntance *otherShape;
					otherShape = (dgCollisionConvexIntance *)treeNode->m_shape;
					if (otherShape->m_graphNode != node) {
						dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *adjacentEdge;
						//                  NEWTON_ASSERT (otherShape->m_graphNode != node)
						for (adjacentEdge = node->GetInfo().GetFirst(); adjacentEdge;
						        adjacentEdge = adjacentEdge->GetNext()) {
							if (adjacentEdge->GetInfo().m_node == otherShape->m_graphNode) {
								break;
							}
						}
						if (!adjacentEdge) {
							dgInt32 disjoint;
							dgTriplex normal;
							dgTriplex contactA;
							dgTriplex contactB;
							const dgFloat32 minSeparation = dgFloat32(2.0f) * collisionPadding + dgFloat32(0.05f);
							const dgFloat32 minSeparation2 = minSeparation * minSeparation;

							disjoint = world->ClosestPoint(myShape, matrix, otherShape,
							                               matrix, contactA, contactB, normal, 0);
							if (disjoint) {
								dgVector qA(contactA.m_x, contactA.m_y, contactA.m_z,
								            dgFloat32(0.0f));
								dgVector qB(contactB.m_x, contactB.m_y, contactB.m_z,
								            dgFloat32(0.0f));
								dgVector dist(qB - qA);
								disjoint = (dist % dist) > minSeparation2;
							}

							if (!disjoint) {
								otherShape->m_graphNode->GetInfo().AddEdge(node);
								node->GetInfo().AddEdge(otherShape->m_graphNode);
							}
						}
					}

				} else {
					NEWTON_ASSERT(treeNode->m_type == m_node);
					pool[stack] = treeNode->m_right;
					stack++;
					pool[stack] = treeNode->m_left;
					stack++;
				}
			}
		}
	}

	ResetAnchor();
	// m_conectivity.Trace();
}

dgCollisionCompoundBreakable::dgCollisionCompoundBreakable(
    const dgCollisionCompoundBreakable &source) : dgCollisionCompound(source), m_conectivity(source.m_conectivity), m_detachedIslands(source.m_conectivity.GetAllocator()) {
	dgInt32 stack;
	dgNodeBase *stackPool[DG_COMPOUND_STACK_DEPTH];
	dgTree<dgCompoundBreakableFilterData, dgCollisionConvex *> graphNodeMap(
	    m_allocator);

	m_lru = source.m_lru;
	m_lastIslandColor = source.m_lastIslandColor;

	m_visibilityMapIndexCount = source.m_visibilityMapIndexCount;
	m_visibilityMap = (dgInt8 *)m_allocator->Malloc(
	                      dgInt32(source.m_visibilityMapIndexCount * sizeof(dgInt8)));
	memcpy(m_visibilityMap, source.m_visibilityMap,
	       size_t(source.m_visibilityMapIndexCount * sizeof(dgInt8)));

	m_visibilityInderectMap = (dgInt32 *)m_allocator->Malloc(
	                              source.m_visibilityMapIndexCount * dgInt32(sizeof(dgInt32)));
	memcpy(m_visibilityInderectMap, source.m_visibilityInderectMap,
	       size_t(source.m_visibilityMapIndexCount * dgInt32(sizeof(dgInt32))));

	m_vertexBuffer = source.m_vertexBuffer;
	m_vertexBuffer->AddRef();
	m_collsionId = m_compoundBreakable;
	m_rtti |= dgCollisionCompoundBreakable_RTTI;

	stack = 0;
	dgDebriGraph::dgListNode *myNode = m_conectivity.GetFirst()->GetNext();
	for (dgDebriGraph::dgListNode *node =
	            source.m_conectivity.GetFirst()->GetNext();
	        node != source.m_conectivity.GetLast(); myNode = myNode->GetNext(), node =
	            node->GetNext()) {
		dgCompoundBreakableFilterData info;
		dgDebriNodeInfo &nodeInfo = node->GetInfo().m_nodeData;

		info.m_index = stack;
		info.m_node = myNode;
		graphNodeMap.Insert(info, nodeInfo.m_shape);
		NEWTON_ASSERT(m_array[stack] == nodeInfo.m_shape);
		stack++;
	}

	stack = 1;
	stackPool[0] = m_root;
	while (stack) {
		dgNodeBase *me;

		stack--;
		me = stackPool[stack];

		if (me->m_type == m_leaf) {
			dgInt32 index;
			dgCollisionConvexIntance *newShape;

			dgCompoundBreakableFilterData &info =
			    graphNodeMap.Find(me->m_shape)->GetInfo();
			index = info.m_index;
			NEWTON_ASSERT(m_array[index] == me->m_shape);
			NEWTON_ASSERT(!info.m_node->GetInfo().m_nodeData.m_shape);

			//          newShape = new dgCollisionConvexIntance (((dgCollisionConvexIntance*) me->m_shape)->m_myShape, info.m_node, 0);
			newShape = new (m_allocator) dgCollisionConvexIntance(
			    *((dgCollisionConvexIntance *)me->m_shape), info.m_node);
			info.m_node->GetInfo().m_nodeData.m_shape = newShape;

			me->m_shape->Release();
			me->m_shape = newShape;
			newShape->AddRef();

			m_array[index]->Release();
			m_array[index] = newShape;
			newShape->AddRef();

		} else {
			NEWTON_ASSERT(me->m_type == m_node);
			stackPool[stack] = me->m_left;
			stack++;

			stackPool[stack] = me->m_right;
			stack++;
		}
	}

	LinkNodes();
}

dgCollisionCompoundBreakable::dgCollisionCompoundBreakable(dgWorld *const world,
        dgDeserialize deserialization, void *const userData) : dgCollisionCompound(world, deserialization, userData), m_conectivity(world->GetAllocator(), deserialization, userData), m_detachedIslands(world->GetAllocator()) {
	dgInt32 stack;
	m_collsionId = m_compoundBreakable;
	m_rtti |= dgCollisionCompoundBreakable_RTTI;

	deserialization(userData, &m_lru, dgInt32(sizeof(dgInt32)));
	deserialization(userData, &m_lastIslandColor, dgInt32(sizeof(dgInt32)));
	deserialization(userData, &m_visibilityMapIndexCount,
	                dgInt32(sizeof(dgInt32)));

	m_visibilityMap = (dgInt8 *)m_allocator->Malloc(
	                      dgInt32(m_visibilityMapIndexCount * sizeof(dgInt8)));
	deserialization(userData, m_visibilityMap,
	                m_visibilityMapIndexCount * sizeof(dgInt8));

	m_visibilityInderectMap = (dgInt32 *)m_allocator->Malloc(
	                              m_visibilityMapIndexCount * dgInt32(sizeof(dgInt32)));
	deserialization(userData, m_visibilityInderectMap,
	                size_t(m_visibilityMapIndexCount * dgInt32(sizeof(dgInt32))));

	m_vertexBuffer = new (m_allocator) dgVertexBuffer(m_allocator,
	        deserialization, userData);

	stack = 0;
	for (dgDebriGraph::dgListNode *node = m_conectivity.GetFirst()->GetNext();
	        node != m_conectivity.GetLast(); node = node->GetNext()) {
		dgCollisionConvexIntance *instance;
		instance = (dgCollisionConvexIntance *)m_array[stack];
		node->GetInfo().m_nodeData.m_shape = instance;
		instance->AddRef();
		instance->m_graphNode = node;
		stack++;
	}

	LinkNodes();
}

dgCollisionCompoundBreakable::~dgCollisionCompoundBreakable(void) {
	if (m_visibilityMap) {
		m_allocator->Free(m_visibilityMap);
		m_allocator->Free(m_visibilityInderectMap);
	}

	if (m_vertexBuffer) {
		m_vertexBuffer->Release();
	}
}

void dgCollisionCompoundBreakable::LinkNodes() {
	dgInt32 stack;
	dgNodeBase *pool[DG_COMPOUND_STACK_DEPTH];

#ifdef _DEBUG
	dgInt32 count = 0;
#endif

	pool[0] = m_root;
	stack = 1;
	while (stack) {
		dgNodeBase *node;

		stack--;
		node = pool[stack];
		if (node->m_type == m_leaf) {
			dgCollisionConvexIntance *shape;
			shape = (dgCollisionConvexIntance *)node->m_shape;
			shape->m_treeNode = node;

#ifdef _DEBUG
			shape->m_ordinal = count;
			count++;
#endif
		} else {
			NEWTON_ASSERT(node->m_type == m_node);
			//  if (node->m_type == m_node) {
			pool[stack] = node->m_right;
			stack++;

			pool[stack] = node->m_left;
			stack++;
		}
	}
}

void dgCollisionCompoundBreakable::Serialize(dgSerialize callback,
        void *const userData) const {
	dgInt32 lru;
	dgCollisionCompound::Serialize(callback, userData);

	m_conectivity.Serialize(callback, userData);

	lru = 0;
	callback(userData, &lru, dgInt32(sizeof(dgInt32)));
	callback(userData, &m_lastIslandColor, dgInt32(sizeof(dgInt32)));
	callback(userData, &m_visibilityMapIndexCount, dgInt32(sizeof(dgInt32)));
	callback(userData, m_visibilityMap,
	         m_visibilityMapIndexCount * sizeof(dgInt8));
	callback(userData, m_visibilityInderectMap,
	         size_t(m_visibilityMapIndexCount * dgInt32(sizeof(dgInt32))));

	m_vertexBuffer->Serialize(callback, userData);
}

void dgCollisionCompoundBreakable::GetCollisionInfo(dgCollisionInfo *info) const {
	dgCollisionCompound::GetCollisionInfo(info);
	info->m_collisionType = m_compoundBreakable;
}

dgInt32 dgCollisionCompoundBreakable::GetSegmentIndexStreamShort(
    const dgDebriGraph::dgListNode *const node, dgMesh::dgListNode *subMeshNode,
    dgInt16 *const index) const {
	dgInt32 currentIndex;
	dgSubMesh *const subMesh = &subMeshNode->GetInfo();
	const dgInt32 *const indexes = subMesh->m_indexes;

	currentIndex = 0;
	if (node == m_conectivity.GetLast()) {
		dgInt32 faceCount;

		const dgInt8 *const visbilityMap = m_visibilityMap;
		const dgInt32 *const visibilityInderectMap =
		    &m_visibilityInderectMap[subMesh->m_faceOffset];

		faceCount = subMesh->m_faceCount;
		for (dgInt32 i = 0; i < faceCount; i++) {
			if (visbilityMap[visibilityInderectMap[i]]) {
				index[currentIndex + 0] = dgInt16(indexes[i * 3 + 0]);
				index[currentIndex + 1] = dgInt16(indexes[i * 3 + 1]);
				index[currentIndex + 2] = dgInt16(indexes[i * 3 + 2]);
				currentIndex += 3;
			}
		}

	} else {
		dgInt32 indexCount;
		indexCount = subMesh->m_faceCount * 3;
		for (dgInt32 i = 0; i < indexCount; i++) {
			index[i] = dgInt16(indexes[i]);
		}
		currentIndex = indexCount;
	}

	return currentIndex;
}

dgInt32 dgCollisionCompoundBreakable::GetSegmentIndexStream(
    dgDebriGraph::dgListNode *const node, dgMesh::dgListNode *subMeshNode,
    dgInt32 *const index) const {
	dgInt32 currentIndex;
	dgSubMesh *const subMesh = &subMeshNode->GetInfo();
	const dgInt32 *const indexes = subMesh->m_indexes;

	currentIndex = 0;
	if (node == m_conectivity.GetLast()) {
		dgInt32 faceCount;

		const dgInt8 *const visbilityMap = m_visibilityMap;
		const dgInt32 *const visibilityInderectMap =
		    &m_visibilityInderectMap[subMesh->m_faceOffset];

		faceCount = subMesh->m_faceCount;
		for (dgInt32 i = 0; i < faceCount; i++) {
			if (visbilityMap[visibilityInderectMap[i]]) {
				index[currentIndex + 0] = indexes[i * 3 + 0];
				index[currentIndex + 1] = indexes[i * 3 + 1];
				index[currentIndex + 2] = indexes[i * 3 + 2];
				currentIndex += 3;
			}
		}

	} else {
		dgInt32 indexCount;
		indexCount = subMesh->m_faceCount * 3;
		for (dgInt32 i = 0; i < indexCount; i++) {
			index[i] = indexes[i];
		}
		currentIndex = indexCount;
	}
	return currentIndex;
}

dgInt32 dgCollisionCompoundBreakable::GetSegmentsInRadius(
    const dgVector &origin, dgFloat32 radius,
    dgDebriGraph::dgListNode **segments, dgInt32 maxCount) const {
	dgInt32 count;
	dgInt32 stack;
	dgNodeBase *stackPool[DG_COMPOUND_STACK_DEPTH];

	const dgTriplex &p = *((const dgTriplex *)&origin.m_x);
	dgVector p0(origin - dgVector(radius, radius, radius, dgFloat32(0.0f)));
	dgVector p1(origin + dgVector(radius, radius, radius, dgFloat32(0.0f)));

	count = 0;
	stack = 1;
	stackPool[0] = m_root;

	while (stack) {
		const dgNodeBase *me;

		stack--;
		me = stackPool[stack];
		NEWTON_ASSERT(me);

		if (dgOverlapTest(me->m_p0, me->m_p1, p0, p1)) {

			if (me->m_type == m_leaf) {

				dgCollisionConvexIntance *const shape =
				    (dgCollisionConvexIntance *)me->m_shape;
				dgDebriGraph::dgListNode *const node = shape->m_graphNode;

				if (node->GetInfo().m_nodeData.m_mesh->m_IsVisible) {
					dgTriplex normal;
					dgTriplex contact;
					m_world->ClosestPoint(p, shape, dgGetIdentityMatrix(), contact,
					                      normal, 0);
					dgVector q(contact.m_x, contact.m_y, contact.m_z, dgFloat32(0.0f));
					dgVector dist(q - origin);
					if ((dist % dist) < radius * radius) {
						segments[count] = node;
						count++;
						if (count >= maxCount) {
							break;
						}
					}
				}
			} else {
				NEWTON_ASSERT(me->m_type == m_node);
				stackPool[stack] = me->m_left;
				stack++;
				NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));

				stackPool[stack] = me->m_right;
				stack++;
				NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
			}
		}
	}

	// segments[0] = segments[2];
	// count = 1;

	return count;
}

/*
 dgInt32 dgCollisionCompoundBreakable::GetDetachedPieces (dgCollision** shapes, dgInt32 maxCount)
 {

 }
 */

void dgCollisionCompoundBreakable::EnumerateIslands() {
	//  dgInt32 islandId;

	m_lastIslandColor = 0;
	//  m_island.RemoveAll();
	for (dgDebriGraph::dgListNode *node = m_conectivity.GetFirst();
	        node != m_conectivity.GetLast(); node = node->GetNext()) {
		node->GetInfo().m_nodeData.m_commonData.m_islandIndex = -1;
	}
	for (dgDebriGraph::dgListNode *node = m_conectivity.GetFirst();
	        node != m_conectivity.GetLast(); node = node->GetNext()) {
		dgDebriNodeInfo &data = node->GetInfo().m_nodeData;
		data.m_commonData.m_distanceToFixNode = DG_DYNAMINIC_ISLAND_COST;
		if (data.m_commonData.m_islandIndex == -1) {
			dgInt32 count;
			dgDebriGraph::dgListNode *stack[1024 * 4];

			count = 1;
			stack[0] = node;
			node->GetInfo().m_nodeData.m_commonData.m_islandIndex = m_lastIslandColor;
			while (count) {
				dgDebriGraph::dgListNode *rootNode;

				count--;
				rootNode = stack[count];
				for (dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *edgeNode =
				            rootNode->GetInfo().GetFirst();
				        edgeNode;
				        edgeNode = edgeNode->GetNext()) {
					dgDebriGraph::dgListNode *childNode;
					childNode = edgeNode->GetInfo().m_node;
					if (childNode->GetInfo().m_nodeData.m_commonData.m_islandIndex != m_lastIslandColor) {
						childNode->GetInfo().m_nodeData.m_commonData.m_islandIndex =
						    m_lastIslandColor;
						stack[count] = childNode;
						count++;
					}
				}
			}
			m_lastIslandColor++;
		}
	}

	m_conectivity.GetFirst()->GetInfo().m_nodeData.m_commonData.m_distanceToFixNode =
	    0;
}

void dgCollisionCompoundBreakable::ResetAnchor() {
	dgDebriGraph::dgListNode *fixNode;
	dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *nextEdge;

	fixNode = m_conectivity.GetFirst();
	for (dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *edgeNode =
	            fixNode->GetInfo().GetFirst();
	        edgeNode; edgeNode = nextEdge) {
		nextEdge = edgeNode->GetNext();
		fixNode->GetInfo().DeleteEdge(edgeNode);
	}
	EnumerateIslands();
}

void dgCollisionCompoundBreakable::SetAnchoredParts(dgInt32 count,
        const dgMatrix *const matrixArray, dgCollision * const *collisionArray) {
	dgDebriGraph::dgListNode *fixNode;
	dgMatrix matrix(dgGetIdentityMatrix());

	fixNode = m_conectivity.GetFirst();
	for (dgDebriGraph::dgListNode *node = m_conectivity.GetFirst()->GetNext();
	        node != m_conectivity.GetLast(); node = node->GetNext()) {
		dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *edgeNode;

		for (edgeNode = fixNode->GetInfo().GetFirst(); edgeNode;
		        edgeNode = edgeNode->GetNext()) {
			if (edgeNode->GetInfo().m_node == fixNode) {
				break;
			}
		}
		if (!edgeNode) {
			dgCollision *shape;
			shape = node->GetInfo().m_nodeData.m_shape;
			for (dgInt32 i = 0; i < count; i++) {
				dgInt32 contactCount;
				dgFloat32 penetration[16];
				dgTriplex points[16];
				dgTriplex normals[16];
				contactCount = m_world->Collide(shape, matrix,
				                                (dgCollision *)collisionArray[i], matrixArray[i], points, normals,
				                                penetration, 4, 0);
				if (contactCount) {
					node->GetInfo().AddEdge(fixNode);
					fixNode->GetInfo().AddEdge(node);
					break;
				}
			}
		}
	}
	// m_conectivity.Trace();

	EnumerateIslands();

	if (fixNode->GetInfo().GetCount() > 0) {
		dgInt32 leadingIndex;
		dgInt32 trealingIndex;
		dgDebriGraph::dgListNode *queue[1024 * 4];

		leadingIndex = 1;
		trealingIndex = 0;
		queue[0] = fixNode;
		fixNode->GetInfo().m_nodeData.m_commonData.m_distanceToFixNode = 0;
		while (trealingIndex != leadingIndex) {
			dgInt32 cost;
			dgDebriGraph::dgListNode *rootNode;

			rootNode = queue[trealingIndex];
			trealingIndex++;
			if (trealingIndex >= dgInt32(sizeof(queue) / sizeof(queue[0]))) {
				trealingIndex = 0;
			}

			cost = rootNode->GetInfo().m_nodeData.m_commonData.m_distanceToFixNode + 1;
			for (dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *edgeNode =
			            rootNode->GetInfo().GetFirst();
			        edgeNode; edgeNode =
			            edgeNode->GetNext()) {
				dgDebriGraph::dgListNode *childNode;
				childNode = edgeNode->GetInfo().m_node;
				if (childNode->GetInfo().m_nodeData.m_commonData.m_distanceToFixNode > cost) {
					childNode->GetInfo().m_nodeData.m_commonData.m_distanceToFixNode =
					    cost;
					queue[leadingIndex] = childNode;
					leadingIndex++;
					if (leadingIndex >= dgInt32(sizeof(queue) / sizeof(queue[0]))) {
						leadingIndex = 0;
					}
					NEWTON_ASSERT(leadingIndex != trealingIndex);
				}
			}
		}
	}
}

void dgCollisionCompoundBreakable::DeleteComponent(
    dgDebriGraph::dgListNode *node) {
	dgMesh *mesh;
	dgNodeBase *treeNode;
	dgCollisionConvexIntance *shape;

	mesh = node->GetInfo().m_nodeData.m_mesh;
	for (dgMesh::dgListNode *meshSgement = mesh->GetFirst(); meshSgement;
	        meshSgement = meshSgement->GetNext()) {
		dgSubMesh *const subMesh = &meshSgement->GetInfo();
		if (subMesh->m_visibleFaces) {
			memset(&m_visibilityMap[subMesh->m_faceOffset], 0,
			       size_t(subMesh->m_faceCount));
		}
	}

	for (dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *edgeNode =
	            node->GetInfo().GetFirst();
	        edgeNode; edgeNode = edgeNode->GetNext()) {
		dgDebriGraph::dgListNode *adjecentNode;
		adjecentNode = edgeNode->GetInfo().m_node;

		mesh = adjecentNode->GetInfo().m_nodeData.m_mesh;
		if (mesh) {
			mesh->m_IsVisible = 1;
			for (dgMesh::dgListNode *meshSgement = mesh->GetFirst(); meshSgement;
			        meshSgement = meshSgement->GetNext()) {
				dgSubMesh *const subMesh = &meshSgement->GetInfo();
				if (!subMesh->m_visibleFaces) {
					subMesh->m_visibleFaces = 1;
					memset(&m_visibilityMap[subMesh->m_faceOffset], 1,
					       size_t(subMesh->m_faceCount));
				}
			}
		}
	}

	for (dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *edgeNode =
	            node->GetInfo().GetFirst();
	        edgeNode; edgeNode = edgeNode->GetNext()) {
		dgDebriGraph::dgListNode *neighborg;
		neighborg = edgeNode->GetInfo().m_node;
		if (neighborg->GetInfo().m_nodeData.m_commonData.m_lru < m_lru) {
			neighborg->GetInfo().m_nodeData.m_commonData.m_lru = m_lru;
			m_detachedIslands.Append(neighborg);
		}
	}

	shape = (dgCollisionConvexIntance *)node->GetInfo().m_nodeData.m_shape;
	NEWTON_ASSERT(
	    shape->m_graphNode->GetInfo().GetFirst() == node->GetInfo().GetFirst());
	m_conectivity.DeleteNode(node);

	for (dgIsland::dgListNode *islandNode = m_detachedIslands.GetFirst();
	        islandNode; islandNode = islandNode->GetNext()) {
		if (node == islandNode->GetInfo()) {
			m_detachedIslands.Remove(islandNode);
			break;
		}
	}

	treeNode = shape->m_treeNode;
	((dgCollisionConvexIntance *)m_array[m_count - 1])->m_treeNode->m_id =
	    treeNode->m_id;
	RemoveCollision(treeNode);
}

dgBody *dgCollisionCompoundBreakable::CreateComponentBody(
    dgDebriGraph::dgListNode *node) const {
	NEWTON_ASSERT(0);
	return NULL;
	/*
	 dgFloat32 Ixx;
	 dgFloat32 Iyy;
	 dgFloat32 Izz;
	 dgFloat32 Ixx1;
	 dgFloat32 Iyy1;
	 dgFloat32 Izz1;
	 dgFloat32 mass;
	 dgBody* body;
	 dgCollisionConvexIntance* shape;

	 shape = (dgCollisionConvexIntance*) node->GetInfo().m_nodeData.m_shape;
	 NEWTON_ASSERT (shape->m_graphNode->GetInfo().GetFirst() == node->GetInfo().GetFirst());


	 body = m_world->CreateBody(shape->m_myShape);
	 body->SetCentreOfMass(shape->m_volume);

	 mass = shape->m_inertia.m_w;
	 Ixx = shape->m_inertia.m_x;
	 Iyy = shape->m_inertia.m_y;
	 Izz = shape->m_inertia.m_z;

	 NEWTON_ASSERT (0);
	 mass = 1.0f;
	 Ixx = shape->m_inertia.m_x/shape->m_inertia.m_w;
	 Iyy = shape->m_inertia.m_y/shape->m_inertia.m_w;
	 Izz = shape->m_inertia.m_z/shape->m_inertia.m_w;


	 Ixx1 = ClampValue (Ixx, dgFloat32 (0.001f) * mass, dgFloat32 (100.0f) * mass);
	 Iyy1 = ClampValue (Iyy, dgFloat32 (0.001f) * mass, dgFloat32 (100.0f) * mass);
	 Izz1 = ClampValue (Izz, dgFloat32 (0.001f) * mass, dgFloat32 (100.0f) * mass);
	 if (mass < dgFloat32 (1.0e-3f)) {
	 mass = DG_INFINITE_MASS * dgFloat32 (1.5f);
	 }

	 body->SetMassMatrix (mass, Ixx1, Iyy1, Izz1);
	 body->SetAparentMassMatrix (dgVector (Ixx, Iyy, Izz, mass));

	 return body;
	 */
}

void dgCollisionCompoundBreakable::DeleteComponentBegin() {
	m_lru++;
	m_detachedIslands.RemoveAll();

	// m_conectivity.Trace();
}

void dgCollisionCompoundBreakable::DeleteComponentEnd() {
	dgInt32 baseLru;
	dgIsland::dgListNode *nextIslandNode;

	int xxxx = 0;
	dgDebriGraph::dgListNode *xxx[1024 * 8];

	m_lru++;
	baseLru = m_lru;
	for (dgIsland::dgListNode *islandNode = m_detachedIslands.GetFirst();
	        islandNode; islandNode = nextIslandNode) {
		dgDebriGraph::dgListNode *node;

		nextIslandNode = islandNode->GetNext();
		node = islandNode->GetInfo();

		m_lru++;
		if (node->GetInfo().m_nodeData.m_commonData.m_lru > baseLru) {
			m_detachedIslands.Remove(node);
		} else {
			dgInt32 count;
			dgInt32 piecesCount;
			dgDebriGraph::dgListNode *stack[1024 * 4];
			dgDebriGraph::dgListNode *newIslandPieces[1024 * 8];

			count = 1;
			stack[0] = node;
			piecesCount = 0;

			node->GetInfo().m_nodeData.m_commonData.m_lru = m_lru;

			while (count) {
				dgInt32 stackOrigin;
				dgDebriGraph::dgListNode *rootNode;

				count--;
				rootNode = stack[count];
				NEWTON_ASSERT(node->GetInfo().m_nodeData.m_commonData.m_lru == m_lru);

				newIslandPieces[piecesCount] = rootNode;
				piecesCount++;
				if (rootNode->GetInfo().m_nodeData.m_commonData.m_distanceToFixNode == 0) {
					piecesCount = 0;
					m_detachedIslands.Remove(node);
					break;
				}

				stackOrigin = count;
				for (dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode *edgeNode =
				            rootNode->GetInfo().GetFirst();
				        edgeNode;
				        edgeNode = edgeNode->GetNext()) {
					dgInt32 cost;
					dgInt32 index;
					dgDebriGraph::dgListNode *childNode;

					childNode = edgeNode->GetInfo().m_node;
					dgDebriNodeInfo &data = childNode->GetInfo().m_nodeData;

					if (data.m_commonData.m_lru < baseLru) {
						data.m_commonData.m_lru = m_lru;
						cost = data.m_commonData.m_distanceToFixNode;
						for (index = count;
						        (index > stackOrigin) && (stack[index - 1]->GetInfo().m_nodeData.m_commonData.m_distanceToFixNode < cost); index--) {
							stack[index] = stack[index - 1];
						}
						stack[index] = childNode;
						count++;
					} else if (data.m_commonData.m_lru < m_lru) {
						count = 0;
						piecesCount = 0;
						m_detachedIslands.Remove(node);
						break;
					}
				}
			}

			if (piecesCount) {
				for (dgInt32 i = 0; i < piecesCount; i++) {
					newIslandPieces[i]->GetInfo().m_nodeData.m_commonData.m_islandIndex =
					    m_lastIslandColor;

					xxx[xxxx] = newIslandPieces[i];
					xxxx++;
#ifdef _DEBUG
					for (dgInt32 j = i + 1; j < piecesCount; j++) {
						NEWTON_ASSERT(newIslandPieces[i] != newIslandPieces[j]);
					}
#endif
				}
				m_lastIslandColor++;
				piecesCount = 0;
			}
		}
	}

	for (int i = 0; i < xxxx; i++) {
		DeleteComponent(xxx[i]);
	}

	/*
	 m_conectivity.GetFirst()->GetInfo().Trace();
	 dgInt32 count;
	 dgDebriGraph::dgListNode* stack[1024 * 4];
	 count = 1;
	 stack[0] = m_conectivity.GetFirst();
	 m_conectivity.GetFirst()->GetInfo().m_nodeData.m_commonData.m_lru = m_lru;
	 while (count) {
	 dgDebriGraph::dgListNode* rootNode;

	 count --;
	 rootNode = stack[count];
	 for (dgGraphNode<dgDebriNodeInfo, dgSharedNodeMesh>::dgListNode* edgeNode = rootNode->GetInfo().GetFirst(); edgeNode; edgeNode = edgeNode->GetNext()) {
	 dgDebriGraph::dgListNode* childNode;

	 childNode = edgeNode->GetInfo().m_node;
	 dgDebriNodeInfo& data = childNode->GetInfo().m_nodeData;

	 if (data.m_commonData.m_lru != m_lru) {
	 data.m_commonData.m_lru = m_lru;
	 stack[count] = childNode;
	 count ++;
	 }
	 }
	 }

	 for (dgDebriGraph::dgListNode* node = m_conectivity.GetFirst(); node != m_conectivity.GetLast(); node = node->GetNext()) {
	 // NEWTON_ASSERT (node->GetInfo().m_nodeData.m_commonData.m_lru == m_lru);
	 }
	 */
}