dgInt32 dgCollisionChamferCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const { dgInt32 count = 0; const dgFloat32 inclination = dgFloat32 (0.9999f); if (normal.m_x < -inclination) { dgMatrix matrix(normal); dgFloat32 x = dgSqrt (dgMax (m_height * m_height - origin.m_x * origin.m_x, dgFloat32 (0.0f))); matrix.m_posit.m_x = origin.m_x; dgVector scale(m_radius + x); const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]); for (dgInt32 i = 0; i < n; i++) { contactsOut[i] = matrix.TransformVector(m_unitCircle[i] * scale) & dgVector::m_triplexMask; } count = RectifyConvexSlice(n, normal, contactsOut); } else if (normal.m_x > inclination) { dgMatrix matrix(normal); dgFloat32 x = dgSqrt (dgMax (m_height * m_height - origin.m_x * origin.m_x, dgFloat32 (0.0f))); matrix.m_posit.m_x = origin.m_x; dgVector scale(m_radius + x); const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]); for (dgInt32 i = 0; i < n; i++) { contactsOut[i] = matrix.TransformVector(m_unitCircle[i] * scale) & dgVector::m_triplexMask; } count = RectifyConvexSlice(n, normal, contactsOut); } else { count = 1; contactsOut[0] = SupportVertex (normal, NULL); } return count; }
dgInt32 dgCollisionCone::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const { dgInt32 count = 0; if (normal.m_x > dgFloat32(0.99f)) { contactsOut[0] = dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); count = 1; } else if (normal.m_x < dgFloat32(-0.995f)) { if (normal.m_x < dgFloat32(-0.9995f)) { dgMatrix matrix(normal); matrix.m_posit.m_x = origin.m_x; dgVector scale(m_radius); const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]); for (dgInt32 i = 0; i < n; i++) { contactsOut[i] = matrix.TransformVector(m_unitCircle[i].CompProduct4(scale)) & dgVector::m_triplexMask; } count = RectifyConvexSlice(n, normal, contactsOut); } else { dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z); dgFloat32 cosAng = normal.m_y * magInv; dgFloat32 sinAng = normal.m_z * magInv; dgAssert(dgAbsf(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f)); dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f)); dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f)); count = dgCollisionConvex::CalculatePlaneIntersection(normal1, origin1, contactsOut); if (count > 6) { dgInt32 dy = 2 * 6; dgInt32 dx = 2 * count; dgInt32 acc = dy - count; dgInt32 index = 0; for (dgInt32 i = 0; i < count; i++) { if (acc > 0) { contactsOut[index] = contactsOut[i]; index++; acc -= dx; } acc += dy; } count = index; } for (dgInt32 i = 0; i < count; i++) { dgFloat32 y = contactsOut[i].m_y; dgFloat32 z = contactsOut[i].m_z; contactsOut[i].m_y = y * cosAng - z * sinAng; contactsOut[i].m_z = z * cosAng + y * sinAng; } } } else { dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z); dgFloat32 cosAng = normal.m_y * magInv; dgFloat32 sinAng = normal.m_z * magInv; dgAssert(dgAbsf(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f)); dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f)); dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f)); count = 0; int i0 = 2; dgVector test0((m_profile[i0] - origin1).DotProduct4(normal1)); for (int i = 0; (i < 3) && (count < 2); i++) { dgVector test1((m_profile[i] - origin1).DotProduct4(normal1)); dgVector acrossPlane(test0.CompProduct4(test1)); if (acrossPlane.m_x < 0.0f) { dgVector step(m_profile[i] - m_profile[i0]); contactsOut[count] = m_profile[i0] - step.Scale4(test0.m_x / (step.DotProduct4(normal1).m_x)); count++; } i0 = i; test0 = test1; } for (dgInt32 i = 0; i < count; i++) { dgFloat32 y = contactsOut[i].m_y; dgFloat32 z = contactsOut[i].m_z; contactsOut[i].m_y = y * cosAng - z * sinAng; contactsOut[i].m_z = z * cosAng + y * sinAng; } } 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; }