bool dgCollisionConvexHull::CheckConvex (dgPolyhedra& polyhedra1, const dgBigVector* hullVertexArray) const
{
dgPolyhedra polyhedra(polyhedra1);
dgPolyhedra::Iterator iter (polyhedra);
dgBigVector center (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgInt32 count = 0;
dgInt32 mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
if (edge->m_mark < mark) {
count ++;
center += hullVertexArray[edge->m_incidentVertex];
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr = ptr->m_twin->m_next;
} while (ptr != edge);
}
}
center = center.Scale3 (dgFloat64 (1.0f) / dgFloat64 (count));
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgBigVector normal0 (FaceNormal (edge, hullVertexArray));
dgBigVector normal1 (FaceNormal (edge->m_twin, hullVertexArray));
dgBigPlane plane0 (normal0, - (normal0 % hullVertexArray[edge->m_incidentVertex]));
dgBigPlane plane1 (normal1, - (normal1 % hullVertexArray[edge->m_twin->m_incidentVertex]));
dgFloat64 test0 = plane0.Evalue(center);
if (test0 > dgFloat64 (1.0e-3f)) {
return false;
}
dgFloat64 test1 = plane1.Evalue(center);
// if (test1 > dgFloat64 (0.0f)) {
if (test1 > dgFloat64 (1.0e-3f)) {
return false;
}
}
return true;
}
static const k3d::ienumeration_property::enumeration_values_t& polyhedron_values()
{
static k3d::ienumeration_property::enumeration_values_t values;
if(values.empty())
{
k3d::string_t polyhedron;
std::istringstream polyhedra(k3d::resource::get_string("/module/uniform_polyhedron/data/polyhedra.txt"));
for(std::getline(polyhedra, polyhedron); polyhedra; std::getline(polyhedra, polyhedron))
{
boost::iterator_range<k3d::string_t::iterator> result = boost::algorithm::find_first(polyhedron, " ");
if(result)
{
values.push_back(
k3d::ienumeration_property::enumeration_value_t(
"(" + k3d::string_cast(values.size()) + ") " + k3d::string_t(result.end(), polyhedron.end()),
k3d::string_t(polyhedron.begin(), result.begin()),
""));
}
}
}
return values;
}
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 ();
}
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;
}
void dgCollisionTaperedCapsule::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height)
{
m_rtti |= dgCollisionTaperedCapsule_RTTI;
m_radio0 = dgAbsf (radio0);
m_radio1 = dgAbsf (radio1);
m_height = dgAbsf (height * 0.5f);
m_clip1 = dgFloat32 (0.0f);
m_clip0 = dgFloat32 (0.0f);
dgFloat32 angle0 = dgFloat32 (-3.141592f / 2.0f);
dgFloat32 angle1 = dgFloat32 ( 3.141592f / 2.0f);
do {
dgFloat32 angle = (angle1 + angle0) * dgFloat32 (0.5f);
dgVector dir (dgSin (angle), dgCos (angle), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p0(dir.Scale3 (m_radio0));
dgVector p1(dir.Scale3 (m_radio1));
p0.m_x += m_height;
p1.m_x -= m_height;
dgFloat32 dir0 = p0 % dir;
dgFloat32 dir1 = p1 % dir;
if (dir0 > dir1) {
angle1 = angle;
m_clip0 = p0.m_x - m_height;
} else {
angle0 = angle;
m_clip1 = p1.m_x + m_height;
}
} while ((angle1 - angle0) > dgFloat32 (0.001f * 3.141592f/180.0f));
dgFloat32 angle = (angle1 + angle0) * dgFloat32 (0.5f);
m_sideNormal = dgVector (dgSin (angle), dgCos (angle), dgFloat32 (0.0f), dgFloat32 (0.0f));
m_clipRadio0 = dgSqrt (m_radio0 * m_radio0 - m_clip0 * m_clip0);
m_clipRadio1 = dgSqrt (m_radio1 * m_radio1 - m_clip1 * m_clip1);
dgInt32 i1 = 0;
dgInt32 i0 = DG_CAPSULE_SEGMENTS * DG_CAP_SEGMENTS * 2;
dgFloat32 dx0 = (m_clip0 - m_radio0) / DG_CAP_SEGMENTS;
dgFloat32 x0 = m_radio0 + dx0;
dgFloat32 dx1 = (m_clip1 + m_radio1) / DG_CAP_SEGMENTS;
dgFloat32 x1 = -m_radio1 + dx1;
for (dgInt32 j = 0; j < DG_CAP_SEGMENTS; j ++) {
dgFloat32 angle = dgFloat32 (0.0f);
dgFloat32 r0 = dgSqrt (m_radio0 * m_radio0 - x0 * x0);
dgFloat32 r1 = dgSqrt (m_radio1 * m_radio1 - x1 * x1);
i0 -= DG_CAPSULE_SEGMENTS;
for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) {
dgFloat32 z = dgSin (angle);
dgFloat32 y = dgCos (angle);
m_vertex[i0] = dgVector ( m_height + x0, y * r0, z * r0, dgFloat32 (0.0f));
m_vertex[i1] = dgVector (-m_height + x1, y * r1, z * r1, dgFloat32 (0.0f));
i0 ++;
i1 ++;
angle += dgPI2 / DG_CAPSULE_SEGMENTS;
}
x0 += dx0;
x1 += dx1;
i0 -= DG_CAPSULE_SEGMENTS;
}
m_vertexCount = DG_CAPSULE_SEGMENTS * DG_CAP_SEGMENTS * 2;
m_edgeCount = DG_CAPSULE_SEGMENTS * (6 + 8 * (DG_CAP_SEGMENTS - 1));
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CAPSULE_SEGMENTS + 10];
i1 = 0;
i0 = DG_CAPSULE_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 j = 0; j < DG_CAP_SEGMENTS * 2 - 1; j ++) {
for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) {
wireframe[0] = i0;
wireframe[1] = i1;
wireframe[2] = i1 + DG_CAPSULE_SEGMENTS;
wireframe[3] = i0 + DG_CAPSULE_SEGMENTS;
i0 = i1;
i1 ++;
polyhedra.AddFace (4, wireframe);
}
i0 = i1 + DG_CAPSULE_SEGMENTS - 1;
}
for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) {
wireframe[i] = DG_CAPSULE_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_CAPSULE_SEGMENTS, wireframe);
for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) {
wireframe[i] = i + DG_CAPSULE_SEGMENTS * (DG_CAP_SEGMENTS * 2 - 1);
}
polyhedra.AddFace (DG_CAPSULE_SEGMENTS, wireframe);
polyhedra.EndFace ();
dgAssert (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
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];
}
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
bool dgCollisionConvexHull::Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance)
{
dgInt32 stride = strideInBytes / sizeof (dgFloat32);
dgStack<dgFloat64> buffer(3 * 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()) {
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;
}
_ASSERTE (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);
}
}
dgInt32 vertexCount = convexHull->GetVertexCount();
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)) {
return false;
}
}
}
dgInt32 maxEdgeCount = polyhedra.GetCount();
dgStack<dgEdge*> stack(1024 + maxEdgeCount);
dgEdge* firstFace = &polyhedra.GetRoot()->GetInfo();
_ASSERTE (firstFace->m_twin->m_next != firstFace);
dgInt32 stackIndex = 1;
stack[0] = firstFace;
dgStack<dgInt32> vertexMap(vertexCount);
memset (&vertexMap[0], -1, vertexCount * sizeof (dgInt32));
// m_edgeCount = 0;
// m_vertexCount = 0;
dgInt32 i1 = polyhedra.IncLRU();
while (stackIndex) {
stackIndex --;
dgEdge* const edge0 = stack[stackIndex];
if (edge0->m_mark != i1) {
if (vertexMap[edge0->m_incidentVertex] == -1) {
vertexMap[edge0->m_incidentVertex] = m_vertexCount;
m_vertexCount ++;
}
dgEdge* ptr = edge0;
do {
stack[stackIndex] = ptr->m_twin;
stackIndex++;
ptr->m_mark = i1;
ptr->m_userData = m_edgeCount;
m_edgeCount ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge0) ;
}
}
m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector)));
m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * 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 (1.0f);
}
}
i1 = polyhedra.IncLRU();
stackIndex = 1;
stack[0] = firstFace;
while (stackIndex) {
stackIndex --;
dgEdge* const edge0 = stack[stackIndex];
if (edge0->m_mark != i1) {
dgEdge *ptr = edge0;
do {
ptr->m_mark = i1;
stack[stackIndex] = ptr->m_twin;
stackIndex++;
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 != edge0) ;
}
}
SetVolumeAndCG ();
m_faceCount = 0;
dgStack<char> mark (m_edgeCount);
memset (&mark[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 (!mark[i]) {
dgConvexSimplexEdge* ptr = face;
do {
_ASSERTE ((ptr - m_simplex) >= 0);
mark[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 *));
delete convexHull;
return true;
}
void dgCollisionCone::Init (dgFloat32 radius, dgFloat32 height)
{
// dgInt32 i;
// dgInt32 j;
// dgEdge *edge;
// dgFloat32 y;
// dgFloat32 z;
// dgFloat32 angle;
m_rtti |= dgCollisionCone_RTTI;
m_radius = dgAbsf (radius);
m_height = dgAbsf (height * dgFloat32 (0.5f));
m_sinAngle = m_radius / dgSqrt (height * height + m_radius * m_radius);;
m_amp = dgFloat32 (0.5f) * m_radius / m_height;
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
dgFloat32 z = dgSin (angle) * m_radius;
dgFloat32 y = dgCos (angle) * m_radius;
m_vertex[i] = dgVector (- m_height, y, z, dgFloat32 (1.0f));
angle += dgPI2 / DG_CONE_SEGMENTS;
}
m_vertex[DG_CONE_SEGMENTS] = dgVector (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f));
m_edgeCount = DG_CONE_SEGMENTS * 4;
m_vertexCount = DG_CONE_SEGMENTS + 1;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CONE_SEGMENTS];
dgInt32 j = DG_CONE_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
wireframe[0] = j;
wireframe[1] = i;
wireframe[2] = DG_CONE_SEGMENTS;
j = i;
polyhedra.AddFace (3, wireframe);
}
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
wireframe[i] = DG_CONE_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_CONE_SEGMENTS, wireframe);
polyhedra.EndFace ();
_ASSERTE (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
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];
}
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
void dgCollisionChamferCylinder::Init (dgFloat32 radius, dgFloat32 height)
{
// dgInt32 i;
// dgInt32 j;
// dgInt32 index;
// dgInt32 index0;
// dgFloat32 sliceStep;
// dgFloat32 sliceAngle;
// dgFloat32 breakStep;
// dgFloat32 breakAngle;
// dgEdge *edge;
m_rtti |= dgCollisionChamferCylinder_RTTI;
m_radius = dgAbsf (radius);
m_height = dgAbsf (height * dgFloat32 (0.5f));
m_radius = GetMax (dgFloat32(0.001f), m_radius - m_height);
m_silhuette[0] = dgVector (m_height, m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_silhuette[1] = dgVector (m_height, -m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_silhuette[2] = dgVector (-m_height, -m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_silhuette[3] = dgVector (-m_height, m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));
// m_tethaStep = GetDiscretedAngleStep (m_radius);
// m_tethaStepInv = dgFloat32 (1.0f) / m_tethaStep;
// m_delCosTetha = dgCos (m_tethaStep);
// m_delSinTetha = dgSin (m_tethaStep);
dgFloat32 sliceAngle = dgFloat32 (0.0f);
//dgFloat32 breakAngle = dgFloat32 (0.0f);
dgFloat32 sliceStep = dgPI / DG_CHAMFERCYLINDER_SLICES;
dgFloat32 breakStep = dgPI2 / DG_CHAMFERCYLINDER_BRAKES;
dgMatrix rot (dgPitchMatrix (breakStep));
dgInt32 index = 0;
for (dgInt32 j = 0; j <= DG_CHAMFERCYLINDER_SLICES; j ++) {
dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (1.0f));
sliceAngle += sliceStep;
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
m_vertex[index] = p0;
index ++;
p0 = rot.UnrotateVector (p0);
}
}
m_edgeCount = (4 * DG_CHAMFERCYLINDER_SLICES + 2)* DG_CHAMFERCYLINDER_BRAKES;
m_vertexCount = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1);
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CHAMFERCYLINDER_SLICES + 10];
for (dgInt32 i = 0; i < DG_MAX_CHAMFERCYLINDER_DIR_COUNT; i ++) {
dgMatrix matrix (dgPitchMatrix (dgFloat32 (dgPI2 * i) / DG_MAX_CHAMFERCYLINDER_DIR_COUNT));
m_shapesDirs[i] = matrix.RotateVector (dgVector (dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)));
}
dgInt32 index = 0;
for (dgInt32 j = 0; j < DG_CHAMFERCYLINDER_SLICES; j ++) {
dgInt32 index0 = index + DG_CHAMFERCYLINDER_BRAKES - 1;
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
wireframe[0] = index;
wireframe[1] = index0;
wireframe[2] = index0 + DG_CHAMFERCYLINDER_BRAKES;
wireframe[3] = index + DG_CHAMFERCYLINDER_BRAKES;
index0 = index;
index ++;
polyhedra.AddFace (4, wireframe);
}
}
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
wireframe[i] = i;
}
polyhedra.AddFace (DG_CHAMFERCYLINDER_BRAKES, wireframe);
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
wireframe[i] = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1) - i - 1;
}
polyhedra.AddFace (DG_CHAMFERCYLINDER_BRAKES, wireframe);
polyhedra.EndFace ();
_ASSERTE (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
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];
}
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
// CalculateDistanceTravel();
}
void dgCollisionBox::Init (dgFloat32 size_x, dgFloat32 size_y, dgFloat32 size_z)
{
m_rtti |= dgCollisionBox_RTTI;
m_size[0].m_x = dgMax (dgAbsf (size_x) * dgFloat32 (0.5f), dgFloat32(2.0f) * D_BOX_SKIN_THINCKNESS);
m_size[0].m_y = dgMax (dgAbsf (size_y) * dgFloat32 (0.5f), dgFloat32(2.0f) * D_BOX_SKIN_THINCKNESS);
m_size[0].m_z = dgMax (dgAbsf (size_z) * dgFloat32 (0.5f), dgFloat32(2.0f) * D_BOX_SKIN_THINCKNESS);
m_size[0].m_w = dgFloat32 (0.0f);
m_size[1].m_x = - m_size[0].m_x;
m_size[1].m_y = - m_size[0].m_y;
m_size[1].m_z = - m_size[0].m_z;
m_size[1].m_w = dgFloat32 (0.0f);
m_edgeCount = 24;
m_vertexCount = 8;
m_vertex[0] = dgVector ( m_size[0].m_x, m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[1] = dgVector (-m_size[0].m_x, m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[2] = dgVector ( m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[3] = dgVector (-m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[4] = dgVector ( m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[5] = dgVector (-m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[6] = dgVector ( m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[7] = dgVector (-m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
dgCollisionConvex::m_vertex = m_vertex;
dgCollisionConvex::m_simplex = m_edgeArray;
if (!m_initSimplex) {
dgPolyhedra polyhedra (GetAllocator());
polyhedra.BeginFace();
for (dgInt32 i = 0; i < 6; i ++) {
polyhedra.AddFace (4, &m_faces[i][0]);
}
polyhedra.EndFace();
int index = 0;
dgInt32 mark = polyhedra.IncLRU();;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr->m_userData = index;
index ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
dgAssert (index == 24);
polyhedra.IncLRU();
mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
dgEdge *ptr = edge;
do {
ptr->m_mark = mark;
dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
simplexPtr->m_vertex = 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) ;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
m_vertexToEdgeMap[edge->m_incidentVertex] = &m_simplex[edge->m_userData];
}
dgInt32 count = 0;
mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
edge->m_mark = mark;
edge->m_twin->m_mark = mark;
m_edgeEdgeMap[count] = &m_simplex[edge->m_userData];
count ++;
dgAssert (count <= 12);
}
}
m_initSimplex = 1;
}
SetVolumeAndCG ();
}
void dgCollisionTaperedCylinder::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height)
{
m_rtti |= dgCollisionTaperedCylinder_RTTI;
m_radio0 = dgAbsf (radio0);
m_radio1 = dgAbsf (radio1);
m_height = dgAbsf (height * dgFloat32 (0.5f));
m_dirVector.m_x = radio1 - radio0;
m_dirVector.m_y = m_height * dgFloat32 (2.0f);
m_dirVector.m_z = dgFloat32 (0.0f);
m_dirVector.m_w = dgFloat32 (0.0f);
m_dirVector = m_dirVector.Scale3(dgRsqrt(m_dirVector % m_dirVector));
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
dgFloat32 sinAngle = dgSin (angle);
dgFloat32 cosAngle = dgCos (angle);
m_vertex[i ] = dgVector (- m_height, m_radio1 * cosAngle, m_radio1 * sinAngle, dgFloat32 (0.0f));
m_vertex[i + DG_CYLINDER_SEGMENTS] = dgVector ( m_height, m_radio0 * cosAngle, m_radio0 * sinAngle, dgFloat32 (0.0f));
angle += dgPI2 / DG_CYLINDER_SEGMENTS;
}
m_edgeCount = DG_CYLINDER_SEGMENTS * 6;
m_vertexCount = DG_CYLINDER_SEGMENTS * 2;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CYLINDER_SEGMENTS];
dgInt32 j = DG_CYLINDER_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[0] = j;
wireframe[1] = i;
wireframe[2] = i + DG_CYLINDER_SEGMENTS;
wireframe[3] = j + DG_CYLINDER_SEGMENTS;
j = i;
polyhedra.AddFace (4, wireframe);
}
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = DG_CYLINDER_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = i + DG_CYLINDER_SEGMENTS;
}
polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);
polyhedra.EndFace ();
dgAssert (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
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];
}
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
dgInt32 dgPolygonSoupDatabaseBuilder::AddConvexFace(dgInt32 count,
dgInt32* const pool, dgInt32* const facesArray)
{
dgPolySoupFilterAllocator polyhedra(m_allocator);
count = polyhedra.AddFilterFace(dgUnsigned32(count), pool);
dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
if (edge->m_incidentFace < 0)
{
edge = edge->m_twin;
}
dgInt32 isconvex = 1;
dgInt32 facesCount = 0;
dgInt32 flag = 1;
while (flag)
{
flag = 0;
if (count >= 3)
{
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
do
{
dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
dgFloat64 mag2 = e0 % e0;
if (mag2 < dgFloat32(1.0e-6f))
{
count--;
flag = 1;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
p0 = p1;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
if (count >= 3)
{
flag = 1;
while (flag)
{
flag = 0;
if (count >= 3)
{
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
do
{
dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e1(p2 - p1);
e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
dgFloat64 mag2 = e1 % e0;
if (mag2 > dgFloat32(0.9999f))
{
count--;
flag = 1;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
dgBigVector normal(
polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
sizeof(dgBigVector)));
dgFloat64 mag2 = normal % normal;
if (mag2 < dgFloat32(1.0e-8f))
{
return 0;
}
normal = normal.Scale(dgRsqrt (mag2));
if (count >= 3)
{
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
do
{
dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e1(p2 - p1);
e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
dgBigVector n(e0 * e1);
dgFloat64 mag2 = n % normal;
if (mag2 < dgFloat32(1.0e-5f))
{
isconvex = 0;
break;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
if (isconvex)
{
dgEdge* const first = edge;
if (count >= 3)
{
count = 0;
dgEdge* ptr = first;
do
{
pool[count] = ptr->m_incidentVertex;
count++;
ptr = ptr->m_next;
} while (ptr != first);
facesArray[facesCount] = count;
facesCount = 1;
}
}
else
{
dgPolyhedra leftOver(m_allocator);
dgPolyhedra polyhedra2(m_allocator);
dgEdge* ptr = edge;
count = 0;
do
{
pool[count] = ptr->m_incidentVertex;
count++;
ptr = ptr->m_next;
} while (ptr != edge);
polyhedra2.BeginFace();
polyhedra2.AddFace(count, pool);
polyhedra2.EndFace();
leftOver.BeginFace();
polyhedra2.ConvexPartition(&m_vertexPoints[0].m_x, sizeof(dgTriplex),
&leftOver);
leftOver.EndFace();
_ASSERTE(leftOver.GetCount() == 0);
dgInt32 mark = polyhedra2.IncLRU();
dgInt32 index = 0;
dgPolyhedra::Iterator iter(polyhedra2);
for (iter.Begin(); iter; iter++)
{
dgEdge* const edge = &(*iter);
if (edge->m_incidentFace < 0)
{
continue;
}
if (edge->m_mark == mark)
{
continue;
}
ptr = edge;
count = 0;
do
{
ptr->m_mark = mark;
pool[index] = ptr->m_incidentVertex;
index++;
count++;
ptr = ptr->m_next;
} while (ptr != edge);
facesArray[facesCount] = count;
facesCount++;
}
}
return facesCount;
}
dgInt32 dgPolygonSoupDatabaseBuilder::FilterFace(dgInt32 count,
dgInt32* const pool)
{
if (count == 3)
{
dgBigVector p0(m_vertexPoints[pool[2]]);
for (dgInt32 i = 0; i < 3; i++)
{
dgBigVector p1(m_vertexPoints[pool[i]]);
dgBigVector edge(p1 - p0);
dgFloat64 mag2 = edge % edge;
if (mag2 < dgFloat32(1.0e-6f))
{
count = 0;
}
p0 = p1;
}
if (count == 3)
{
dgBigVector edge0(m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
dgBigVector edge1(m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
dgBigVector normal(edge0 * edge1);
dgFloat64 mag2 = normal % normal;
if (mag2 < dgFloat32(1.0e-8f))
{
count = 0;
}
}
}
else
{
dgPolySoupFilterAllocator polyhedra(m_allocator);
count = polyhedra.AddFilterFace(dgUnsigned32(count), pool);
if (!count)
{
return 0;
}
dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
if (edge->m_incidentFace < 0)
{
edge = edge->m_twin;
}
bool flag = true;
while (flag)
{
flag = false;
if (count >= 3)
{
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
do
{
dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
dgFloat64 mag2 = e0 % e0;
if (mag2 < dgFloat32(1.0e-6f))
{
count--;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
p0 = p1;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
if (count >= 3)
{
flag = true;
dgBigVector normal(
polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
sizeof(dgBigVector)));
_ASSERTE((normal % normal) > dgFloat32 (1.0e-10f));
normal = normal.Scale(dgRsqrt (normal % normal + dgFloat32 (1.0e-20f)));
while (flag)
{
flag = false;
if (count >= 3)
{
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
do
{
dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e1(p2 - p1);
e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
dgFloat64 mag2 = e1 % e0;
if (mag2 > dgFloat32(0.9999f))
{
count--;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
dgBigVector n(e0 * e1);
mag2 = n % normal;
if (mag2 < dgFloat32(1.0e-5f))
{
count--;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
}
dgEdge* first = edge;
if (count >= 3)
{
dgFloat64 best = dgFloat32(2.0f);
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
do
{
dgBigVector p2(
&m_vertexPoints[ptr->m_next->m_next->m_incidentVertex].m_x);
dgBigVector e1(p2 - p1);
e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
dgFloat64 mag2 = fabs(e1 % e0);
if (mag2 < best)
{
best = mag2;
first = ptr;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
count = 0;
ptr = first;
do
{
pool[count] = ptr->m_incidentVertex;
count++;
ptr = ptr->m_next;
} while (ptr != first);
}
#ifdef _DEBUG
if (count >= 3)
{
dgInt32 j0 = count - 2;
dgInt32 j1 = count - 1;
dgBigVector normal(
polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
sizeof(dgBigVector)));
for (dgInt32 j2 = 0; j2 < count; j2++)
{
dgBigVector p0(&m_vertexPoints[pool[j0]].m_x);
dgBigVector p1(&m_vertexPoints[pool[j1]].m_x);
dgBigVector p2(&m_vertexPoints[pool[j2]].m_x);
dgBigVector e0((p0 - p1));
dgBigVector e1((p2 - p1));
dgBigVector n(e1 * e0);
_ASSERTE((n % normal) > dgFloat32 (0.0f));
j0 = j1;
j1 = j2;
}
}
#endif
}
return (count >= 3) ? count : 0;
}
void dgPolygonSoupDatabaseBuilder::OptimizeByGroupID(
dgPolygonSoupDatabaseBuilder& source, dgInt32 faceNumber,
dgInt32 faceIndexNumber, dgPolygonSoupDatabaseBuilder& leftOver)
{
dgInt32 indexPool[1024 * 1];
dgInt32 atributeData[1024 * 1];
dgVector vertexPool[1024 * 1];
dgPolyhedra polyhedra(m_allocator);
dgInt32 attribute = source.m_vertexIndex[faceIndexNumber];
for (dgInt32 i = 0; i < dgInt32(sizeof(atributeData) / sizeof(dgInt32)); i++)
{
indexPool[i] = i;
atributeData[i] = attribute;
}
leftOver.Begin();
polyhedra.BeginFace();
for (dgInt32 i = faceNumber; i < source.m_faceCount; i++)
{
dgInt32 indexCount;
indexCount = source.m_faceVertexCount[i];
_ASSERTE(indexCount < 1024);
if (source.m_vertexIndex[faceIndexNumber] == attribute)
{
dgEdge* const face = polyhedra.AddFace(indexCount - 1,
&source.m_vertexIndex[faceIndexNumber + 1]);
if (!face)
{
dgInt32 faceArray;
for (dgInt32 j = 0; j < indexCount - 1; j++)
{
dgInt32 index;
index = source.m_vertexIndex[faceIndexNumber + j + 1];
vertexPool[j] = source.m_vertexPoints[index];
}
faceArray = indexCount - 1;
leftOver.AddMesh(&vertexPool[0].m_x, indexCount - 1, sizeof(dgVector),
1, &faceArray, indexPool, atributeData, dgGetIdentityMatrix());
}
else
{
// set the attribute
dgEdge* ptr = face;
do
{
ptr->m_userData = dgUnsigned64(attribute);
ptr = ptr->m_next;
} while (ptr != face);
}
}
faceIndexNumber += indexCount;
}
leftOver.Optimize(false);
polyhedra.EndFace();
dgPolyhedra facesLeft(m_allocator);
facesLeft.BeginFace();
polyhedra.ConvexPartition(&source.m_vertexPoints[0].m_x, sizeof(dgBigVector),
&facesLeft);
facesLeft.EndFace();
dgInt32 mark = polyhedra.IncLRU();
dgPolyhedra::Iterator iter(polyhedra);
for (iter.Begin(); iter; iter++)
{
dgEdge* const edge = &(*iter);
if (edge->m_incidentFace < 0)
{
continue;
}
if (edge->m_mark == mark)
{
continue;
}
dgEdge* ptr = edge;
dgInt32 indexCount = 0;
do
{
ptr->m_mark = mark;
vertexPool[indexCount] = source.m_vertexPoints[ptr->m_incidentVertex];
indexCount++;
ptr = ptr->m_next;
} while (ptr != edge);
if (indexCount >= 3)
{
AddMesh(&vertexPool[0].m_x, indexCount, sizeof(dgVector), 1, &indexCount,
indexPool, atributeData, dgGetIdentityMatrix());
}
}
mark = facesLeft.IncLRU();
dgPolyhedra::Iterator iter1(facesLeft);
for (iter1.Begin(); iter1; iter1++)
{
dgEdge* const edge = &(*iter1);
if (edge->m_incidentFace < 0)
{
continue;
}
if (edge->m_mark == mark)
{
continue;
}
dgEdge* ptr = edge;
dgInt32 indexCount = 0;
do
{
ptr->m_mark = mark;
vertexPool[indexCount] = source.m_vertexPoints[ptr->m_incidentVertex];
indexCount++;
ptr = ptr->m_next;
} while (ptr != edge);
if (indexCount >= 3)
{
AddMesh(&vertexPool[0].m_x, indexCount, sizeof(dgVector), 1, &indexCount, indexPool, atributeData, dgGetIdentityMatrix());
}
}
}
