diff --git a/include/bounce/dynamics/contacts/collide/collide.h b/include/bounce/dynamics/contacts/collide/collide.h index 901ce63..2026b05 100644 --- a/include/bounce/dynamics/contacts/collide/collide.h +++ b/include/bounce/dynamics/contacts/collide/collide.h @@ -59,14 +59,14 @@ struct b3FeatureCache { // Read the current state of the cache. // Return e_unkown if neither a separation or penetration was detected. - b3SATCacheType ReadState(const b3Transform& xf1, float32 r1, const b3Hull* hull1, - const b3Transform& xf2, float32 r2, const b3Hull* hull2); + b3SATCacheType ReadState(const b3Transform& xf1, const b3Hull* hull1, + const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius); - b3SATCacheType ReadEdge(const b3Transform& xf1, float32 r1, const b3Hull* hull1, - const b3Transform& xf2, float32 r2, const b3Hull* hull2); + b3SATCacheType ReadEdge(const b3Transform& xf1, const b3Hull* hull1, + const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius); - b3SATCacheType ReadFace(const b3Transform& xf1, float32 r1, const b3Hull* hull1, - const b3Transform& xf2, float32 r2, const b3Hull* hull2); + b3SATCacheType ReadFace(const b3Transform& xf1, const b3Hull* hull1, + const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius); // We could increase the cache size (e.g. a feature pair of the last two frames). b3SATFeaturePair m_featurePair; diff --git a/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp b/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp index 2013c01..dfd09b5 100644 --- a/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp @@ -27,16 +27,17 @@ #include static void b3BuildEdgeContact(b3Manifold& manifold, - const b3Transform& xf1, const b3Segment* hull1, - const b3Transform& xf2, u32 index2, const b3Hull* hull2) + const b3Transform& xf1, const b3CapsuleShape* s1, + const b3Transform& xf2, u32 index2, const b3HullShape* s2) { - b3Vec3 P1 = xf1 * hull1->GetVertex(0); - b3Vec3 Q1 = xf1 * hull1->GetVertex(1); + b3Vec3 P1 = xf1 * s1->m_centers[0]; + b3Vec3 Q1 = xf1 * s1->m_centers[1]; b3Vec3 E1 = Q1 - P1; b3Vec3 N1 = E1; float32 L1 = N1.Normalize(); B3_ASSERT(L1 > 0.0f); + const b3Hull* hull2 = s2->m_hull; const b3HalfEdge* edge2 = hull2->GetEdge(index2); const b3HalfEdge* twin2 = hull2->GetEdge(index2 + 1); @@ -88,16 +89,20 @@ static void b3BuildEdgeContact(b3Manifold& manifold, } static void b3BuildFaceContact(b3Manifold& manifold, - const b3Transform& xf1, float32 r1, const b3Segment* hull1, - const b3Transform& xf2, float32 r2, u32 index2, const b3Hull* hull2) + const b3Transform& xf1, const b3CapsuleShape* s1, + const b3Transform& xf2, u32 index2, const b3HullShape* s2) { // Clip edge 1 against the side planes of the face 2. - b3Segment tempEdge1; - tempEdge1.vertices[0] = xf1 * hull1->vertices[0]; - tempEdge1.vertices[1] = xf1 * hull1->vertices[1]; - + b3Segment segment1; + segment1.vertices[0] = xf1 * s1->m_centers[0]; + segment1.vertices[1] = xf1 * s1->m_centers[1]; + float32 r1 = s1->m_radius; + b3ClipVertex edge1[2]; - b3BuildEdge(edge1, &tempEdge1); + b3BuildEdge(edge1, &segment1); + + const b3Hull* hull2 = s2->m_hull; + float32 r2 = s2->m_radius; b3ClipVertex clipEdge1[2]; u32 clipCount = b3ClipEdgeToFace(clipEdge1, edge1, xf2, r2, index2, hull2); @@ -109,6 +114,7 @@ static void b3BuildFaceContact(b3Manifold& manifold, const b3HalfEdge* edge2 = hull2->GetEdge(face2->edge); b3Vec3 localPoint2 = hull2->GetVertex(edge2->origin); + // Ensure normal orientation to hull 2. b3Vec3 n1 = -plane2.normal; float32 totalRadius = r1 + r2; @@ -142,10 +148,11 @@ void b3CollideCapsuleAndHull(b3Manifold& manifold, { b3ShapeGJKProxy proxy1(s1, 0); b3ShapeGJKProxy proxy2(s2, 0); - + b3GJKOutput gjk = b3GJK(xf1, proxy1, xf2, proxy2); - - float32 r1 = s1->m_radius, r2 = s2->m_radius; + + float32 r1 = s1->m_radius; + float32 r2 = s2->m_radius; float32 totalRadius = r1 + r2; @@ -179,7 +186,7 @@ void b3CollideCapsuleAndHull(b3Manifold& manifold, { // Reference face found. // Try to build a face contact. - b3BuildFaceContact(manifold, xf1, r1, &hull1, xf2, r2, index2, hull2); + b3BuildFaceContact(manifold, xf1, s1, xf2, index2, s2); if (manifold.pointCount == 2) { return; @@ -211,10 +218,10 @@ void b3CollideCapsuleAndHull(b3Manifold& manifold, const float32 kTol = 0.1f * B3_LINEAR_SLOP; if (edgeQuery.separation > faceQuery2.separation + kTol) { - b3BuildEdgeContact(manifold, xf1, &hull1, xf2, edgeQuery.index2, hull2); + b3BuildEdgeContact(manifold, xf1, s1, xf2, edgeQuery.index2, s2); } else { - b3BuildFaceContact(manifold, xf1, r1, &hull1, xf2, r2, faceQuery2.index, hull2); + b3BuildFaceContact(manifold, xf1, s1, xf2, faceQuery2.index, s2); } } \ No newline at end of file diff --git a/src/bounce/dynamics/contacts/collide/collide_hulls.cpp b/src/bounce/dynamics/contacts/collide/collide_hulls.cpp index 0ea2f51..6b59880 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls.cpp @@ -24,9 +24,10 @@ #include void b3BuildEdgeContact(b3Manifold& manifold, - const b3Transform& xf1, u32 index1, const b3Hull* hull1, - const b3Transform& xf2, u32 index2, const b3Hull* hull2) + const b3Transform& xf1, u32 index1, const b3HullShape* s1, + const b3Transform& xf2, u32 index2, const b3HullShape* s2) { + const b3Hull* hull1 = s1->m_hull; const b3HalfEdge* edge1 = hull1->GetEdge(index1); const b3HalfEdge* twin1 = hull1->GetEdge(index1 + 1); @@ -38,6 +39,7 @@ void b3BuildEdgeContact(b3Manifold& manifold, float32 L1 = N1.Normalize(); B3_ASSERT(L1 > B3_LINEAR_SLOP); + const b3Hull* hull2 = s2->m_hull; const b3HalfEdge* edge2 = hull2->GetEdge(index2); const b3HalfEdge* twin2 = hull2->GetEdge(index2 + 1); @@ -70,6 +72,7 @@ void b3BuildEdgeContact(b3Manifold& manifold, b3Vec3 c1 = P1 + s * N1; b3Vec3 c2 = P2 + t * N2; + // Ensure normal orientation to hull 2. b3Vec3 N = b3Cross(E1, E2); float32 LN = N.Normalize(); B3_ASSERT(LN > 0.0f); @@ -89,10 +92,18 @@ void b3BuildEdgeContact(b3Manifold& manifold, } void b3BuildFaceContact(b3Manifold& manifold, - const b3Transform& xf1, float32 r1, u32 index1, const b3Hull* hull1, - const b3Transform& xf2, float32 r2, const b3Hull* hull2, + const b3Transform& xf1, u32 index1, const b3HullShape* s1, + const b3Transform& xf2, const b3HullShape* s2, bool flipNormal) { + const b3Hull* hull1 = s1->m_hull; + float32 r1 = s1->m_radius; + + const b3Hull* hull2 = s2->m_hull; + float32 r2 = s2->m_radius; + + float32 totalRadius = r1 + r2; + // 1. Define the reference face plane (1). const b3Face* face1 = hull1->GetFace(index1); const b3HalfEdge* edge1 = hull1->GetEdge(face1->edge); @@ -112,8 +123,6 @@ void b3BuildFaceContact(b3Manifold& manifold, b3BuildPolygon(polygon2, xf2, index2, hull2); // 3. Clip incident face polygon (2) against the reference face (1) side planes. - float32 totalRadius = r1 + r2; - b3StackArray clipPolygon2; b3ClipPolygonToFace(clipPolygon2, polygon2, xf1, totalRadius, index1, hull1); if (clipPolygon2.IsEmpty()) @@ -155,6 +164,8 @@ void b3BuildFaceContact(b3Manifold& manifold, // 5. Reduce. b3Vec3 normal = plane1.normal; + + // Ensure normal orientation to hull 2. b3Vec3 s_normal = flipNormal ? -normal : normal; b3StackArray reducedPolygon1; @@ -173,6 +184,7 @@ void b3BuildFaceContact(b3Manifold& manifold, if (flipNormal) { + // Swap the feature pairs. b3FeaturePair pair = b3MakePair(v2.pair.inEdge2, v2.pair.inEdge1, v2.pair.outEdge2, v2.pair.outEdge1); mp->localNormal1 = b3MulT(xf2.rotation, s_normal); @@ -199,10 +211,11 @@ void b3CollideHulls(b3Manifold& manifold, const b3Transform& xf2, const b3HullShape* s2) { const b3Hull* hull1 = s1->m_hull; - const b3Hull* hull2 = s2->m_hull; - float32 r1 = s1->m_radius; + + const b3Hull* hull2 = s2->m_hull; float32 r2 = s2->m_radius; + float32 totalRadius = r1 + r2; b3FaceQuery faceQuery1 = b3QueryFaceSeparation(xf1, hull1, xf2, hull2); @@ -226,17 +239,17 @@ void b3CollideHulls(b3Manifold& manifold, const float32 kTol = 0.1f * B3_LINEAR_SLOP; if (edgeQuery.separation > b3Max(faceQuery1.separation, faceQuery2.separation) + kTol) { - b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, hull1, xf2, edgeQuery.index2, hull2); + b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, s1, xf2, edgeQuery.index2, s2); } else { if (faceQuery1.separation + kTol > faceQuery2.separation) { - b3BuildFaceContact(manifold, xf1, r1, faceQuery1.index, hull1, xf2, r2, hull2, false); + b3BuildFaceContact(manifold, xf1, faceQuery1.index, s1, xf2, s2, false); } else { - b3BuildFaceContact(manifold, xf2, r2, faceQuery2.index, hull2, xf1, r1, hull1, true); + b3BuildFaceContact(manifold, xf2, faceQuery2.index, s2, xf1, s1, true); } } } diff --git a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp index 01dbfdd..19f5402 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp @@ -24,12 +24,12 @@ #include void b3BuildEdgeContact(b3Manifold& manifold, - const b3Transform& xf1, u32 index1, const b3Hull* hull1, - const b3Transform& xf2, u32 index2, const b3Hull* hull2); + const b3Transform& xf1, u32 index1, const b3HullShape* s1, + const b3Transform& xf2, u32 index2, const b3HullShape* s2); void b3BuildFaceContact(b3Manifold& manifold, - const b3Transform& xf1, float32 r1, u32 index1, const b3Hull* hull1, - const b3Transform& xf2, float32 r2, const b3Hull* hull2, + const b3Transform& xf1, u32 index1, const b3HullShape* s1, + const b3Transform& xf2, const b3HullShape* s2, bool flipNormal); static void b3RebuildEdgeContact(b3Manifold& manifold, @@ -37,8 +37,6 @@ static void b3RebuildEdgeContact(b3Manifold& manifold, const b3Transform& xf2, u32 index2, const b3HullShape* s2) { const b3Hull* hull1 = s1->m_hull; - const b3Hull* hull2 = s2->m_hull; - const b3HalfEdge* edge1 = hull1->GetEdge(index1); const b3HalfEdge* twin1 = hull1->GetEdge(index1 + 1); @@ -50,6 +48,7 @@ static void b3RebuildEdgeContact(b3Manifold& manifold, float32 L1 = N1.Normalize(); B3_ASSERT(L1 > 0.0f); + const b3Hull* hull2 = s2->m_hull; const b3HalfEdge* edge2 = hull2->GetEdge(index2); const b3HalfEdge* twin2 = hull2->GetEdge(index2 + 1); @@ -155,7 +154,7 @@ static void b3RebuildFaceContact(b3Manifold& manifold, const float32 kTol = 0.995f; if (b3Abs(q.w) > kTol) { - b3BuildFaceContact(manifold, xf1, r1, index1, hull1, xf2, r2, hull2, flipNormal); + b3BuildFaceContact(manifold, xf1, index1, s1, xf2, s2, flipNormal); } } @@ -167,10 +166,11 @@ void b3CollideCache(b3Manifold& manifold, B3_ASSERT(cache->m_featurePair.state == b3SATCacheType::e_empty); const b3Hull* hull1 = s1->m_hull; - const b3Hull* hull2 = s2->m_hull; - float32 r1 = s1->m_radius; + + const b3Hull* hull2 = s2->m_hull; float32 r2 = s2->m_radius; + float32 totalRadius = r1 + r2; b3FaceQuery faceQuery1 = b3QueryFaceSeparation(xf1, hull1, xf2, hull2); @@ -210,7 +210,7 @@ void b3CollideCache(b3Manifold& manifold, if (edgeQuery.separation > b3Max(faceQuery1.separation, faceQuery2.separation) + kTol) { - b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, hull1, xf2, edgeQuery.index2, hull2); + b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, s1, xf2, edgeQuery.index2, s2); if(manifold.pointCount > 0) { // Write an overlap cache. @@ -224,7 +224,7 @@ void b3CollideCache(b3Manifold& manifold, { if (faceQuery1.separation + kTol > faceQuery2.separation) { - b3BuildFaceContact(manifold, xf1, r1, faceQuery1.index, hull1, xf2, r2, hull2, false); + b3BuildFaceContact(manifold, xf1, faceQuery1.index, s1, xf2, s2, false); if(manifold.pointCount > 0) { // Write an overlap cache. @@ -236,7 +236,7 @@ void b3CollideCache(b3Manifold& manifold, } else { - b3BuildFaceContact(manifold, xf2, r2, faceQuery2.index, hull2, xf1, r1, hull1, true); + b3BuildFaceContact(manifold, xf2, faceQuery2.index, s2, xf1, s1, true); if(manifold.pointCount > 0) { // Write an overlap cache. @@ -250,8 +250,8 @@ void b3CollideCache(b3Manifold& manifold, } b3SATCacheType b3FeatureCache::ReadState( - const b3Transform& xf1, float32 r1, const b3Hull* hull1, - const b3Transform& xf2, float32 r2, const b3Hull* hull2) + const b3Transform& xf1, const b3Hull* hull1, + const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius) { // If the cache was empty or flushed choose an arbitrary feature pair. if (m_featurePair.state == b3SATCacheType::e_empty) @@ -266,15 +266,15 @@ b3SATCacheType b3FeatureCache::ReadState( { case b3SATFeaturePair::e_edge1: { - return ReadEdge(xf1, r1, hull1, xf2, r2, hull2); + return ReadEdge(xf1, hull1, xf2, hull2, totalRadius); } case b3SATFeaturePair::e_face1: { - return ReadFace(xf1, r1, hull1, xf2, r2, hull2); + return ReadFace(xf1, hull1, xf2, hull2, totalRadius); } case b3SATFeaturePair::e_face2: { - return ReadFace(xf2, r2, hull2, xf1, r1, hull1); + return ReadFace(xf2, hull2, xf1, hull1, totalRadius); } default: { @@ -284,14 +284,14 @@ b3SATCacheType b3FeatureCache::ReadState( } b3SATCacheType b3FeatureCache::ReadFace( - const b3Transform& xf1, float32 r1, const b3Hull* hull1, - const b3Transform& xf2, float32 r2, const b3Hull* hull2) + const b3Transform& xf1, const b3Hull* hull1, + const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius) { // Perform computations in the local space of the second hull. b3Transform xf = b3MulT(xf2, xf1); b3Plane plane = xf * hull1->GetPlane(m_featurePair.index1); float32 separation = b3Project(hull2, plane); - if (separation > r1 + r2) + if (separation > totalRadius) { return e_separation; } @@ -299,8 +299,8 @@ b3SATCacheType b3FeatureCache::ReadFace( } b3SATCacheType b3FeatureCache::ReadEdge( - const b3Transform& xf1, float32 r1, const b3Hull* hull1, - const b3Transform& xf2, float32 r2, const b3Hull* hull2) + const b3Transform& xf1, const b3Hull* hull1, + const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius) { u32 i = m_featurePair.index1; u32 j = m_featurePair.index2; @@ -340,7 +340,7 @@ b3SATCacheType b3FeatureCache::ReadEdge( if (b3IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2)) { float32 separation = b3Project(P1, E1, P2, E2, C1); - if (separation > r1 + r2) + if (separation > totalRadius) { return b3SATCacheType::e_separation; } @@ -368,9 +368,11 @@ void b3CollideHulls(b3Manifold& manifold, const b3Hull* hull2 = s2->m_hull; float32 r2 = s2->m_radius; + float32 totalRadius = r1 + r2; + // Read cache b3SATCacheType state0 = cache->m_featurePair.state; - b3SATCacheType state1 = cache->ReadState(xf1, r1, hull1, xf2, r2, hull2); + b3SATCacheType state1 = cache->ReadState(xf1, hull1, xf2, hull2, totalRadius); if (state0 == b3SATCacheType::e_separation && state1 == b3SATCacheType::e_separation)