dgAABBPointTree4d* dgConvexHull4d::BuildTree (dgAABBPointTree4d* const parent, dgHullVector* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** memoryPool, dgInt32& maxMemSize) const
{
dgAABBPointTree4d* tree = NULL;
dgAssert (count);
dgBigVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f));
dgBigVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f));
if (count <= DG_VERTEX_CLUMP_SIZE_4D) {
dgAABBPointTree4dClump* const clump = new (*memoryPool) dgAABBPointTree4dClump;
*memoryPool += sizeof (dgAABBPointTree4dClump);
maxMemSize -= sizeof (dgAABBPointTree4dClump);
dgAssert (maxMemSize >= 0);
dgAssert (clump);
clump->m_count = count;
for (dgInt32 i = 0; i < count; i ++) {
clump->m_indices[i] = i + baseIndex;
const dgBigVector& p = points[i];
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
minP.m_w = dgMin (p.m_w, minP.m_w);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
maxP.m_w = dgMax (p.m_w, maxP.m_w);
}
clump->m_left = NULL;
clump->m_right = NULL;
tree = clump;
} else {
dgBigVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgBigVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < count; i ++) {
const dgBigVector& p = points[i];
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
minP.m_w = dgMin (p.m_w, minP.m_w);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
maxP.m_w = dgMax (p.m_w, maxP.m_w);
median = median + p;
varian = varian + p.CompProduct4(p);
}
varian = varian.Scale4 (dgFloat32 (count)) - median.CompProduct4(median);
dgInt32 index = 0;
dgFloat64 maxVarian = dgFloat64 (-1.0e10f);
for (dgInt32 i = 0; i < 4; i ++) {
if (varian[i] > maxVarian) {
index = i;
maxVarian = varian[i];
}
}
dgBigVector center = median.Scale4 (dgFloat64 (1.0f) / dgFloat64 (count));
dgFloat64 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = count - 1;
do {
for (; i0 <= i1; i0 ++) {
dgFloat64 val = points[i0][index];
if (val > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
dgFloat64 val = points[i1][index];
if (val < test) {
break;
}
}
if (i0 < i1) {
dgSwap(points[i0], points[i1]);
i0++;
i1--;
}
} while (i0 <= i1);
if (i0 == 0){
i0 = count / 2;
}
if (i0 >= (count - 1)){
i0 = count / 2;
}
tree = new (*memoryPool) dgAABBPointTree4d;
*memoryPool += sizeof (dgAABBPointTree4d);
maxMemSize -= sizeof (dgAABBPointTree4d);
dgAssert (maxMemSize >= 0);
dgAssert (i0);
dgAssert (count - i0);
tree->m_left = BuildTree (tree, points, i0, baseIndex, memoryPool, maxMemSize);
tree->m_right = BuildTree (tree, &points[i0], count - i0, i0 + baseIndex, memoryPool, maxMemSize);
}
dgAssert (tree);
tree->m_parent = parent;
tree->m_box[0] = minP - dgBigVector (dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f));
tree->m_box[1] = maxP + dgBigVector (dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f));
return tree;
}
bool dgCollisionConvexHull::Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance)
{
dgInt32 stride = strideInBytes / sizeof (dgFloat32);
dgStack<dgFloat64> buffer(3 * 2 * count);
for (dgInt32 i = 0; i < count; i ++) {
buffer[i * 3 + 0] = vertexArray[i * stride + 0];
buffer[i * 3 + 1] = vertexArray[i * stride + 1];
buffer[i * 3 + 2] = vertexArray[i * stride + 2];
}
dgConvexHull3d* convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
if (!convexHull->GetCount()) {
// this is a degenerated hull hull to add some thickness and for a thick plane
delete convexHull;
dgStack<dgVector> tmp(3 * count);
for (dgInt32 i = 0; i < count; i ++) {
tmp[i][0] = dgFloat32 (buffer[i*3 + 0]);
tmp[i][1] = dgFloat32 (buffer[i*3 + 1]);
tmp[i][2] = dgFloat32 (buffer[i*3 + 2]);
tmp[i][2] = dgFloat32 (0.0f);
}
dgObb sphere;
sphere.SetDimensions (&tmp[0][0], sizeof (dgVector), count);
dgInt32 index = 0;
dgFloat32 size = dgFloat32 (1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (sphere.m_size[i] < size) {
index = i;
size = sphere.m_size[i];
}
}
dgVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
normal[index] = dgFloat32 (1.0f);
dgVector step = sphere.RotateVector (normal.Scale3 (dgFloat32 (0.05f)));
for (dgInt32 i = 0; i < count; i ++) {
dgVector p1 (tmp[i] + step);
dgVector p2 (tmp[i] - step);
buffer[i * 3 + 0] = p1.m_x;
buffer[i * 3 + 1] = p1.m_y;
buffer[i * 3 + 2] = p1.m_z;
buffer[(i + count) * 3 + 0] = p2.m_x;
buffer[(i + count) * 3 + 1] = p2.m_y;
buffer[(i + count) * 3 + 2] = p2.m_z;
}
count *= 2;
convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
if (!convexHull->GetCount()) {
delete convexHull;
return false;
}
}
// check for degenerated faces
for (bool success = false; !success; ) {
success = true;
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgStack<dgInt8> mask(convexHull->GetVertexCount());
memset (&mask[0], 1, mask.GetSizeInBytes());
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
const dgBigVector& p0 = hullVertexArray[face.m_index[0]];
const dgBigVector& p1 = hullVertexArray[face.m_index[1]];
const dgBigVector& p2 = hullVertexArray[face.m_index[2]];
dgBigVector p1p0 (p1 - p0);
dgBigVector p2p0 (p2 - p0);
dgBigVector normal (p2p0 * p1p0);
dgFloat64 mag2 = normal % normal;
if (mag2 < dgFloat64 (1.0e-6f * 1.0e-6f)) {
success = false;
dgInt32 index = -1;
dgBigVector p2p1 (p2 - p1);
dgFloat64 dist10 = p1p0 % p1p0;
dgFloat64 dist20 = p2p0 % p2p0;
dgFloat64 dist21 = p2p1 % p2p1;
if ((dist10 >= dist20) && (dist10 >= dist21)) {
index = 2;
} else if ((dist20 >= dist10) && (dist20 >= dist21)) {
index = 1;
} else if ((dist21 >= dist10) && (dist21 >= dist20)) {
index = 0;
}
dgAssert (index != -1);
mask[face.m_index[index]] = 0;
}
}
if (!success) {
dgInt32 count = 0;
dgInt32 vertexCount = convexHull->GetVertexCount();
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (mask[i]) {
buffer[count * 3 + 0] = hullVertexArray[i].m_x;
buffer[count * 3 + 1] = hullVertexArray[i].m_y;
buffer[count * 3 + 2] = hullVertexArray[i].m_z;
count ++;
}
}
delete convexHull;
convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
}
}
dgAssert (convexHull);
dgInt32 vertexCount = convexHull->GetVertexCount();
if (vertexCount < 4) {
delete convexHull;
return false;
}
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgPolyhedra polyhedra (GetAllocator());
polyhedra.BeginFace();
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
polyhedra.AddFace (face.m_index[0], face.m_index[1], face.m_index[2]);
}
polyhedra.EndFace();
if (vertexCount > 4) {
// bool edgeRemoved = false;
// while (RemoveCoplanarEdge (polyhedra, hullVertexArray)) {
// edgeRemoved = true;
// }
// if (edgeRemoved) {
// if (!CheckConvex (polyhedra, hullVertexArray)) {
// delete convexHull;
// return false;
// }
// }
while (RemoveCoplanarEdge (polyhedra, hullVertexArray));
}
dgStack<dgInt32> vertexMap(vertexCount);
memset (&vertexMap[0], -1, vertexCount * sizeof (dgInt32));
dgInt32 mark = polyhedra.IncLRU();
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
if (vertexMap[edge->m_incidentVertex] == -1) {
vertexMap[edge->m_incidentVertex] = m_vertexCount;
m_vertexCount ++;
}
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr->m_userData = m_edgeCount;
m_edgeCount ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector)));
m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge)));
m_vertexToEdgeMapping = (const dgConvexSimplexEdge**) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgConvexSimplexEdge*)));
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (vertexMap[i] != -1) {
m_vertex[vertexMap[i]] = hullVertexArray[i];
m_vertex[vertexMap[i]].m_w = dgFloat32 (0.0f);
}
}
delete convexHull;
vertexCount = m_vertexCount;
mark = polyhedra.IncLRU();;
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
dgEdge *ptr = edge;
do {
ptr->m_mark = mark;
dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
simplexPtr->m_vertex = vertexMap[ptr->m_incidentVertex];
simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
m_faceCount = 0;
dgStack<char> faceMarks (m_edgeCount);
memset (&faceMarks[0], 0, m_edgeCount * sizeof (dgInt8));
dgStack<dgConvexSimplexEdge*> faceArray (m_edgeCount);
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const face = &m_simplex[i];
if (!faceMarks[i]) {
dgConvexSimplexEdge* ptr = face;
do {
dgAssert ((ptr - m_simplex) >= 0);
faceMarks[dgInt32 (ptr - m_simplex)] = '1';
ptr = ptr->m_next;
} while (ptr != face);
faceArray[m_faceCount] = face;
m_faceCount ++;
}
}
m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *)));
memcpy (m_faceArray, &faceArray[0], m_faceCount * sizeof(dgConvexSimplexEdge *));
if (vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) {
// create a face structure for support vertex
dgStack<dgConvexBox> boxTree (vertexCount);
dgTree<dgVector,dgInt32> sortTree(GetAllocator());
dgStack<dgTree<dgVector,dgInt32>::dgTreeNode*> vertexNodeList(vertexCount);
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < vertexCount; i ++) {
const dgVector& p = m_vertex[i];
vertexNodeList[i] = sortTree.Insert (p, i);
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
boxTree[0].m_box[0] = minP;
boxTree[0].m_box[1] = maxP;
boxTree[0].m_leftBox = -1;
boxTree[0].m_rightBox = -1;
boxTree[0].m_vertexStart = 0;
boxTree[0].m_vertexCount = vertexCount;
dgInt32 boxCount = 1;
dgInt32 stack = 1;
dgInt32 stackBoxPool[64];
stackBoxPool[0] = 0;
while (stack) {
stack --;
dgInt32 boxIndex = stackBoxPool[stack];
dgConvexBox& box = boxTree[boxIndex];
if (box.m_vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) {
dgVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < box.m_vertexCount; i ++) {
dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
median += p;
varian += p.CompProduct3 (p);
}
varian = varian.Scale3 (dgFloat32 (box.m_vertexCount)) - median.CompProduct3(median);
dgInt32 index = 0;
dgFloat64 maxVarian = dgFloat64 (-1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (varian[i] > maxVarian) {
index = i;
maxVarian = varian[i];
}
}
dgVector center = median.Scale3 (dgFloat32 (1.0f) / dgFloat32 (box.m_vertexCount));
dgFloat32 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = box.m_vertexCount - 1;
do {
for (; i0 <= i1; i0 ++) {
dgFloat32 val = vertexNodeList[box.m_vertexStart + i0]->GetInfo()[index];
if (val > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
dgFloat32 val = vertexNodeList[box.m_vertexStart + i1]->GetInfo()[index];
if (val < test) {
break;
}
}
if (i0 < i1) {
dgSwap(vertexNodeList[box.m_vertexStart + i0], vertexNodeList[box.m_vertexStart + i1]);
i0++;
i1--;
}
} while (i0 <= i1);
if (i0 == 0){
i0 = box.m_vertexCount / 2;
}
if (i0 >= (box.m_vertexCount - 1)){
i0 = box.m_vertexCount / 2;
}
{
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = i0; i < box.m_vertexCount; i ++) {
const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
box.m_rightBox = boxCount;
boxTree[boxCount].m_box[0] = minP;
boxTree[boxCount].m_box[1] = maxP;
boxTree[boxCount].m_leftBox = -1;
boxTree[boxCount].m_rightBox = -1;
boxTree[boxCount].m_vertexStart = box.m_vertexStart + i0;
boxTree[boxCount].m_vertexCount = box.m_vertexCount - i0;
stackBoxPool[stack] = boxCount;
stack ++;
boxCount ++;
}
{
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < i0; i ++) {
const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
box.m_leftBox = boxCount;
boxTree[boxCount].m_box[0] = minP;
boxTree[boxCount].m_box[1] = maxP;
boxTree[boxCount].m_leftBox = -1;
boxTree[boxCount].m_rightBox = -1;
boxTree[boxCount].m_vertexStart = box.m_vertexStart;
boxTree[boxCount].m_vertexCount = i0;
stackBoxPool[stack] = boxCount;
stack ++;
boxCount ++;
}
}
}
for (dgInt32 i = 0; i < m_vertexCount; i ++) {
m_vertex[i] = vertexNodeList[i]->GetInfo();
vertexNodeList[i]->GetInfo().m_w = dgFloat32 (i);
}
m_supportTreeCount = boxCount;
m_supportTree = (dgConvexBox*) m_allocator->Malloc(dgInt32 (boxCount * sizeof(dgConvexBox)));
memcpy (m_supportTree, &boxTree[0], boxCount * sizeof(dgConvexBox));
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const ptr = &m_simplex[i];
dgTree<dgVector,dgInt32>::dgTreeNode* const node = sortTree.Find(ptr->m_vertex);
dgInt32 index = dgInt32 (node->GetInfo().m_w);
ptr->m_vertex = dgInt16 (index);
}
}
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const edge = &m_simplex[i];
m_vertexToEdgeMapping[edge->m_vertex] = edge;
}
SetVolumeAndCG ();
return true;
}
dgCollisionDeformableMesh::dgDeformableNode* dgCollisionDeformableMesh::BuildTopDown (dgInt32 count, dgDeformableNode* const children, dgDeformableNode* const parent)
{
dgDeformableNode* root = NULL;
if (count == 1) {
root = children;
root->m_left = NULL;
root->m_right = NULL;
root->m_parent = parent;
} else if (count == 2) {
root = &m_nodesMemory[m_nodesCount];
m_nodesCount ++;
root->m_indexStart = -1;
root->m_parent = parent;
root->m_left = BuildTopDown (1, children, root);
root->m_right = BuildTopDown (1, &children[1], root);
root->m_surfaceArea = CalculateSurfaceArea (root->m_left, root->m_right, root->m_minBox, root->m_maxBox);
} else {
dgVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < count; i ++) {
const dgDeformableNode* const node = &children[i];
dgVector p ((node->m_minBox + node->m_maxBox).Scale3 (0.5f));
median += p;
varian += p.CompProduct3 (p);
}
varian = varian.Scale3 (dgFloat32 (count)) - median.CompProduct3(median);
dgInt32 index = 0;
dgFloat32 maxVarian = dgFloat32 (-1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (varian[i] > maxVarian) {
index = i;
maxVarian = varian[i];
}
}
dgVector center = median.Scale3 (dgFloat32 (1.0f) / dgFloat32 (count));
dgFloat32 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = count - 1;
do {
for (; i0 <= i1; i0 ++) {
const dgDeformableNode* const node = &children[i0];
dgFloat32 val = (node->m_minBox[index] + node->m_maxBox[index]) * dgFloat32 (0.5f);
if (val > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
const dgDeformableNode* const node = &children[i1];
dgFloat32 val = (node->m_minBox[index] + node->m_maxBox[index]) * dgFloat32 (0.5f);
if (val < test) {
break;
}
}
if (i0 < i1) {
dgSwap(children[i0], children[i1]);
i0++;
i1--;
}
} while (i0 <= i1);
if (i0 > 0){
i0 --;
}
if ((i0 + 1) >= count) {
i0 = count - 2;
}
dgInt32 spliteCount = i0 + 1;
root = &m_nodesMemory[m_nodesCount];
m_nodesCount ++;
root->m_indexStart = -1;
root->m_parent = parent;
root->m_left = BuildTopDown (spliteCount, children, root);
root->m_right = BuildTopDown (count - spliteCount, &children[spliteCount], root);
root->m_surfaceArea = CalculateSurfaceArea (root->m_left, root->m_right, root->m_minBox, root->m_maxBox);
}
return root;
}
void dgPolygonSoupDatabaseBuilder::Optimize(bool optimize)
{
#define DG_PATITION_SIZE (1024 * 4)
if (optimize && (m_faceCount > DG_PATITION_SIZE)) {
dgBigVector median (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
dgBigVector varian (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
dgStack<dgVector> pool (1024 * 2);
dgStack<hacd::HaI32> indexArray (1024 * 2);
hacd::HaI32 polygonIndex = 0;
for (hacd::HaI32 i = 0; i < m_faceCount; i ++) {
dgBigVector p0 (hacd::HaF32 ( 1.0e10f), hacd::HaF32 ( 1.0e10f), hacd::HaF32 ( 1.0e10f), hacd::HaF32 (0.0f));
dgBigVector p1 (hacd::HaF32 (-1.0e10f), hacd::HaF32 (-1.0e10f), hacd::HaF32 (-1.0e10f), hacd::HaF32 (0.0f));
hacd::HaI32 count = m_faceVertexCount[i];
for (hacd::HaI32 j = 1; j < count; j ++) {
hacd::HaI32 k = m_vertexIndex[polygonIndex + j];
p0.m_x = GetMin (p0.m_x, hacd::HaF64 (m_vertexPoints[k].m_x));
p0.m_y = GetMin (p0.m_y, hacd::HaF64 (m_vertexPoints[k].m_y));
p0.m_z = GetMin (p0.m_z, hacd::HaF64 (m_vertexPoints[k].m_z));
p1.m_x = GetMax (p1.m_x, hacd::HaF64 (m_vertexPoints[k].m_x));
p1.m_y = GetMax (p1.m_y, hacd::HaF64 (m_vertexPoints[k].m_y));
p1.m_z = GetMax (p1.m_z, hacd::HaF64 (m_vertexPoints[k].m_z));
}
dgBigVector p ((p0 + p1).Scale (0.5f));
median += p;
varian += p.CompProduct (p);
polygonIndex += count;
}
varian = varian.Scale (hacd::HaF32 (m_faceCount)) - median.CompProduct(median);
hacd::HaI32 axis = 0;
hacd::HaF32 maxVarian = hacd::HaF32 (-1.0e10f);
for (hacd::HaI32 i = 0; i < 3; i ++) {
if (varian[i] > maxVarian) {
axis = i;
maxVarian = hacd::HaF32 (varian[i]);
}
}
dgBigVector center = median.Scale (hacd::HaF32 (1.0f) / hacd::HaF32 (m_faceCount));
hacd::HaF64 axisVal = center[axis];
dgPolygonSoupDatabaseBuilder left;
dgPolygonSoupDatabaseBuilder right;
left.Begin();
right.Begin();
polygonIndex = 0;
for (hacd::HaI32 i = 0; i < m_faceCount; i ++) {
hacd::HaI32 side = 0;
hacd::HaI32 count = m_faceVertexCount[i];
for (hacd::HaI32 j = 1; j < count; j ++) {
hacd::HaI32 k;
k = m_vertexIndex[polygonIndex + j];
dgVector p (&m_vertexPoints[k].m_x);
if (p[axis] > axisVal) {
side = 1;
break;
}
}
hacd::HaI32 faceArray = count - 1;
hacd::HaI32 faceTagsData = m_vertexIndex[polygonIndex];
for (hacd::HaI32 j = 1; j < count; j ++) {
hacd::HaI32 k = m_vertexIndex[polygonIndex + j];
pool[j - 1] = m_vertexPoints[k];
indexArray[j - 1] = j - 1;
}
if (!side) {
left.AddMesh (&pool[0].m_x, count - 1, sizeof (dgVector), 1, &faceArray, &indexArray[0], &faceTagsData, dgGetIdentityMatrix());
} else {
right.AddMesh (&pool[0].m_x, count - 1, sizeof (dgVector), 1, &faceArray, &indexArray[0], &faceTagsData, dgGetIdentityMatrix());
}
polygonIndex += count;
}
left.Optimize(optimize);
right.Optimize(optimize);
m_faceCount = 0;
m_indexCount = 0;
m_vertexCount = 0;
m_normalCount = 0;
polygonIndex = 0;
for (hacd::HaI32 i = 0; i < left.m_faceCount; i ++) {
hacd::HaI32 count = left.m_faceVertexCount[i];
hacd::HaI32 faceArray = count - 1;
hacd::HaI32 faceTagsData = left.m_vertexIndex[polygonIndex];
for (hacd::HaI32 j = 1; j < count; j ++) {
hacd::HaI32 k = left.m_vertexIndex[polygonIndex + j];
pool[j - 1] = left.m_vertexPoints[k];
indexArray[j - 1] = j - 1;
}
AddMesh (&pool[0].m_x, count - 1, sizeof (dgVector), 1, &faceArray, &indexArray[0], &faceTagsData, dgGetIdentityMatrix());
polygonIndex += count;
}
polygonIndex = 0;
for (hacd::HaI32 i = 0; i < right.m_faceCount; i ++) {
hacd::HaI32 count = right.m_faceVertexCount[i];
hacd::HaI32 faceArray = count - 1;
hacd::HaI32 faceTagsData = right.m_vertexIndex[polygonIndex];
for (hacd::HaI32 j = 1; j < count; j ++) {
hacd::HaI32 k = right.m_vertexIndex[polygonIndex + j];
pool[j - 1] = right.m_vertexPoints[k];
indexArray[j - 1] = j - 1;
}
AddMesh (&pool[0].m_x, count - 1, sizeof (dgVector), 1, &faceArray, &indexArray[0], &faceTagsData, dgGetIdentityMatrix());
polygonIndex += count;
}
if (m_faceCount < DG_PATITION_SIZE) {
EndAndOptimize(optimize);
} else {
EndAndOptimize(false);
}
} else {
EndAndOptimize(optimize);
}
}
void dgCollisionDeformableSolidMesh::CreateClusters (dgInt32 count, dgFloat32 overlapingWidth)
{
if (m_clusterPosit) {
dgFree (m_clusterAqqInv);
dgFree (m_clusterRotationInitialGuess);
dgFree (m_clusterCom0);
dgFree (m_clusterMass);
dgFree (m_clusterWeight);
dgFree (m_clusterPosit);
}
if (count <= 1) {
// special case of only one region
m_clustersCount = 1;
m_clusterAqqInv = (dgMatrix*) dgMallocStack (sizeof (dgMatrix) * m_clustersCount);
m_clusterRotationInitialGuess = (dgMatrix*) dgMallocStack (sizeof (dgMatrix) * m_clustersCount);
m_clusterCom0 = (dgVector*) dgMallocStack (sizeof (dgVector) * m_clustersCount);
m_clusterMass = (dgFloat32*) dgMallocStack (sizeof (dgFloat32) * m_clustersCount);
m_clusterWeight = (dgFloat32*) dgMallocStack (sizeof (dgFloat32) * (m_particles.m_count));
m_clusterPosit = (dgInt32*) dgMallocStack (sizeof (dgInt32) * (m_particles.m_count));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
m_clusterPositStart[i] = i;
m_clusterPosit[i] = 0;
m_clusterWeight[i] = dgFloat32 (1.0f);
}
m_clusterPositStart[m_particles.m_count] = m_particles.m_count;
} else {
dgClusterBuilder clusterList (GetAllocator());
dgCluster& cluster = clusterList.Append()->GetInfo();
cluster.m_count = m_particles.m_count;
cluster.m_points = ((dgInt32*) dgMallocStack (sizeof (dgUnsigned32) * cluster.m_count));
for (dgInt32 i = 0; i < cluster.m_count; i ++) {
cluster.m_points[i] = i;
}
dgClusterBuilder::dgListNode* nextNode = NULL;
for (dgClusterBuilder::dgListNode* node = clusterList.GetFirst(); node && (clusterList.GetCount() < count); node = nextNode) {
dgCluster& cluster = node->GetInfo();
dgVector median (dgFloat32 (0.0f));
dgVector varian (dgFloat32 (0.0f));
for (dgInt32 i = 0; i < cluster.m_count; i ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i]];
median += p;
varian += p.CompProduct4(p);
}
varian = varian.Scale4 (dgFloat32 (cluster.m_count)) - median.CompProduct4(median);
dgInt32 index = 0;
dgFloat32 maxVarian = dgFloat32 (-1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (varian[i] > maxVarian) {
index = i;
maxVarian = varian[i];
}
}
dgVector center = median.Scale4 (dgFloat32 (1.0f) / dgFloat32 (cluster.m_count));
dgFloat32 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = cluster.m_count - 1;
do {
for (; i0 <= i1; i0 ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i0]];
if (p[index] > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
const dgVector& p = m_shapePosit[cluster.m_points[i1]];
if (p[index] < test) {
break;
}
}
if (i0 < i1) {
dgSwap(cluster.m_points[i0], cluster.m_points[i1]);
i0++;
i1--;
}
} while (i0 <= i1);
dgInt32 middle = i0 + 1;
dgInt32 leftSideOvelap = 0;
dgFloat32 leftBarrier = test + overlapingWidth;
for (dgInt32 i = middle; i < cluster.m_count; i ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i]];
leftSideOvelap += (p[index] < leftBarrier) ? 1 : 0;
}
dgInt32 rightSideOvelap = 0;
dgFloat32 rightBarrier = test - overlapingWidth;
for (dgInt32 i = 0; i < middle; i ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i]];
rightSideOvelap += (p[index] > rightBarrier) ? 1 : 0;
}
if (rightSideOvelap || leftSideOvelap) {
dgCluster& leftCluster = clusterList.Append()->GetInfo();
leftCluster.m_count = middle + leftSideOvelap;
leftCluster.m_points = ((dgInt32*) dgMallocStack (sizeof (dgUnsigned32) * leftCluster.m_count));
dgInt32 j = 0;
for (dgInt32 i = 0; i < middle; i ++) {
leftCluster.m_points[j] = cluster.m_points[i];
j ++;
}
for (dgInt32 i = middle; i < cluster.m_count; i ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i]];
if (p[index] < leftBarrier) {
leftCluster.m_points[j] = cluster.m_points[i];
j ++;
dgAssert (j <= leftCluster.m_count);
}
}
j = 0;
dgCluster& rightCluster = clusterList.Append()->GetInfo();
rightCluster.m_count = cluster.m_count - middle + rightSideOvelap;
rightCluster.m_points = ((dgInt32*) dgMallocStack (sizeof (dgUnsigned32) * rightCluster.m_count));
for (dgInt32 i = middle; i < cluster.m_count; i ++) {
rightCluster.m_points[j] = cluster.m_points[i];
j ++;
}
for (dgInt32 i = 0; i < middle; i ++) {
const dgVector& p = m_shapePosit[cluster.m_points[i]];
if (p[index] > rightBarrier) {
rightCluster.m_points[j] = cluster.m_points[i];
j ++;
dgAssert (j <= rightCluster.m_count);
}
}
nextNode = node->GetNext();
clusterList.Remove(node);
} else {
dgAssert(0);
}
}
m_clustersCount = clusterList.GetCount();
m_clusterAqqInv = (dgMatrix*) dgMallocStack (sizeof (dgMatrix) * m_clustersCount);
m_clusterRotationInitialGuess = (dgMatrix*) dgMallocStack (sizeof (dgMatrix) * m_clustersCount);
m_clusterCom0 = (dgVector*) dgMallocStack (sizeof (dgVector) * m_clustersCount);
m_clusterMass = (dgFloat32*) dgMallocStack (sizeof (dgFloat32) * m_clustersCount);
dgInt32 poolSize = 0;
dgStack<dgInt32> particleClusterCountPool(m_particles.m_count);
dgInt32* const particleClusterCount = &particleClusterCountPool[0];
memset (particleClusterCount, 0, particleClusterCountPool.GetSizeInBytes());
for (dgClusterBuilder::dgListNode* node = clusterList.GetFirst(); node; node = node->GetNext()) {
dgCluster& cluster = node->GetInfo();
poolSize += cluster.m_count;
for (dgInt32 i = 0; i < cluster.m_count; i ++) {
dgInt32 j = cluster.m_points[i];
particleClusterCount[j] ++;
}
}
m_clusterWeight = (dgFloat32*) dgMallocStack (sizeof (dgFloat32) * poolSize);
m_clusterPosit = (dgInt32*) dgMallocStack (sizeof (dgInt32) * poolSize);
dgInt32 acc = 0;
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgInt32 count = particleClusterCount[i];
m_clusterPositStart[i] = acc;
particleClusterCount[i] = acc;
acc += count;
}
m_clusterPositStart[m_particles.m_count] = acc;
dgInt32 clusterIndex = 0;
for (dgClusterBuilder::dgListNode* node = clusterList.GetFirst(); node; node = node->GetNext()) {
dgCluster& cluster = node->GetInfo();
for (dgInt32 i = 0; i < cluster.m_count; i ++) {
dgInt32 j = cluster.m_points[i];
dgInt32 base = particleClusterCount[j];
m_clusterPosit[base] = clusterIndex;
m_clusterWeight[base] += dgFloat32 (1.0f);
particleClusterCount[j] ++;
}
clusterIndex ++;
}
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
const dgInt32 start = m_clusterPositStart[i];
const dgInt32 count = m_clusterPositStart[i + 1] - start;
dgAssert (count);
dgFloat32 weight = dgFloat32 (1.0f) / count;
for (dgInt32 j = 0; j < count; j ++) {
m_clusterWeight[start + j] = weight;
}
}
}
}
