2023-01-23 15:55:45 +08:00
3 changed files with 185 additions and 15 deletions
--- 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
--- 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<const SHSphereCollisionShape&>(A);
+    const SHConvexPolyhedronCollisionShape& CONVEX  = dynamic_cast<const SHConvexPolyhedronCollisionShape&>(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<float>::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<int32_t>(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<int32_t>(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;
  }

--- 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<const SHSphereCollisionShape&>(A);
-    const SHSphereCollisionShape& SPHERE_B = reinterpret_cast<const SHSphereCollisionShape&>(B);
+    const SHSphereCollisionShape& SPHERE_A = dynamic_cast<const SHSphereCollisionShape&>(A);
+    const SHSphereCollisionShape& SPHERE_B = dynamic_cast<const SHSphereCollisionShape&>(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<const SHSphereCollisionShape&>(A);
-    const SHSphereCollisionShape& SPHERE_B = reinterpret_cast<const SHSphereCollisionShape&>(B);
+    // Convert to underlying types
+    const SHSphereCollisionShape& SPHERE_A = dynamic_cast<const SHSphereCollisionShape&>(A);
+    const SHSphereCollisionShape& SPHERE_B = dynamic_cast<const SHSphereCollisionShape&>(B);

    const SHVec3  A_TO_B                            = SPHERE_B.GetCenter() - SPHERE_A.GetCenter();
    const float   DISTANCE_BETWEEN_CENTERS_SQUARED  = A_TO_B.LengthSquared();