From 3586c7ffdc675f7d5a244fc6716948fc729929ad Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Fri, 30 Dec 2022 21:43:22 +0800 Subject: [PATCH] Added mostly working sphere vs convex polyhedron collision detection --- Assets/Scenes/PhysicsSandbox.shade | 20 +-- .../Narrowphase/SHSphereVsConvex.cpp | 170 ++++++++++++++++++ .../Narrowphase/SHSphereVsSphere.cpp | 10 +- 3 files changed, 185 insertions(+), 15 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 4e5e40c2..cd74173e 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -4,7 +4,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0.0700113177, y: 2.5, z: 0} + Translate: {x: 0, y: 2.5, z: 0} Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true @@ -30,8 +30,8 @@ Colliders: - Is Trigger: false Collision Tag: 1 - Type: Sphere - Radius: 1 + Type: Box + Half Extents: {x: 1, y: 1, z: 1} Friction: 0.400000006 Bounciness: 0 Density: 1 @@ -49,9 +49,9 @@ NumberOfChildren: 0 Components: Camera Component: - Position: {x: 0, y: 0.5, z: 5} + Position: {x: 3, y: 4, z: 0} Pitch: 0 - Yaw: 0 + Yaw: 90 Roll: 0 Width: 1920 Height: 1080 @@ -107,8 +107,8 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0, y: 5, z: 0} - Rotate: {x: -0, y: 0, z: 0} + Translate: {x: 0, y: 5, z: 0.834425449} + Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true RigidBody Component: @@ -122,7 +122,7 @@ Interpolate: true Sleeping Enabled: true Freeze Position X: false - Freeze Position Y: false + Freeze Position Y: true Freeze Position Z: false Freeze Rotation X: false Freeze Rotation Y: false @@ -133,8 +133,8 @@ Colliders: - Is Trigger: false Collision Tag: 1 - Type: Box - Half Extents: {x: 1, y: 1, z: 1} + Type: Sphere + Radius: 1 Friction: 0.400000006 Bounciness: 0 Density: 1 diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index a8a9cd48..8f12475a 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -47,6 +47,176 @@ namespace SHADE bool SHCollision::SphereVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept { + // Convert to underlying types + // For the convex, we only need the convex polyhedron shape since the get vertex is pure virtual. + const SHSphereCollisionShape& SPHERE = dynamic_cast(A); + const SHConvexPolyhedronCollisionShape& CONVEX = dynamic_cast(B); + + // Ensure a gap between A & B + const float TOTAL_RADIUS = SPHERE.GetWorldRadius() + CONVEX.RADIUS; + + // Find closest face of polygon to circle + + int32_t closestFaceIndex = -1; + int32_t closestPointIndex = -1; + float bestDistance = std::numeric_limits::lowest(); + + const SHHalfEdgeDS* HALF_EDGE_STRUCTURE = CONVEX.GetHalfEdgeStructure(); + + /* + * Test against each face + * + * TODO: + * This check is now O(n^2) because we find the closest point. + * It can be optimised to O(n) by utilising the following steps: + * 1. Rotate sphere into polyhedron's space + * 2. Build a plane equation from the face in point-normal form. We need the plane's offset from the origin. + * 3. Compute distance to the face. + */ + for (int32_t i = 0; i < HALF_EDGE_STRUCTURE->GetFaceCount(); ++i) + { + const SHHalfEdgeDS::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(i); + + // TODO: Remove and optimise + // Find the closest point on the face to the sphere + const int32_t NUM_VERTICES = static_cast(FACE.vertexIndices.size()); + for (int32_t j = 0; j < NUM_VERTICES; ++j) + { + // Get vector from center to a vertex on the face + const SHVec3 A_TO_B = SPHERE.GetCenter() - CONVEX.GetVertex(FACE.vertexIndices[j]); + + const float PROJECTION = SHVec3::Dot(A_TO_B, FACE.normal); + + // Early out + if (PROJECTION > TOTAL_RADIUS) + return false; + + if (PROJECTION > bestDistance) + { + bestDistance = PROJECTION; + closestFaceIndex = i; + closestPointIndex = j; + } + } + } + + uint32_t numContacts = 0; + const float penetration = TOTAL_RADIUS - bestDistance; + + // Rotate the normal into the world space + const SHVec3& BEST_NORMAL = CONVEX.GetNormal(closestFaceIndex); + + // Check if center is inside polyhedron (below the face) + if (bestDistance < SHMath::EPSILON) + { + SHContact newContact; + newContact.penetration = penetration; + newContact.position = SPHERE.GetCenter(); + newContact.featurePair.key = 0; + + manifold.contacts[numContacts++] = newContact; + manifold.normal = BEST_NORMAL; + + manifold.numContacts = numContacts; + return true; + } + + // Check against voronoi regions of the face to determine the type of the intersection test + // We have 3 voronoi regions to check: cp -> prev, cp -> next and cp -> center + // If none of these are true, the sphere is above the face but not separating + + const SHHalfEdgeDS::Face& CLOSEST_FACE = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex); + const int32_t NUM_VERTICES_ON_FACE = static_cast(CLOSEST_FACE.vertexIndices.size()); + + const SHVec3& CLOSEST_POINT = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[closestPointIndex]); + const SHVec3 CP_TO_CENTER = SPHERE.GetCenter() - CLOSEST_POINT; + + // Check closest point -> prev point + { + const int32_t PREV_POINT_INDEX = closestPointIndex == 0 ? NUM_VERTICES_ON_FACE - 1 : closestPointIndex - 1; + const SHVec3& PREV_POINT = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[PREV_POINT_INDEX]); + + const SHVec3 CP_TO_PREV = SHVec3::Normalise(PREV_POINT - CLOSEST_POINT); + + float projection = SHVec3::Dot(CP_TO_CENTER, CP_TO_PREV); + if (projection >= 0.0f) + { + // Sphere is inside this region, check if distance from center is lesser than radius + if (penetration >= TOTAL_RADIUS) + return false; + + SHContact newContact; + newContact.penetration = penetration; + newContact.position = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS; + newContact.featurePair.key = 0; + + manifold.contacts[numContacts++] = newContact; + manifold.normal = BEST_NORMAL; + + manifold.numContacts = numContacts; + return true; + } + } + + // Check closest point -> next point + { + const int32_t NEXT_POINT_INDEX = closestPointIndex + 1 % NUM_VERTICES_ON_FACE; + const SHVec3& NEXT_POINT = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[NEXT_POINT_INDEX]); + + const SHVec3 CP_TO_NEXT = SHVec3::Normalise(NEXT_POINT - CLOSEST_POINT); + + float projection = SHVec3::Dot(CP_TO_CENTER, CP_TO_NEXT); + if (projection >= 0.0f) + { + // Sphere is inside this region, check if distance from center is lesser than radius + if (penetration >= TOTAL_RADIUS) + return false; + + SHContact newContact; + newContact.penetration = penetration; + newContact.position = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS; + newContact.featurePair.key = 0; + + manifold.contacts[numContacts++] = newContact; + manifold.normal = BEST_NORMAL; + + manifold.numContacts = numContacts; + return true; + } + } + + // Check if it hit the closest point + { + if (CP_TO_CENTER.LengthSquared() < TOTAL_RADIUS * TOTAL_RADIUS) + { + SHContact newContact; + newContact.penetration = penetration; + newContact.position = CLOSEST_POINT; + newContact.featurePair.key = 0; + + manifold.contacts[numContacts++] = newContact; + manifold.normal = SHVec3::Normalise(CP_TO_CENTER); + + manifold.numContacts = numContacts; + return true; + } + } + + // It is above the closest face + if (penetration <= TOTAL_RADIUS) + { + SHContact newContact; + newContact.penetration = penetration; + newContact.position = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS; + newContact.featurePair.key = 0; + + manifold.contacts[numContacts++] = newContact; + manifold.normal = BEST_NORMAL; + + manifold.numContacts = numContacts; + return true; + } + return false; } diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp index 8b529241..07b6c9a6 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp @@ -25,17 +25,17 @@ namespace SHADE bool SHCollision::SphereVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - const SHSphereCollisionShape& SPHERE_A = reinterpret_cast(A); - const SHSphereCollisionShape& SPHERE_B = reinterpret_cast(B); + const SHSphereCollisionShape& SPHERE_A = dynamic_cast(A); + const SHSphereCollisionShape& SPHERE_B = dynamic_cast(B); return SHSphere::Intersect(SPHERE_A, SPHERE_B); } bool SHCollision::SphereVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - // Convert to spheres - const SHSphereCollisionShape& SPHERE_A = reinterpret_cast(A); - const SHSphereCollisionShape& SPHERE_B = reinterpret_cast(B); + // Convert to underlying types + const SHSphereCollisionShape& SPHERE_A = dynamic_cast(A); + const SHSphereCollisionShape& SPHERE_B = dynamic_cast(B); const SHVec3 A_TO_B = SPHERE_B.GetCenter() - SPHERE_A.GetCenter(); const float DISTANCE_BETWEEN_CENTERS_SQUARED = A_TO_B.LengthSquared();