void dgCollisionSphere::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
dgTriplex pool[1024 * 2];
dgVector tmpVectex[1024 * 2];
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 = 3;
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);
for (dgInt32 i = 0; i < count; i ++) {
tmpVectex[i] = tmpVectex[i].Scale4 (m_radius);
}
//dgMatrix matrix (GetLocalMatrix() * matrixPtr);
matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &tmpVectex[0].m_x, sizeof (dgVector), count);
for (dgInt32 i = 0; i < count; i += 3) {
callback (userData, 3, &pool[i].m_x, 0);
}
}
void dgCollisionChamferCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//dgCollisionConvex::DebugCollision (matrix, callback, userData);
//return;
dgInt32 slices = 12;
dgInt32 brakes = 24;
dgFloat32 sliceAngle = dgFloat32 (0.0f);
dgFloat32 sliceStep = dgPI / slices;
dgFloat32 breakStep = dgPI2 / brakes;
dgTriplex pool[24 * (12 + 1)];
dgMatrix rot (dgPitchMatrix (breakStep));
dgInt32 index = 0;
for (dgInt32 j = 0; j <= slices; j ++) {
dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (0.0f));
sliceAngle += sliceStep;
for (dgInt32 i = 0; i < brakes; i ++) {
pool[index].m_x = p0.m_x;
pool[index].m_y = p0.m_y;
pool[index].m_z = p0.m_z;
index ++;
p0 = rot.UnrotateVector (p0);
}
}
//dgMatrix matrix (GetLocalMatrix() * matrixPtr);
matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * (12 + 1));
dgTriplex face[32];
index = 0;
for (dgInt32 j = 0; j < slices; j ++) {
dgInt32 index0 = index + brakes - 1;
for (dgInt32 i = 0; i < brakes; i ++) {
face[0] = pool[index];
face[1] = pool[index0];
face[2] = pool[index0 + brakes];
face[3] = pool[index + brakes];
index0 = index;
index ++;
callback (userData, 4, &face[0].m_x, 0);
}
}
for (dgInt32 i = 0; i < brakes; i ++) {
face[i] = pool[i];
}
callback (userData, 24, &face[0].m_x, 0);
for (dgInt32 i = 0; i < brakes; i ++) {
face[i] = pool[brakes * (slices + 1) - i - 1];
}
callback (userData, 24, &face[0].m_x, 0);
}
void dgCollisionTaperedCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
dgTriplex pool[24 * 2];
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < 24; i ++) {
dgFloat32 z = dgSin (angle);
dgFloat32 y = dgCos (angle);
pool[i].m_x = - m_height;
pool[i].m_y = y * m_radio1;
pool[i].m_z = z * m_radio1;
pool[i + 24].m_x = m_height;
pool[i + 24].m_y = y * m_radio0;
pool[i + 24].m_z = z * m_radio0;
angle += dgPI2 / dgFloat32 (24.0f);
}
matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * 2);
dgTriplex face[24];
dgInt32 j = 24 - 1;
for (dgInt32 i = 0; i < 24; i ++) {
face[0] = pool[j];
face[1] = pool[i];
face[2] = pool[i + 24];
face[3] = pool[j + 24];
j = i;
callback (userData, 4, &face[0].m_x, 0);
}
for (dgInt32 i = 0; i < 24; i ++) {
face[i] = pool[24 - 1 - i];
}
callback (userData, 24, &face[0].m_x, 0);
for (dgInt32 i = 0; i < 24; i ++) {
face[i] = pool[i + 24];
}
callback (userData, 24, &face[0].m_x, 0);
}
void dgCollisionCone::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//dgCollisionConvex::DebugCollision (matrix, callback, userData);
//return;
#define NUMBER_OF_DEBUG_SEGMENTS 40
dgTriplex pool[NUMBER_OF_DEBUG_SEGMENTS + 1];
dgTriplex face[NUMBER_OF_DEBUG_SEGMENTS];
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) {
dgFloat32 z = dgSin (angle) * m_radius;
dgFloat32 y = dgCos (angle) * m_radius;
pool[i].m_x = -m_height;
pool[i].m_y = y;
pool[i].m_z = z;
angle += dgPI2 / dgFloat32 (NUMBER_OF_DEBUG_SEGMENTS);
}
pool[NUMBER_OF_DEBUG_SEGMENTS].m_x = m_height;
pool[NUMBER_OF_DEBUG_SEGMENTS].m_y = dgFloat32 (0.0f);
pool[NUMBER_OF_DEBUG_SEGMENTS].m_z = dgFloat32 (0.0f);
matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), NUMBER_OF_DEBUG_SEGMENTS + 1);
dgInt32 j = NUMBER_OF_DEBUG_SEGMENTS - 1;
for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) {
face[0] = pool[j];
face[1] = pool[i];
face[2] = pool[NUMBER_OF_DEBUG_SEGMENTS];
j = i;
callback (userData, 3, &face[0].m_x, 0);
}
for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) {
face[i] = pool[NUMBER_OF_DEBUG_SEGMENTS - 1 - i];
}
callback (userData, NUMBER_OF_DEBUG_SEGMENTS, &face[0].m_x, 0);
}
void dgCollisionConvexHull::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
// dgTriplex tmp[1024 * 4];
// matrix.TransformTriplex (&tmp[0].m_x, sizeof (dgTriplex), &m_vertex[0].m_x, sizeof (dgVector), m_vertexCount);
dgTriplex vertex[256];
for (dgInt32 i = 0; i < m_faceCount; i ++) {
dgConvexSimplexEdge* const face = m_faceArray[i];
dgConvexSimplexEdge* ptr = face;
dgInt32 count = 0;
do {
//vertex[count] = tmp[ptr->m_vertex];
vertex[count].m_x = m_vertex[ptr->m_vertex].m_x;
vertex[count].m_y = m_vertex[ptr->m_vertex].m_y;
vertex[count].m_z = m_vertex[ptr->m_vertex].m_z;
count ++;
dgAssert (count < sizeof (vertex)/ sizeof (vertex[0]));
ptr = ptr->m_next;
} while (ptr != face);
matrix.TransformTriplex (&vertex[0].m_x, sizeof (dgTriplex), &vertex[0].m_x, sizeof (dgTriplex), count);
callback (userData, count, &vertex[0].m_x, 0);
}
}
void dgCollisionTaperedCapsule::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
#define POWER 2
dgVector tmpVectex[1024 * 2];
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 count = 0;
TesselateTriangle (POWER, p0, p2, p4, count, tmpVectex);
TesselateTriangle (POWER, p0, p4, p3, count, tmpVectex);
TesselateTriangle (POWER, p0, p3, p5, count, tmpVectex);
TesselateTriangle (POWER, p0, p5, p2, count, tmpVectex);
TesselateTriangle (POWER, p1, p4, p2, count, tmpVectex);
TesselateTriangle (POWER, p1, p3, p4, count, tmpVectex);
TesselateTriangle (POWER, p1, p5, p3, count, tmpVectex);
TesselateTriangle (POWER, p1, p2, p5, count, tmpVectex);
dgFloat32 scale = m_radio1 / m_radio0;
dgVector edgeP0(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector edgeP1(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < count; i += 3) {
dgVector face0[4];
dgVector face1[4];
dgInt32 n0 = 0;
dgInt32 n1 = 0;
dgVector p0 (tmpVectex[i + 2]);
for (dgInt32 j = 0; j < 3; j ++) {
dgVector p1 (tmpVectex[i + j]);
if (p1.m_x > m_clip0) {
if (p0.m_x < m_clip0) {
dgFloat32 t = (m_clip0 - p0.m_x) / (p1.m_x - p0.m_x);
edgeP0 = p0 + (p1 - p0).Scale3 (t);
edgeP0.m_x = m_clip0;
face0[n0] = edgeP0;
face0[n0].m_x += m_height;
n0 ++;
face1[n1] = edgeP0.Scale3 (scale);
face1[n1].m_x -= m_height;
n1 ++;
}
face0[n0] = p1;
face0[n0].m_x += m_height;
n0 ++;
} else {
if (p0.m_x > m_clip0) {
dgFloat32 t = (m_clip0 - p0.m_x) / (p1.m_x - p0.m_x);
edgeP1 = p0 + (p1 - p0).Scale3 (t);
edgeP1.m_x = m_clip0;
face0[n0] = edgeP1;
face0[n0].m_x += m_height;
n0 ++;
face1[n1] = edgeP1.Scale3 (scale);
face1[n1].m_x -= m_height;
n1 ++;
}
face1[n1] = p1.Scale3 (scale);;
face1[n1].m_x -= m_height;
n1 ++;
}
p0 = p1;
}
dgTriplex face[4];
if (n0) {
matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face0[0].m_x, sizeof (dgVector), n0);
callback (userData, n0, &face[0].m_x, 0);
}
if (n1) {
matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face1[0].m_x, sizeof (dgVector), n1);
callback (userData, n1, &face[0].m_x, 0);
}
if (n0 && n1) {
face0[0] = edgeP0;
face0[1] = edgeP1;
face0[2] = edgeP1.Scale3 (scale);
face0[3] = edgeP0.Scale3 (scale);
face0[0].m_x += m_height;
face0[1].m_x += m_height;
face0[2].m_x -= m_height;
face0[3].m_x -= m_height;
dgPlane plane (face0[0], face0[1], face0[2]);
if (plane.m_w >= dgFloat32 (0.0f)) {
dgAssert (0);
}
matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face0[0].m_x, sizeof (dgVector), 4);
callback (userData, 4, &face[0].m_x, 0);
}
}
}
void dgPolygonSoupDatabaseBuilder::AddMesh (const hacd::HaF32* const vertex, hacd::HaI32 vertexCount, hacd::HaI32 strideInBytes, hacd::HaI32 faceCount,
const hacd::HaI32* const faceArray, const hacd::HaI32* const indexArray, const hacd::HaI32* const faceTagsData, const dgMatrix& worldMatrix)
{
hacd::HaI32 faces[256];
hacd::HaI32 pool[2048];
m_vertexPoints[m_vertexCount + vertexCount].m_x = hacd::HaF64 (0.0f);
dgBigVector* const vertexPool = &m_vertexPoints[m_vertexCount];
worldMatrix.TransformTriplex (&vertexPool[0].m_x, sizeof (dgBigVector), vertex, strideInBytes, vertexCount);
for (hacd::HaI32 i = 0; i < vertexCount; i ++) {
vertexPool[i].m_w = hacd::HaF64 (0.0f);
}
hacd::HaI32 totalIndexCount = faceCount;
for (hacd::HaI32 i = 0; i < faceCount; i ++) {
totalIndexCount += faceArray[i];
}
m_vertexIndex[m_indexCount + totalIndexCount] = 0;
m_faceVertexCount[m_faceCount + faceCount] = 0;
hacd::HaI32 k = 0;
for (hacd::HaI32 i = 0; i < faceCount; i ++) {
hacd::HaI32 count = faceArray[i];
for (hacd::HaI32 j = 0; j < count; j ++) {
hacd::HaI32 index = indexArray[k];
pool[j] = index + m_vertexCount;
k ++;
}
hacd::HaI32 convexFaces = 0;
if (count == 3) {
convexFaces = 1;
dgBigVector p0 (m_vertexPoints[pool[2]]);
for (hacd::HaI32 i = 0; i < 3; i ++) {
dgBigVector p1 (m_vertexPoints[pool[i]]);
dgBigVector edge (p1 - p0);
hacd::HaF64 mag2 = edge % edge;
if (mag2 < hacd::HaF32 (1.0e-6f)) {
convexFaces = 0;
}
p0 = p1;
}
if (convexFaces) {
dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
dgBigVector normal (edge0 * edge1);
hacd::HaF64 mag2 = normal % normal;
if (mag2 < hacd::HaF32 (1.0e-8f)) {
convexFaces = 0;
}
}
if (convexFaces) {
faces[0] = 3;
}
} else {
convexFaces = AddConvexFace (count, pool, faces);
}
hacd::HaI32 index = 0;
for (hacd::HaI32 k = 0; k < convexFaces; k ++) {
hacd::HaI32 count = faces[k];
m_vertexIndex[m_indexCount] = faceTagsData[i];
m_indexCount ++;
for (hacd::HaI32 j = 0; j < count; j ++) {
m_vertexIndex[m_indexCount] = pool[index];
index ++;
m_indexCount ++;
}
m_faceVertexCount[m_faceCount] = count + 1;
m_faceCount ++;
}
}
m_vertexCount += vertexCount;
m_run -= vertexCount;
if (m_run <= 0) {
PackArray();
}
}