dgInt32 dgCollisionSphere::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgAssert (normal.m_w == 0.0f);
//contactsOut[0] = normal.Scale3(normal % point);
contactsOut[0] = normal.CompProduct4 (normal.DotProduct4(point));
return 1;
}
dgInt32 dgCollisionSphere::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgAssert (normal.m_w == 0.0f);
dgAssert ((normal % normal) > dgFloat32 (0.999f));
//contactsOut[0] = normal.Scale3 (normal % point);
contactsOut[0] = normal.CompProduct4 (normal.DotProduct4(point));
return 1;
}
dgIntersectStatus dgCollisionBVH::GetPolygon (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance)
{
dgPolygonMeshDesc& data = (*(dgPolygonMeshDesc*) context);
if (data.m_faceCount >= DG_MAX_COLLIDING_FACES) {
dgTrace (("buffer Over float, try using a lower resolution mesh for collision\n"));
return t_StopSearh;
}
if ((data.m_globalIndexCount + indexCount * 2 + 3) >= DG_MAX_COLLIDING_INDICES) {
dgTrace (("buffer Over float, try using a lower resolution mesh for collision\n"));
return t_StopSearh;
}
if (data.m_me->GetDebugCollisionCallback()) {
dgTriplex triplex[128];
dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
const dgVector scale = data.m_polySoupInstance->GetScale();
dgMatrix matrix (data.m_polySoupInstance->GetLocalMatrix() * data.m_polySoupBody->GetMatrix());
for (dgInt32 i = 0; i < indexCount; i ++ ) {
//dgVector p (&polygon[indexArray[i] * stride]);
//p = matrix.TransformVector(scale.CompProduct4(p));
dgVector p (matrix.TransformVector(scale.CompProduct4(dgVector(&polygon[indexArray[i] * stride]))));
triplex[i].m_x = p.m_x;
triplex[i].m_y = p.m_y;
triplex[i].m_z = p.m_z;
}
if (data.m_polySoupBody) {
data.m_me->GetDebugCollisionCallback() (data.m_polySoupBody, data.m_objBody, indexArray[indexCount], indexCount, &triplex[0].m_x, sizeof (dgTriplex));
}
}
dgAssert (data.m_vertex == polygon);
dgInt32 count = indexCount * 2 + 3;
data.m_faceIndexCount[data.m_faceCount] = indexCount;
// data.m_faceIndexStart[data.m_faceCount] = data.m_faceCount ? (data.m_faceIndexStart[data.m_faceCount - 1] + data.m_faceIndexCount[data.m_faceCount - 1]) : 0;
data.m_faceIndexStart[data.m_faceCount] = data.m_globalIndexCount;
data.m_hitDistance[data.m_faceCount] = hitDistance;
data.m_faceCount ++;
dgInt32* const dst = &data.m_faceVertexIndex[data.m_globalIndexCount];
//the docks say memcpy is an intrinsic function but as usual this is another Microsoft lied
//memcpy (dst, indexArray, sizeof (dgInt32) * count);
for (dgInt32 i = 0; i < count; i ++) {
dst[i] = indexArray[i];
}
data.m_globalIndexCount += count;
return t_ContinueSearh;
}
dgInt32 dgCollisionChamferCylinder::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgFloat32 disc2 = point.m_y * point.m_y + point.m_z * point.m_z;
if (disc2 < m_radius * m_radius) {
dgVector cylinderNormal ((point.m_x >= dgFloat32 (0.0)) ? dgFloat32 (-1.0f) : dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
return CalculateContactsGeneric (point, cylinderNormal, proxy, contactsOut);
} else {
dgVector r (dgFloat32 (0.0f), point.m_y, point.m_z, dgFloat32 (0.0f));
dgAssert ((r % r) > dgFloat32 (0.0f));
// r = r.Scale3(m_radius * dgRsqrt (r % r));
r = r.CompProduct4(r.InvMagSqrt()).Scale4(m_radius);
//dgFloat32 t = normal % (r - point);
dgVector t (normal.DotProduct4(r - point));
//contactsOut[0] = r - normal.Scale3 (t);
contactsOut[0] = r - normal.CompProduct4(t);
return 1;
}
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
dgAssert(proxy.m_referenceCollision->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_floatingCollision->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
const dgCollisionInstance* const polygonInstance = proxy.m_floatingCollision;
dgAssert(this == polygonInstance->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
dgInt32 count = 0;
m_normal = m_normal.CompProduct4(polyInstanceInvScale);
dgAssert(m_normal.m_w == dgFloat32(0.0f));
m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
dgVector savedFaceNormal(m_normal);
dgVector savedPosit (proxy.m_matrix.m_posit);
proxy.m_matrix.m_posit = dgVector::m_wOne;
dgVector hullOrigin(proxy.m_matrix.UnrotateVector (savedPosit));
for (dgInt32 i = 0; i < m_count; i++) {
m_localPoly[i] = hullOrigin + polyInstanceScale.CompProduct4(dgVector(&m_vertex[m_vertexIndex[i] * m_stride]));
dgAssert(m_localPoly[i].m_w == dgFloat32(0.0f));
}
dgContact* const contactJoint = proxy.m_contactJoint;
const dgCollisionInstance* const hull = proxy.m_referenceCollision;
dgVector normalInHull(proxy.m_matrix.RotateVector(m_normal));
dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
dgVector p0(proxy.m_matrix.UntransformVector(pointInHull));
dgVector p1(proxy.m_matrix.UntransformVector(hull->SupportVertex(normalInHull, NULL)));
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
contactJoint->m_closestDistance = -penetration;
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
contactJoint->m_closestDistance = dgFloat32(0.0f);
dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
if (distance >= dgFloat32(0.0f)) {
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e);
//dgPlane plane(n, -(m_localPoly[i0] % n));
dgPlane plane(n, - m_localPoly[i0].DotProduct4 (n).GetScalar());
plane = proxy.m_matrix.TransformPlane(plane);
//dgFloat32 supportDist = dgAbsf(plane.m_x) * boxSize.m_x + dgAbsf(plane.m_y) * boxSize.m_y + dgAbsf(plane.m_z) * boxSize.m_z;
//dgFloat32 centerDist = plane.Evalue(boxOrigin);
dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();
if ((centerDist + supportDist) < dgFloat32(0.0f)) {
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
if ((centerDist - supportDist) < dgFloat32(0.0f)) {
inside = false;
break;
}
i0 = i;
}
const dgInt32 hullId = hull->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
dgAssert(penetration >= dgFloat32(0.0f));
dgVector pointsContacts[64];
dgAssert(penetration >= 0.0f);
dgVector point(pointInHull + normalInHull.Scale4(penetration));
count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, pointsContacts, 1.0f);
dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));
const dgMatrix& worldMatrix = hull->m_globalMatrix;
dgContactPoint* const contactsOut = proxy.m_contacts;
dgAssert(contactsOut);
dgVector globalNormal(worldMatrix.RotateVector(normalInHull));
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = worldMatrix.TransformVector(pointsContacts[i] + step);
contactsOut[i].m_normal = globalNormal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
} else {
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
dgCollisionConvex* const convexShape = (dgCollisionConvex*)hull->m_childShape;
count = convexShape->CalculateConvexToConvexContact(proxy);
dgAssert(proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal(polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
if (normal.DotProduct4(savedFaceNormal).GetScalar() < dgFloat32(0.9995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector n(&m_vertex[index * m_stride]);
if ((savedFaceNormal.DotProduct4(n).GetScalar() > dgFloat32(0.9995f))) {
normal = n;
} else {
dgVector dir0(n * savedFaceNormal);
dgVector dir1(n * normal);
dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
if (projection <= dgFloat32(0.0f)) {
normal = n;
}
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_normal = normal;
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
}
proxy.m_matrix.m_posit = savedPosit;
return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI));
dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI));
const dgCollisionInstance* const hull = proxy.m_referenceCollision;
dgAssert (this == proxy.m_floatingCollision->GetChildShape());
dgAssert (m_count);
dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const floatingBody = proxy.m_floatingBody;
const dgBody* const referenceBody = proxy.m_referenceBody;
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32 (1.0e10f);
m_normal = m_normal.CompProduct4(polyInstanceInvScale);
dgAssert (m_normal.m_w == dgFloat32 (0.0f));
m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
const dgVector savedFaceNormal (m_normal);
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride]));
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
}
dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f))));
hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne;
dgMatrix polygonMatrix;
polygonMatrix[0] = m_localPoly[1] - m_localPoly[0];
polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = hullOrigin;
dgAssert (polygonMatrix.TestOrthogonal());
dgMatrix savedProxyMatrix (proxy.m_matrix);
proxy.m_matrix = polygonMatrix * proxy.m_matrix;
dgVector floatingVeloc (floatingBody->m_veloc);
dgVector referenceVeloc (referenceBody->m_veloc);
const dgMatrix& hullMatrix = hull->GetGlobalMatrix();
dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc));
dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc));
dgVector polyBoxP0 (dgFloat32 ( 1.0e15f));
dgVector polyBoxP1 (dgFloat32 (-1.0e15f));
m_normal = polygonMatrix.UnrotateVector(m_normal);
if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]);
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]);
polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]);
}
dgInt32 count = 0;
dgVector hullBoxP0;
dgVector hullBoxP1;
hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1);
dgVector minBox (polyBoxP0 - hullBoxP1);
dgVector maxBox (polyBoxP1 - hullBoxP0);
dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
if (distance < dgFloat32 (1.0f)) {
dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// boxOrigin += polyRelativeVeloc.Scale4 (distance);
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal);
dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane));
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector e (m_localPoly[i] - m_localPoly[i0]);
dgVector n (m_normal * e);
dgPlane plane (n, - (m_localPoly[i0] % n));
dgVector supportDist (plane.Abs().DotProduct4 (boxSize));
dgFloat32 centerDist = plane.Evalue(boxOrigin);
if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
if ((centerDist - supportDist.m_x) < dgFloat32 (0.0f)) {
inside = false;
}
i0 = i;
}
// for the time being for the minkousky contact calculation
inside = false;
const dgInt32 hullId = hull->GetUserDataID();
if (inside) {
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector p0 (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 timetoImpact = dgFloat32 (0.0f);
//dgFloat32 closestDistance = dgFloat32 (0.0f);
dgAssert (0);
// dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness + DG_IMPULSIVE_CONTACT_PENETRATION;
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32 (0.0f)) {
timetoImpact = penetration / (polyRelativeVeloc % m_normal);
dgAssert (timetoImpact >= dgFloat32 (0.0f));
// closestDistance = -penetration;
}
if (timetoImpact <= proxy.m_timestep) {
dgVector pointsContacts[64];
contactJoint->m_closestDistance = penetration;
dgAssert (0);
// dgVector point (pointInHull - normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector point (pointInHull);
count = hull->CalculatePlaneIntersection (normalInHull, point, pointsContacts, 1.0f);
dgAssert (0);
// dgVector step (hullRelativeVeloc.Scale3 (timetoImpact) + normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector step (hullRelativeVeloc.Scale3 (timetoImpact));
penetration = dgMax (penetration, dgFloat32 (0.0f));
const dgMatrix& worldMatrix = hull->m_globalMatrix;
dgContactPoint* const contactsOut = proxy.m_contacts;
dgVector globalNormal (worldMatrix.RotateVector(normalInHull));
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_point = worldMatrix.TransformVector (pointsContacts[i] + step);
contactsOut[i].m_normal = globalNormal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
} else {
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize ();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping (boxOrigin, convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
dgCollisionConvex* const convexShape = (dgCollisionConvex*) hull->m_childShape;
count = convexShape->CalculateConvexCastContacts (proxy);
// dgAssert (proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
#if 0
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal (polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
if ((normal % savedFaceNormal) < dgFloat32 (0.995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector n (&m_vertex[index * m_stride]);
dgVector dir0 (n * savedFaceNormal);
dgVector dir1 (n * normal);
dgFloat32 projection = dir0 % dir1;
if (projection <= dgFloat32 (0.0f)) {
normal = n;
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_normal = normal;
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i ++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
#endif
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
proxy.m_matrix = savedProxyMatrix;
return count;
}
dgInt32 dgCollisionBox::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const
{
dgVector support[4];
dgInt32 featureCount = 3;
const dgConvexSimplexEdge** const vertToEdgeMapping = GetVertexToEdgeMapping();
if (vertToEdgeMapping) {
dgInt32 edgeIndex;
//support[0] = SupportVertex (normal.Scale4(normalSign), &edgeIndex);
support[0] = SupportVertex (normal, &edgeIndex);
dgFloat32 dist = normal.DotProduct4(support[0] - point).GetScalar();
if (dist <= DG_IMPULSIVE_CONTACT_PENETRATION) {
dgVector normalAlgin (normal.Abs());
if (!((normalAlgin.m_x > dgFloat32 (0.9999f)) || (normalAlgin.m_y > dgFloat32 (0.9999f)) || (normalAlgin.m_z > dgFloat32 (0.9999f)))) {
// 0.25 degrees
const dgFloat32 tiltAngle = dgFloat32 (0.005f);
const dgFloat32 tiltAngle2 = tiltAngle * tiltAngle ;
dgPlane testPlane (normal, - (normal.DotProduct4(support[0]).GetScalar()));
featureCount = 1;
const dgConvexSimplexEdge* const edge = vertToEdgeMapping[edgeIndex];
const dgConvexSimplexEdge* ptr = edge;
do {
const dgVector& p = m_vertex[ptr->m_twin->m_vertex];
dgFloat32 test1 = testPlane.Evalue(p);
dgVector dist (p - support[0]);
dgFloat32 angle2 = test1 * test1 / (dist.DotProduct4(dist).GetScalar());
if (angle2 < tiltAngle2) {
support[featureCount] = p;
featureCount ++;
}
ptr = ptr->m_twin->m_next;
} while ((ptr != edge) && (featureCount < 3));
}
}
}
dgInt32 count = 0;
switch (featureCount)
{
case 1:
{
contactsOut[0] = support[0] - normal.CompProduct4(normal.DotProduct4(support[0] - point));
count = 1;
break;
}
case 2:
{
contactsOut[0] = support[0] - normal.CompProduct4(normal.DotProduct4(support[0] - point));
contactsOut[1] = support[1] - normal.CompProduct4(normal.DotProduct4(support[1] - point));
count = 2;
break;
}
default:
{
dgFloat32 test[8];
dgAssert(normal.m_w == dgFloat32(0.0f));
dgPlane plane(normal, -(normal.DotProduct4(point).GetScalar()));
for (dgInt32 i = 0; i < 8; i++) {
dgAssert(m_vertex[i].m_w == dgFloat32(0.0f));
test[i] = plane.DotProduct4(m_vertex[i] | dgVector::m_wOne).m_x;
}
dgConvexSimplexEdge* edge = NULL;
for (dgInt32 i = 0; i < dgInt32 (sizeof (m_edgeEdgeMap) / sizeof (m_edgeEdgeMap[0])); i ++) {
dgConvexSimplexEdge* const ptr = m_edgeEdgeMap[i];
dgFloat32 side0 = test[ptr->m_vertex];
dgFloat32 side1 = test[ptr->m_twin->m_vertex];
if ((side0 * side1) < dgFloat32 (0.0f)) {
edge = ptr;
break;
}
}
if (edge) {
if (test[edge->m_vertex] < dgFloat32 (0.0f)) {
edge = edge->m_twin;
}
dgAssert (test[edge->m_vertex] > dgFloat32 (0.0f));
dgConvexSimplexEdge* ptr = edge;
dgConvexSimplexEdge* firstEdge = NULL;
dgFloat32 side0 = test[edge->m_vertex];
do {
dgAssert (m_vertex[ptr->m_twin->m_vertex].m_w == dgFloat32 (0.0f));
dgFloat32 side1 = test[ptr->m_twin->m_vertex];
if (side1 < side0) {
if (side1 < dgFloat32 (0.0f)) {
firstEdge = ptr;
break;
}
side0 = side1;
edge = ptr->m_twin;
ptr = edge;
}
ptr = ptr->m_twin->m_next;
} while (ptr != edge);
if (firstEdge) {
edge = firstEdge;
ptr = edge;
do {
dgVector dp (m_vertex[ptr->m_twin->m_vertex] - m_vertex[ptr->m_vertex]);
dgFloat32 t = plane.DotProduct4(dp).m_x;
if (t >= dgFloat32 (-1.e-24f)) {
t = dgFloat32 (0.0f);
} else {
t = test[ptr->m_vertex] / t;
if (t > dgFloat32 (0.0f)) {
t = dgFloat32 (0.0f);
}
if (t < dgFloat32 (-1.0f)) {
t = dgFloat32 (-1.0f);
}
}
dgAssert (t <= dgFloat32 (0.01f));
dgAssert (t >= dgFloat32 (-1.05f));
contactsOut[count] = m_vertex[ptr->m_vertex] - dp.Scale4 (t);
count ++;
dgConvexSimplexEdge* ptr1 = ptr->m_next;
for (; ptr1 != ptr; ptr1 = ptr1->m_next) {
dgInt32 index0 = ptr1->m_twin->m_vertex;
if (test[index0] >= dgFloat32 (0.0f)) {
dgAssert (test[ptr1->m_vertex] <= dgFloat32 (0.0f));
break;
}
}
dgAssert (ptr != ptr1);
ptr = ptr1->m_twin;
} while ((ptr != edge) && (count < 8));
}
}
}
}
if (count > 2) {
count = RectifyConvexSlice (count, normal, contactsOut);
}
return count;
}
dgFloat32 dgCollisionInstance::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
if (!preFilter || preFilter(body, this, userData)) {
switch(m_scaleType)
{
case m_unit:
{
dgFloat32 t = m_childShape->RayCast (localP0, localP1, maxT, contactOut, body, userData, preFilter);
if (t <= maxT) {
if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) {
contactOut.m_shapeId0 = GetUserDataID();
contactOut.m_shapeId1 = GetUserDataID();
}
if (!m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI)) {
contactOut.m_collision0 = this;
contactOut.m_collision1 = this;
}
}
return t;
}
case m_uniform:
{
dgVector p0 (localP0.CompProduct4(m_invScale));
dgVector p1 (localP1.CompProduct4(m_invScale));
dgFloat32 t = m_childShape->RayCast (p0, p1, maxT, contactOut, body, userData, preFilter);
if (t <= maxT) {
if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) {
contactOut.m_shapeId0 = GetUserDataID();
contactOut.m_shapeId1 = GetUserDataID();
}
if (!m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI)) {
contactOut.m_collision0 = this;
contactOut.m_collision1 = this;
}
}
return t;
}
case m_nonUniform:
{
dgVector p0 (localP0.CompProduct4(m_invScale));
dgVector p1 (localP1.CompProduct4(m_invScale));
dgFloat32 t = m_childShape->RayCast (p0, p1, maxT, contactOut, body, userData, preFilter);
if (t <= maxT) {
if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) {
contactOut.m_shapeId0 = GetUserDataID();
contactOut.m_shapeId1 = GetUserDataID();
dgVector n (m_invScale.CompProduct4 (contactOut.m_normal));
contactOut.m_normal = n.CompProduct4(n.InvMagSqrt());
}
if (!m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI)) {
contactOut.m_collision0 = this;
contactOut.m_collision1 = this;
}
}
return t;
}
case m_global:
default:
{
dgVector p0 (m_aligmentMatrix.UntransformVector (localP0.CompProduct4(m_invScale)));
dgVector p1 (m_aligmentMatrix.UntransformVector (localP1.CompProduct4(m_invScale)));
dgFloat32 t = m_childShape->RayCast (p0, p1, maxT, contactOut, body, userData, preFilter);
if (t <= maxT) {
if (!(m_childShape->IsType(dgCollision::dgCollisionMesh_RTTI) || m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) {
contactOut.m_shapeId0 = GetUserDataID();
contactOut.m_shapeId1 = GetUserDataID();
dgVector n (m_aligmentMatrix.RotateVector(m_invScale.CompProduct4 (contactOut.m_normal)));
contactOut.m_normal = n.CompProduct4(n.InvMagSqrt());
}
if (!(m_childShape->IsType(dgCollision::dgCollisionCompound_RTTI))) {
contactOut.m_collision0 = this;
contactOut.m_collision1 = this;
}
}
return t;
}
}
}
return dgFloat32 (1.2f);
}
