void dgPolygonSoupDatabaseBuilder::EndAndOptimize(bool optimize)
{
if (m_faceCount) {
dgStack<hacd::HaI32> indexMapPool (m_indexCount + m_vertexCount);
hacd::HaI32* const indexMap = &indexMapPool[0];
m_vertexCount = dgVertexListToIndexList (&m_vertexPoints[0].m_x, sizeof (dgBigVector), 3, m_vertexCount, &indexMap[0], hacd::HaF32 (1.0e-4f));
hacd::HaI32 k = 0;
for (hacd::HaI32 i = 0; i < m_faceCount; i ++) {
k ++;
hacd::HaI32 count = m_faceVertexCount[i];
for (hacd::HaI32 j = 1; j < count; j ++) {
hacd::HaI32 index = m_vertexIndex[k];
index = indexMap[index];
m_vertexIndex[k] = index;
k ++;
}
}
OptimizeByIndividualFaces();
if (optimize) {
OptimizeByGroupID();
OptimizeByIndividualFaces();
}
}
}
void dgPolygonSoupDatabaseBuilder::End(bool optimize)
{
Optimize(optimize);
// build the normal array and adjacency array
// calculate all face the normals
hacd::HaI32 indexCount = 0;
m_normalPoints[m_faceCount].m_x = hacd::HaF64 (0.0f);
for (hacd::HaI32 i = 0; i < m_faceCount; i ++) {
hacd::HaI32 faceIndexCount = m_faceVertexCount[i];
hacd::HaI32* const ptr = &m_vertexIndex[indexCount + 1];
dgBigVector v0 (&m_vertexPoints[ptr[0]].m_x);
dgBigVector v1 (&m_vertexPoints[ptr[1]].m_x);
dgBigVector e0 (v1 - v0);
dgBigVector normal (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
for (hacd::HaI32 j = 2; j < faceIndexCount - 1; j ++) {
dgBigVector v2 (&m_vertexPoints[ptr[j]].m_x);
dgBigVector e1 (v2 - v0);
normal += e0 * e1;
e0 = e1;
}
normal = normal.Scale (dgRsqrt (normal % normal));
m_normalPoints[i].m_x = normal.m_x;
m_normalPoints[i].m_y = normal.m_y;
m_normalPoints[i].m_z = normal.m_z;
indexCount += faceIndexCount;
}
// compress normals array
m_normalIndex[m_faceCount] = 0;
m_normalCount = dgVertexListToIndexList(&m_normalPoints[0].m_x, sizeof (dgBigVector), 3, m_faceCount, &m_normalIndex[0], hacd::HaF32 (1.0e-4f));
}
void dgPolygonSoupDatabaseBuilder::PackArray()
{
dgStack<hacd::HaI32> indexMapPool (m_vertexCount);
hacd::HaI32* const indexMap = &indexMapPool[0];
m_vertexCount = dgVertexListToIndexList (&m_vertexPoints[0].m_x, sizeof (dgBigVector), 3, m_vertexCount, &indexMap[0], hacd::HaF32 (1.0e-6f));
hacd::HaI32 k = 0;
for (hacd::HaI32 i = 0; i < m_faceCount; i ++) {
k ++;
hacd::HaI32 count = m_faceVertexCount[i];
for (hacd::HaI32 j = 1; j < count; j ++) {
hacd::HaI32 index = m_vertexIndex[k];
index = indexMap[index];
m_vertexIndex[k] = index;
k ++;
}
}
m_run = DG_POINTS_RUN;
}
void dgCollisionSphere::Init (dgFloat32 radius, dgMemoryAllocator* allocator)
{
m_rtti |= dgCollisionSphere_RTTI;
m_radius = radius;
m_edgeCount = DG_SPHERE_EDGE_COUNT;
m_vertexCount = DG_SPHERE_VERTEX_COUNT;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgInt32 indexList[256];
dgVector tmpVectex[256];
dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));
dgInt32 i = 1;
dgInt32 count = 0;
TesselateTriangle (i, p4, p0, p2, count, tmpVectex);
TesselateTriangle (i, p4, p2, p1, count, tmpVectex);
TesselateTriangle (i, p4, p1, p3, count, tmpVectex);
TesselateTriangle (i, p4, p3, p0, count, tmpVectex);
TesselateTriangle (i, p5, p2, p0, count, tmpVectex);
TesselateTriangle (i, p5, p1, p2, count, tmpVectex);
TesselateTriangle (i, p5, p3, p1, count, tmpVectex);
TesselateTriangle (i, p5, p0, p3, count, tmpVectex);
//dgAssert (count == EDGE_COUNT);
dgInt32 vertexCount = dgVertexListToIndexList (&tmpVectex[0].m_x, sizeof (dgVector), 3 * sizeof (dgFloat32), 0, count, indexList, 0.001f);
dgAssert (vertexCount == DG_SPHERE_VERTEX_COUNT);
for (dgInt32 i = 0; i < vertexCount; i ++) {
m_unitSphere[i] = tmpVectex[i];
}
dgPolyhedra polyhedra(m_allocator);
polyhedra.BeginFace();
for (dgInt32 i = 0; i < count; i += 3) {
#ifdef _DEBUG
dgEdge* const edge = polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]);
dgAssert (edge);
#else
polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]);
#endif
}
polyhedra.EndFace();
dgUnsigned64 i1 = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i1;
i1 ++;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
for (dgInt32 i = 0; i < DG_SPHERE_VERTEX_COUNT; i ++) {
m_vertex[i] = m_unitSphere[i].Scale4 (m_radius);
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
dgMeshEffect * dgMeshEffect::CreateVoronoiConvexDecomposition (dgMemoryAllocator * const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32 * const pointCloud, dgInt32 materialId, const dgMatrix & textureProjectionMatrix)
{
dgFloat32 normalAngleInRadians = 30.0f * 3.1416f / 180.0f;
dgStack<dgBigVector> buffer (pointCount + 16);
dgBigVector * const pool = &buffer[0];
dgInt32 count = 0;
dgFloat64 quantizeFactor = dgFloat64 (16.0f);
dgFloat64 invQuantizeFactor = dgFloat64 (1.0f) / quantizeFactor;
dgInt32 stride = pointStrideInBytes / sizeof (dgFloat32);
dgBigVector pMin (dgFloat32 (1.0e10f), dgFloat32 (1.0e10f), dgFloat32 (1.0e10f), dgFloat32 (0.0f));
dgBigVector pMax (dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < pointCount; i ++)
{
dgFloat64 x = pointCloud[i * stride + 0];
dgFloat64 y = pointCloud[i * stride + 1];
dgFloat64 z = pointCloud[i * stride + 2];
x = floor (x * quantizeFactor) * invQuantizeFactor;
y = floor (y * quantizeFactor) * invQuantizeFactor;
z = floor (z * quantizeFactor) * invQuantizeFactor;
dgBigVector p (x, y, z, dgFloat64 (0.0f));
pMin = dgBigVector (dgMin (x, pMin.m_x), dgMin (y, pMin.m_y), dgMin (z, pMin.m_z), dgFloat64 (0.0f));
pMax = dgBigVector (dgMax (x, pMax.m_x), dgMax (y, pMax.m_y), dgMax (z, pMax.m_z), dgFloat64 (0.0f));
pool[count] = p;
count ++;
}
// add the bbox as a barrier
pool[count + 0] = dgBigVector ( pMin.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 1] = dgBigVector ( pMax.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 2] = dgBigVector ( pMin.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 3] = dgBigVector ( pMax.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 4] = dgBigVector ( pMin.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 5] = dgBigVector ( pMax.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 6] = dgBigVector ( pMin.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 7] = dgBigVector ( pMax.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
count += 8;
dgStack<dgInt32> indexList (count);
count = dgVertexListToIndexList (&pool[0].m_x, sizeof (dgBigVector), 3, count, &indexList[0], dgFloat64 (5.0e-2f));
dgAssert (count >= 8);
dgFloat64 maxSize = dgMax (pMax.m_x - pMin.m_x, pMax.m_y - pMin.m_y, pMax.m_z - pMin.m_z);
pMin -= dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f));
pMax += dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f));
// add the a guard zone, so that we do no have to clip
dgInt32 guadVertexKey = count;
pool[count + 0] = dgBigVector ( pMin.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 1] = dgBigVector ( pMax.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 2] = dgBigVector ( pMin.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 3] = dgBigVector ( pMax.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 4] = dgBigVector ( pMin.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 5] = dgBigVector ( pMax.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 6] = dgBigVector ( pMin.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 7] = dgBigVector ( pMax.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
count += 8;
dgDelaunayTetrahedralization delaunayTetrahedras (allocator, &pool[0].m_x, count, sizeof (dgBigVector), dgFloat32 (0.0f));
delaunayTetrahedras.RemoveUpperHull ();
// delaunayTetrahedras.Save("xxx0.txt");
dgInt32 tetraCount = delaunayTetrahedras.GetCount();
dgStack<dgBigVector> voronoiPoints (tetraCount + 32);
dgStack<dgDelaunayTetrahedralization::dgListNode *> tetradrumNode (tetraCount);
dgTree<dgList<dgInt32>, dgInt32> delanayNodes (allocator);
dgInt32 index = 0;
const dgHullVector * const delanayPoints = delaunayTetrahedras.GetHullVertexArray();
for (dgDelaunayTetrahedralization::dgListNode * node = delaunayTetrahedras.GetFirst(); node; node = node->GetNext())
{
dgConvexHull4dTetraherum & tetra = node->GetInfo();
voronoiPoints[index] = tetra.CircumSphereCenter (delanayPoints);
tetradrumNode[index] = node;
for (dgInt32 i = 0; i < 4; i ++)
{
dgTree<dgList<dgInt32>, dgInt32>::dgTreeNode * header = delanayNodes.Find (tetra.m_faces[0].m_index[i]);
if (!header)
{
dgList<dgInt32> list (allocator);
header = delanayNodes.Insert (list, tetra.m_faces[0].m_index[i]);
}
header->GetInfo().Append (index);
}
index ++;
}
dgMeshEffect * const voronoiPartition = new (allocator) dgMeshEffect (allocator);
voronoiPartition->BeginPolygon();
dgFloat64 layer = dgFloat64 (0.0f);
dgTree<dgList<dgInt32>, dgInt32>::Iterator iter (delanayNodes);
for (iter.Begin(); iter; iter ++)
{
dgTree<dgList<dgInt32>, dgInt32>::dgTreeNode * const nodeNode = iter.GetNode();
const dgList<dgInt32> & list = nodeNode->GetInfo();
dgInt32 key = nodeNode->GetKey();
if (key < guadVertexKey)
{
dgBigVector pointArray[512];
dgInt32 indexArray[512];
dgInt32 count = 0;
for (dgList<dgInt32>::dgListNode * ptr = list.GetFirst(); ptr; ptr = ptr->GetNext())
{
dgInt32 i = ptr->GetInfo();
pointArray[count] = voronoiPoints[i];
count ++;
dgAssert (count < dgInt32 (sizeof (pointArray) / sizeof (pointArray[0])));
}
count = dgVertexListToIndexList (&pointArray[0].m_x, sizeof (dgBigVector), 3, count, &indexArray[0], dgFloat64 (1.0e-3f));
if (count >= 4)
{
dgMeshEffect convexMesh (allocator, &pointArray[0].m_x, count, sizeof (dgBigVector), dgFloat64 (0.0f));
if (convexMesh.GetCount())
{
convexMesh.CalculateNormals (normalAngleInRadians);
convexMesh.UniformBoxMapping (materialId, textureProjectionMatrix);
for (dgInt32 i = 0; i < convexMesh.m_pointCount; i ++)
convexMesh.m_points[i].m_w = layer;
for (dgInt32 i = 0; i < convexMesh.m_atribCount; i ++)
convexMesh.m_attrib[i].m_vertex.m_w = layer;
voronoiPartition->MergeFaces (&convexMesh);
layer += dgFloat64 (1.0f);
}
}
}
}
voronoiPartition->EndPolygon (dgFloat64 (1.0e-8f), false);
// voronoiPartition->SaveOFF("xxx0.off");
//voronoiPartition->ConvertToPolygons();
return voronoiPartition;
}