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); }