From c9f0b0cf93ea1b7cbb11b505058bbf8ee9145d5a Mon Sep 17 00:00:00 2001 From: Irlan <-> Date: Sun, 29 Apr 2018 22:27:41 -0300 Subject: [PATCH] moved SAT code into collision module --- include/bounce/collision/sat/sat.h | 53 ++++++ .../dynamics/contacts/collide/collide.h | 39 ---- src/bounce/collision/sat/sat.cpp | 105 +++++++++++ .../contacts/collide/collide_hulls.cpp | 2 +- .../contacts/collide/collide_hulls_cache.cpp | 171 +++--------------- 5 files changed, 183 insertions(+), 187 deletions(-) diff --git a/include/bounce/collision/sat/sat.h b/include/bounce/collision/sat/sat.h index 5066ba7..8f45ee2 100644 --- a/include/bounce/collision/sat/sat.h +++ b/include/bounce/collision/sat/sat.h @@ -52,4 +52,57 @@ float32 b3Project(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3 b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xf1, const b3Hull* hull1, const b3Transform& xf2, const b3Hull* hull2); + +/////////////////////////////////////////////////////////////////////////////////////////////////// + +enum b3SATCacheType +{ + e_separation, + e_overlap, + e_empty, +}; + +enum b3SATFeatureType +{ + e_edge1, // an edge on hull 1 and an edge on hull 2 + e_face1, // a face on hull 1 and a vertex/edge/face on hull 2 + e_face2, // a face on hull 2 and a vertex/edge/face on hull 1 +}; + +// A cached feature pair is used to improve the performance +// of the SAT when called more than once. +struct b3SATFeaturePair +{ + b3SATCacheType state; // sat result + b3SATFeatureType type; // feature pair type + u32 index1; // feature index on hull 1 + u32 index2; // feature index on hull 2 +}; + +inline b3SATFeaturePair b3MakeFeaturePair(b3SATCacheType state, b3SATFeatureType type, u32 index1, u32 index2) +{ + b3SATFeaturePair pair; + pair.state = state; + pair.type = type; + pair.index1 = index1; + pair.index2 = index2; + return pair; +} + +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, const b3Hull* hull1, + const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius); + + b3SATCacheType ReadEdge(const b3Transform& xf1, const b3Hull* hull1, + const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius); + + b3SATCacheType ReadFace(const b3Transform& xf1, const b3Hull* hull1, + const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius); + + b3SATFeaturePair m_featurePair; +}; + #endif diff --git a/include/bounce/dynamics/contacts/collide/collide.h b/include/bounce/dynamics/contacts/collide/collide.h index 2664c69..584e437 100644 --- a/include/bounce/dynamics/contacts/collide/collide.h +++ b/include/bounce/dynamics/contacts/collide/collide.h @@ -33,45 +33,6 @@ class b3MeshShape; struct b3Manifold; -enum b3SATCacheType -{ - e_separation, - e_overlap, - e_empty, -}; - -struct b3SATFeaturePair -{ - enum Type - { - e_edge1, // an edge on hull 1 and an edge on hull 2 - e_face1, // a face on hull 1 and a vertex/edge/face on hull 2 - e_face2, // a face on hull 2 and a vertex/edge/face on hull 1 - }; - - b3SATCacheType state; // sat result - Type type; // feature pair type - u32 index1; // feature index on hull 1 - u32 index2; // feature index on hull 2 -}; - -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, const b3Hull* hull1, - const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius); - - b3SATCacheType ReadEdge(const b3Transform& xf1, const b3Hull* hull1, - const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius); - - 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; -}; - // A convex cache contains information used to exploit temporal // coherence of the contact generation algorithms between two shapes. struct b3ConvexCache diff --git a/src/bounce/collision/sat/sat.cpp b/src/bounce/collision/sat/sat.cpp index 0985189..565cabf 100644 --- a/src/bounce/collision/sat/sat.cpp +++ b/src/bounce/collision/sat/sat.cpp @@ -171,3 +171,108 @@ b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xf1, const b3Hull* hull1, out.separation = maxSeparation; return out; } + +/////////////////////////////////////////////////////////////////////////////////////////////////// + +b3SATCacheType b3FeatureCache::ReadState( + 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) + { + m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_face1, 0, 0); + } + + switch (m_featurePair.type) + { + case b3SATFeatureType::e_edge1: + { + return ReadEdge(xf1, hull1, xf2, hull2, totalRadius); + } + case b3SATFeatureType::e_face1: + { + return ReadFace(xf1, hull1, xf2, hull2, totalRadius); + } + case b3SATFeatureType::e_face2: + { + return ReadFace(xf2, hull2, xf1, hull1, totalRadius); + } + default: + { + return b3SATCacheType::e_empty; + } + } +} + +b3SATCacheType b3FeatureCache::ReadFace( + 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 > totalRadius) + { + return e_separation; + } + return e_overlap; +} + +b3SATCacheType b3FeatureCache::ReadEdge( + const b3Transform& xf1, const b3Hull* hull1, + const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius) +{ + u32 i = m_featurePair.index1; + u32 j = m_featurePair.index2; + + // Query minimum separation distance and axis of the first hull planes. + // Perform computations in the local space of the second hull. + b3Transform xf = b3MulT(xf2, xf1); + b3Vec3 C1 = xf * hull1->centroid; + + const b3HalfEdge* edge1 = hull1->GetEdge(i); + const b3HalfEdge* twin1 = hull1->GetEdge(i + 1); + + B3_ASSERT(edge1->twin == i + 1 && twin1->twin == i); + + b3Vec3 P1 = xf * hull1->GetVertex(edge1->origin); + b3Vec3 Q1 = xf * hull1->GetVertex(twin1->origin); + b3Vec3 E1 = Q1 - P1; + + // The Gauss Map of edge 1. + b3Vec3 U1 = xf.rotation * hull1->GetPlane(edge1->face).normal; + b3Vec3 V1 = xf.rotation * hull1->GetPlane(twin1->face).normal; + + const b3HalfEdge* edge2 = hull2->GetEdge(j); + const b3HalfEdge* twin2 = hull2->GetEdge(j + 1); + + B3_ASSERT(edge2->twin == j + 1 && twin2->twin == j); + + b3Vec3 P2 = hull2->GetVertex(edge2->origin); + b3Vec3 Q2 = hull2->GetVertex(twin2->origin); + b3Vec3 E2 = Q2 - P2; + + // The Gauss Map of edge 2. + b3Vec3 U2 = hull2->GetPlane(edge2->face).normal; + b3Vec3 V2 = hull2->GetPlane(twin2->face).normal; + + // Negate the Gauss Map 2 for account for the MD. + if (b3IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2)) + { + float32 separation = b3Project(P1, E1, P2, E2, C1); + if (separation > totalRadius) + { + return b3SATCacheType::e_separation; + } + else + { + return b3SATCacheType::e_overlap; + } + } + + // We can't determine the cache type + // therefore must run SAT. + return b3SATCacheType::e_empty; +} \ 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 67759bf..a6c30ae 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls.cpp @@ -236,7 +236,7 @@ void b3CollideHulls(b3Manifold& manifold, return; } - const float32 kTol = 0.1f * B3_LINEAR_SLOP; + const float32 kTol = 0.05f * B3_LINEAR_SLOP; if (edgeQuery.separation > b3Max(faceQuery1.separation, faceQuery2.separation) + kTol) { b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, s1, xf2, edgeQuery.index2, s2); diff --git a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp index d06f653..5ce7f9d 100644 --- a/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp @@ -139,7 +139,7 @@ static void b3RebuildFaceContact(b3Manifold& manifold, // The new relative rotation. // "q(2) - q(1)" b3Quat dq = b3Conjugate(q1) * q2; - + // Relative rotation between the new relative rotation and the old relative rotation. // "dq(2) - dq0(1)" b3Quat q = b3Conjugate(dq0) * dq; @@ -162,20 +162,17 @@ void b3CollideCache(b3Manifold& manifold, 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; b3FaceQuery faceQuery1 = b3QueryFaceSeparation(xf1, hull1, xf2, hull2); if (faceQuery1.separation > totalRadius) { // Write a separation cache. - cache->m_featurePair.state = b3SATCacheType::e_separation; - cache->m_featurePair.type = b3SATFeaturePair::e_face1; - cache->m_featurePair.index1 = faceQuery1.index; - cache->m_featurePair.index2 = faceQuery1.index; + cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_face1, faceQuery1.index, faceQuery1.index); return; } @@ -183,10 +180,7 @@ void b3CollideCache(b3Manifold& manifold, if (faceQuery2.separation > totalRadius) { // Write a separation cache. - cache->m_featurePair.state = b3SATCacheType::e_separation; - cache->m_featurePair.type = b3SATFeaturePair::e_face2; - cache->m_featurePair.index1 = faceQuery2.index; - cache->m_featurePair.index2 = faceQuery2.index; + cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_face2, faceQuery2.index, faceQuery2.index); return; } @@ -194,25 +188,19 @@ void b3CollideCache(b3Manifold& manifold, if (edgeQuery.separation > totalRadius) { // Write a separation cache. - cache->m_featurePair.state = b3SATCacheType::e_separation; - cache->m_featurePair.type = b3SATFeaturePair::e_edge1; - cache->m_featurePair.index1 = edgeQuery.index1; - cache->m_featurePair.index2 = edgeQuery.index2; + cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_separation, b3SATFeatureType::e_edge1, edgeQuery.index1, edgeQuery.index2); return; } - const float32 kTol = 0.1f * B3_LINEAR_SLOP; + const float32 kTol = 0.05f * B3_LINEAR_SLOP; if (edgeQuery.separation > b3Max(faceQuery1.separation, faceQuery2.separation) + kTol) { b3BuildEdgeContact(manifold, xf1, edgeQuery.index1, s1, xf2, edgeQuery.index2, s2); - if(manifold.pointCount > 0) - { - // Write an overlap cache. - cache->m_featurePair.state = b3SATCacheType::e_overlap; - cache->m_featurePair.type = b3SATFeaturePair::e_edge1; - cache->m_featurePair.index1 = edgeQuery.index1; - cache->m_featurePair.index2 = edgeQuery.index2; + if (manifold.pointCount > 0) + { + // Write an overlap cache. + cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_overlap, b3SATFeatureType::e_edge1, edgeQuery.index1, edgeQuery.index2); } } else @@ -220,136 +208,24 @@ void b3CollideCache(b3Manifold& manifold, if (faceQuery1.separation + kTol > faceQuery2.separation) { b3BuildFaceContact(manifold, xf1, faceQuery1.index, s1, xf2, s2, false); - if(manifold.pointCount > 0) + if (manifold.pointCount > 0) { // Write an overlap cache. - cache->m_featurePair.state = b3SATCacheType::e_overlap; - cache->m_featurePair.type = b3SATFeaturePair::e_face1; - cache->m_featurePair.index1 = faceQuery1.index; - cache->m_featurePair.index2 = faceQuery1.index; + cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_overlap, b3SATFeatureType::e_face1, faceQuery1.index, faceQuery1.index); } } else { b3BuildFaceContact(manifold, xf2, faceQuery2.index, s2, xf1, s1, true); - if(manifold.pointCount > 0) + if (manifold.pointCount > 0) { // Write an overlap cache. - cache->m_featurePair.state = b3SATCacheType::e_overlap; - cache->m_featurePair.type = b3SATFeaturePair::e_face2; - cache->m_featurePair.index1 = faceQuery2.index; - cache->m_featurePair.index2 = faceQuery2.index; + cache->m_featurePair = b3MakeFeaturePair(b3SATCacheType::e_overlap, b3SATFeatureType::e_face2, faceQuery2.index, faceQuery2.index); } } } } -b3SATCacheType b3FeatureCache::ReadState( - 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) - { - m_featurePair.state = b3SATCacheType::e_separation; - m_featurePair.type = b3SATFeaturePair::e_face1; - m_featurePair.index1 = 0; - m_featurePair.index2 = 0; - } - - switch (m_featurePair.type) - { - case b3SATFeaturePair::e_edge1: - { - return ReadEdge(xf1, hull1, xf2, hull2, totalRadius); - } - case b3SATFeaturePair::e_face1: - { - return ReadFace(xf1, hull1, xf2, hull2, totalRadius); - } - case b3SATFeaturePair::e_face2: - { - return ReadFace(xf2, hull2, xf1, hull1, totalRadius); - } - default: - { - return b3SATCacheType::e_empty; - } - } -} - -b3SATCacheType b3FeatureCache::ReadFace( - 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 > totalRadius) - { - return e_separation; - } - return e_overlap; -} - -b3SATCacheType b3FeatureCache::ReadEdge( - const b3Transform& xf1, const b3Hull* hull1, - const b3Transform& xf2, const b3Hull* hull2, float32 totalRadius) -{ - u32 i = m_featurePair.index1; - u32 j = m_featurePair.index2; - - // Query minimum separation distance and axis of the first hull planes. - // Perform computations in the local space of the second hull. - b3Transform xf = b3MulT(xf2, xf1); - b3Vec3 C1 = xf * hull1->centroid; - - const b3HalfEdge* edge1 = hull1->GetEdge(i); - const b3HalfEdge* twin1 = hull1->GetEdge(i + 1); - - B3_ASSERT(edge1->twin == i + 1 && twin1->twin == i); - - b3Vec3 P1 = xf * hull1->GetVertex(edge1->origin); - b3Vec3 Q1 = xf * hull1->GetVertex(twin1->origin); - b3Vec3 E1 = Q1 - P1; - - // The Gauss Map of edge 1. - b3Vec3 U1 = xf.rotation * hull1->GetPlane(edge1->face).normal; - b3Vec3 V1 = xf.rotation * hull1->GetPlane(twin1->face).normal; - - const b3HalfEdge* edge2 = hull2->GetEdge(j); - const b3HalfEdge* twin2 = hull2->GetEdge(j + 1); - - B3_ASSERT(edge2->twin == j + 1 && twin2->twin == j); - - b3Vec3 P2 = hull2->GetVertex(edge2->origin); - b3Vec3 Q2 = hull2->GetVertex(twin2->origin); - b3Vec3 E2 = Q2 - P2; - - // The Gauss Map of edge 2. - b3Vec3 U2 = hull2->GetPlane(edge2->face).normal; - b3Vec3 V2 = hull2->GetPlane(twin2->face).normal; - - // Negate the Gauss Map 2 for account for the MD. - if (b3IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2)) - { - float32 separation = b3Project(P1, E1, P2, E2, C1); - if (separation > totalRadius) - { - return b3SATCacheType::e_separation; - } - else - { - return b3SATCacheType::e_overlap; - } - } - - // We can't determine the cache type - // therefore must run SAT. - return b3SATCacheType::e_empty; -} - extern u32 b3_convexCacheHits; void b3CollideHulls(b3Manifold& manifold, @@ -359,16 +235,16 @@ void b3CollideHulls(b3Manifold& manifold, { 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; // Read cache b3SATCacheType state0 = cache->m_featurePair.state; b3SATCacheType state1 = cache->ReadState(xf1, hull1, xf2, hull2, totalRadius); - + if (state0 == b3SATCacheType::e_separation && state1 == b3SATCacheType::e_separation) { @@ -376,23 +252,24 @@ void b3CollideHulls(b3Manifold& manifold, ++b3_convexCacheHits; return; } - else if (state0 == b3SATCacheType::e_overlap && - state1 == b3SATCacheType::e_overlap) + + if (state0 == b3SATCacheType::e_overlap && + state1 == b3SATCacheType::e_overlap) { // Try to rebuild or reclip the features. switch (cache->m_featurePair.type) { - case b3SATFeaturePair::e_edge1: + case b3SATFeatureType::e_edge1: { b3RebuildEdgeContact(manifold, xf1, cache->m_featurePair.index1, s1, xf2, cache->m_featurePair.index2, s2); break; } - case b3SATFeaturePair::e_face1: + case b3SATFeatureType::e_face1: { b3RebuildFaceContact(manifold, xf1, cache->m_featurePair.index1, s1, xf2, s2, false); break; } - case b3SATFeaturePair::e_face2: + case b3SATFeatureType::e_face2: { b3RebuildFaceContact(manifold, xf2, cache->m_featurePair.index1, s2, xf1, s1, true); break;