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; } } } }