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; }
dgVector dgCollisionCone::SupportVertexSpecial(const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert(dgAbsf((dir % dir - dgFloat32(1.0f))) < dgFloat32(1.0e-3f)); if (dir.m_x < dgFloat32(-0.9999f)) { return dgVector(-(m_height - D_CONE_SKIN_THINCKNESS), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); } else if (dir.m_x > dgFloat32(0.9999f)) { return dgVector(m_height - D_CONE_SKIN_THINCKNESS, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); } else { dgVector dir_yz(dir); dir_yz.m_x = dgFloat32(0.0f); dgFloat32 mag2 = dir_yz.DotProduct4(dir_yz).GetScalar(); dgAssert(mag2 > dgFloat32(0.0f)); dir_yz = dir_yz.Scale4(dgFloat32(1.0f) / dgSqrt(mag2)); dgVector p0(dir_yz.Scale4(m_radius - D_CONE_SKIN_THINCKNESS)); dgVector p1(dgVector::m_zero); p0.m_x = -(m_height - D_CONE_SKIN_THINCKNESS); p1.m_x = m_height - D_CONE_SKIN_THINCKNESS; dgFloat32 dist0 = dir.DotProduct4(p0).GetScalar(); dgFloat32 dist1 = dir.DotProduct4(p1).GetScalar(); if (dist1 >= dist0) { p0 = p1; } return p0; } }
dgVector dgCollisionCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f)); if (dir.m_x < dgFloat32(-0.9999f)) { return dgVector(-m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); } else if (dir.m_x > dgFloat32(0.9999f)) { return dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); } else { dgVector dir_yz (dir); dir_yz.m_x = dgFloat32 (0.0f); dgFloat32 mag2 = dir_yz.DotProduct4(dir_yz).GetScalar(); dgAssert (mag2 > dgFloat32 (0.0f)); dir_yz = dir_yz.Scale4 (dgFloat32 (1.0f) / dgSqrt (mag2)); dgVector p0 (dir_yz.Scale4 (m_radio0)); dgVector p1 (dir_yz.Scale4 (m_radio1)); p0.m_x = -m_height; p1.m_x = m_height; dgFloat32 dist0 = dir.DotProduct4(p0).GetScalar(); dgFloat32 dist1 = dir.DotProduct4(p1).GetScalar(); if (dist1 >= dist0) { p0 = p1; } return p0; } }
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; }
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; } }
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; }
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; }