bool dgCollisionConvexHull::OOBBTest (const dgMatrix& matrix, const dgCollisionConvex* const shape, void* const cacheOrder) const
{
bool ret;
_ASSERTE (cacheOrder);
ret = dgCollisionConvex::OOBBTest (matrix, shape, cacheOrder);
if (ret) {
const dgConvexSimplexEdge* const* faceArray = m_faceArray;
dgCollisionBoundPlaneCache* const cache = (dgCollisionBoundPlaneCache*)cacheOrder;
for (dgInt32 i = 0; i < dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)); i ++) {
dgFloat32 dist;
const dgPlane& plane = cache->m_planes[i];
if ((plane % plane) > dgFloat32 (0.0f)) {
dgVector dir (matrix.UnrotateVector(plane.Scale (-1.0f)));
dir.m_w = dgFloat32 (0.0f);
dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
dist = plane.Evalue (p);
if (dist > dgFloat32 (0.1f)){
return false;
}
}
}
for (dgInt32 i = 0; i < m_boundPlanesCount; i ++) {
dgInt32 i0;
dgInt32 i1;
dgInt32 i2;
dgFloat32 dist;
const dgConvexSimplexEdge* const face = faceArray[i];
i0 = face->m_prev->m_vertex;
i1 = face->m_vertex;
i2 = face->m_next->m_vertex;
const dgVector& p0 = m_vertex[i0];
dgVector normal ((m_vertex[i1] - p0) * (m_vertex[i2] - p0));
normal = normal.Scale (dgFloat32 (1.0f) / dgSqrt (normal % normal));
dgVector dir (matrix.UnrotateVector(normal.Scale (-1.0f)));
dir.m_w = dgFloat32 (0.0f);
dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
//_ASSERTE ((normal % (m_boxOrigin - p0)) < 0.0f);
dist = normal % (p - p0);
if (dist > dgFloat32 (0.1f)){
for (dgInt32 j = 0; j < (dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)) - 1); j ++) {
cache->m_planes[j + 1] = cache->m_planes[j];
}
cache->m_planes[1] = dgPlane (normal, - (normal % p0));
return false;
}
}
}
return ret;
}
void dgCollisionConvexPolygon::BeamClipping (const dgVector& origin, dgFloat32 dist)
{
dgPlane planes[4];
dgVector points[sizeof (m_localPoly) / sizeof (m_localPoly[0]) + 8];
dgClippedFaceEdge clippedFace [2 * sizeof (m_localPoly) / sizeof (m_localPoly[0]) + 8];
dgVector dir (m_localPoly[1] - m_localPoly[0]);
dgAssert (dir.m_w == dgFloat32 (0.0f));
dgAssert ((dir % dir) > dgFloat32 (1.0e-8f));
dir = dir.CompProduct4 (dir.InvMagSqrt());
dgFloat32 distH = origin.DotProduct4(dir).GetScalar();
planes[0] = dgPlane (dir, dist - distH);
planes[2] = dgPlane (dir.CompProduct4 (dgVector::m_negOne), dist + distH);
dir = m_normal * dir;
dgFloat32 distV = origin.DotProduct4(dir).GetScalar();
planes[1] = dgPlane (dir, dist - distV);
planes[3] = dgPlane (dir.CompProduct4 (dgVector::m_negOne), dist + distV);
for (dgInt32 i = 0; i < m_count; i ++) {
dgInt32 j = i << 1;
dgAssert (j < sizeof (clippedFace) / sizeof (clippedFace[0]));
points[i] = m_localPoly[i];
clippedFace[j + 0].m_twin = &clippedFace[j + 1];
clippedFace[j + 0].m_next = &clippedFace[j + 2];
clippedFace[j + 0].m_incidentVertex = i;
clippedFace[j + 0].m_incidentNormal = m_adjacentFaceEdgeNormalIndex[i];
clippedFace[j + 1].m_twin = &clippedFace[j + 0];
clippedFace[j + 1].m_next = &clippedFace[j - 2];
clippedFace[j + 1].m_incidentVertex = i + 1;
clippedFace[j + 1].m_incidentNormal = -1;
}
clippedFace[1].m_next = &clippedFace[m_count * 2 - 2 + 1];
dgAssert ((m_count * 2 - 2) >= 0);
clippedFace[m_count * 2 - 2].m_next = &clippedFace[0];
clippedFace[m_count * 2 - 2 + 1].m_incidentVertex = 0;
dgInt32 edgeCount = m_count * 2;
dgInt32 indexCount = m_count;
dgClippedFaceEdge* first = &clippedFace[0];
for (dgInt32 i = 0; i < 4; i ++) {
const dgPlane& plane = planes[i];
dgInt32 conectCount = 0;
dgClippedFaceEdge* connect[2];
dgClippedFaceEdge* ptr = first;
dgClippedFaceEdge* newFirst = first;
dgFloat32 test0 = plane.Evalue(points[ptr->m_incidentVertex]);
do {
dgFloat32 test1 = plane.Evalue(points[ptr->m_next->m_incidentVertex]);
if (test0 > dgFloat32 (1.0e-2f)) {
if (test1 <= dgFloat32 (-1.0e-2f)) {
const dgVector& p0 = points[ptr->m_incidentVertex];
const dgVector& p1 = points[ptr->m_next->m_incidentVertex];
dgVector dp (p1 - p0);
points[indexCount] = p0 - dp.Scale4 (test0 / dp.DotProduct4(plane).GetScalar());
dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_twin->m_incidentNormal = ptr->m_incidentNormal;
newEdge->m_incidentNormal = ptr->m_incidentNormal;
newEdge->m_incidentVertex = indexCount;
newEdge->m_twin->m_incidentVertex = ptr->m_next->m_incidentVertex;
ptr->m_twin->m_incidentVertex = indexCount;
newEdge->m_next = ptr->m_next;
ptr->m_next->m_twin->m_next = newEdge->m_twin;
newEdge->m_twin->m_next = ptr->m_twin;
ptr->m_next = newEdge;
connect[conectCount] = ptr;
conectCount ++;
indexCount ++;
edgeCount += 2;
ptr = newEdge;
}
} else {
if (test1 > dgFloat32 (1.0e-2f)) {
newFirst = ptr->m_next;
const dgVector& p0 = points[ptr->m_incidentVertex];
const dgVector& p1 = points[ptr->m_next->m_incidentVertex];
dgVector dp (p1 - p0);
points[indexCount] = p0 - dp.Scale4 (test0 / dp.DotProduct4(plane).GetScalar());
dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_twin->m_incidentNormal = ptr->m_incidentNormal;;
newEdge->m_incidentNormal = ptr->m_incidentNormal;
newEdge->m_incidentVertex = indexCount;
newEdge->m_twin->m_incidentVertex = ptr->m_next->m_incidentVertex;
ptr->m_twin->m_incidentVertex = indexCount;
newEdge->m_next = ptr->m_next;
ptr->m_next->m_twin->m_next = newEdge->m_twin;
newEdge->m_twin->m_next = ptr->m_twin;
ptr->m_next = newEdge;
connect[conectCount] = ptr;
conectCount ++;
indexCount ++;
edgeCount += 2;
ptr = newEdge;
}
}
test0 = test1;
ptr = ptr->m_next;
} while (ptr != first);
if(conectCount > 1) {
first = newFirst;
dgAssert (conectCount == 2);
dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_incidentNormal = m_faceNormalIndex;;
newEdge->m_incidentVertex = connect[0]->m_next->m_incidentVertex;
newEdge->m_twin->m_next = connect[0]->m_next;
connect[0]->m_next = newEdge;
newEdge->m_twin->m_incidentNormal = m_faceNormalIndex;;
newEdge->m_twin->m_incidentVertex = connect[1]->m_next->m_incidentVertex;
newEdge->m_next = connect[1]->m_next;
connect[1]->m_next = newEdge->m_twin;
edgeCount += 2;
}
}
dgClippedFaceEdge* ptr = first;
do {
dgVector dist (points[ptr->m_next->m_incidentVertex] - points[ptr->m_incidentVertex]);
dgFloat32 error = dist % dist;
if (error < dgFloat32 (1.0e-6f)) {
ptr->m_next = ptr->m_next->m_next;
first = ptr;
}
ptr = ptr->m_next;
} while (ptr != first);
dgInt32 count = 0;
dgInt32 isConvexCap = 0;
m_adjacentFaceEdgeNormalIndex = &m_clippEdgeNormal[0];
do {
m_clippEdgeNormal[count] = ptr->m_incidentNormal;
isConvexCap |= (ptr->m_incidentNormal - m_faceNormalIndex);
m_localPoly[count] = points[ptr->m_incidentVertex];
count ++;
ptr = ptr->m_next;
} while (ptr != first);
m_count = count;
}
