Solved Sphere VS Convex in local space

Assets/Application.SHConfig

@@ -1,4 +1,4 @@
 Start in Fullscreen: false
-Starting Scene ID: 92914350
+Starting Scene ID: 97402985
 Window Size: {x: 1920, y: 1080}
 Window Title: SHADE Engine

Assets/Scenes/PhysicsSandbox.shade

@@ -28,9 +28,9 @@
   NumberOfChildren: 0
   Components:
     Camera Component:
-      Position: {x: 0, y: 2, z: 10}
+      Position: {x: 5, y: 5, z: 0}
       Pitch: 0
-      Yaw: 0
+      Yaw: 90
       Roll: 0
       Width: 1920
       Height: 1080
@@ -45,7 +45,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 2, y: 7.5, z: 0}
+      Translate: {x: 0, y: 7.5, z: 0.75}
       Rotate: {x: -0, y: 0, z: 0.785398185}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true
@@ -60,7 +60,7 @@
       Interpolate: true
       Sleeping Enabled: true
       Freeze Position X: false
-      Freeze Position Y: true
+      Freeze Position Y: false
       Freeze Position Z: false
       Freeze Rotation X: false
       Freeze Rotation Y: false
@@ -176,72 +176,3 @@
           Rotation Offset: {x: 0, y: 0, z: 0}
       IsActive: true
   Scripts: ~
-- EID: 7
-  Name: Default
-  IsActive: true
-  NumberOfChildren: 0
-  Components:
-    Transform Component:
-      Translate: {x: 0, y: 2, z: 3}
-      Rotate: {x: 0, y: 0.785398185, z: 0}
-      Scale: {x: 0.999988496, y: 0.999994099, z: 0.999984443}
-      IsActive: true
-    RigidBody Component:
-      Type: Dynamic
-      Auto Mass: false
-      Mass: 1
-      Drag: 0.00999999978
-      Angular Drag: 0
-      Use Gravity: true
-      Gravity Scale: 1
-      Interpolate: true
-      Sleeping Enabled: true
-      Freeze Position X: false
-      Freeze Position Y: false
-      Freeze Position Z: false
-      Freeze Rotation X: false
-      Freeze Rotation Y: false
-      Freeze Rotation Z: false
-      IsActive: true
-    Collider Component:
-      DrawColliders: false
-      Colliders:
-        - Is Trigger: false
-          Collision Tag: 1
-          Type: Box
-          Half Extents: {x: 1, y: 1, z: 1}
-          Friction: 0.400000006
-          Bounciness: 0
-          Density: 1
-          Position Offset: {x: 0, y: 0, z: 0}
-          Rotation Offset: {x: 0, y: 0, z: 0}
-      IsActive: true
-  Scripts:
-    - Type: PhysicsTestObj
-      Enabled: true
-      forceAmount: 200
-      torqueAmount: 5
-- EID: 8
-  Name: Default
-  IsActive: true
-  NumberOfChildren: 0
-  Components:
-    Transform Component:
-      Translate: {x: 0, y: 0, z: 3}
-      Rotate: {x: -0, y: 0, z: -0}
-      Scale: {x: 5, y: 1, z: 5}
-      IsActive: true
-    Collider Component:
-      DrawColliders: false
-      Colliders:
-        - Is Trigger: false
-          Collision Tag: 1
-          Type: Box
-          Half Extents: {x: 1, y: 1, z: 1}
-          Friction: 0.400000006
-          Bounciness: 0
-          Density: 1
-          Position Offset: {x: 0, y: 0, z: 0}
-          Rotation Offset: {x: 0, y: 0, z: 0}
-      IsActive: true
-  Scripts: ~

Assets/Scenes/SS_Playground.shade

@@ -4,7 +4,7 @@
   NumberOfChildren: 0
   Components:
     Camera Component:
-      Position: {x: 0, y: 2, z: 7}
+      Position: {x: 0, y: 1, z: 3}
       Pitch: 0
       Yaw: 0
       Roll: 0
@@ -21,17 +21,34 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0.5, y: 0, z: 0}
+      Translate: {x: 0, y: 0, z: 0}
       Rotate: {x: 0, y: 0, z: 0}
-      Scale: {x: 1, y: 1, z: 1}
+      Scale: {x: 2, y: 1, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: false
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: false
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
       IsActive: true
     Collider Component:
-      DrawColliders: true
+      DrawColliders: false
       Colliders:
         - Is Trigger: false
           Collision Tag: 1
-          Type: Sphere
+          Type: Box
-          Radius: 3
+          Half Extents: {x: 1, y: 1, z: 1}
           Friction: 0.400000006
           Bounciness: 0
           Density: 1
@@ -45,7 +62,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0, y: 3, z: 0}
+      Translate: {x: 1.14999998, y: 3, z: 0.550000012}
       Rotate: {x: 0, y: 0, z: 0}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true
@@ -56,7 +73,7 @@
       Drag: 0.00999999978
       Angular Drag: 0.00999999978
       Use Gravity: true
-      Gravity Scale: 1
+      Gravity Scale: 0.25
       Interpolate: true
       Sleeping Enabled: true
       Freeze Position X: false
@@ -67,7 +84,7 @@
       Freeze Rotation Z: false
       IsActive: true
     Collider Component:
-      DrawColliders: true
+      DrawColliders: false
       Colliders:
         - Is Trigger: false
           Collision Tag: 1

SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.cpp

@@ -28,10 +28,10 @@ namespace SHADE
   /* Public Member Functions Definitions                                               */
   /*-----------------------------------------------------------------------------------*/
 
-  void SHCollisionUtils::ShapeTransform::Invert() noexcept
+  SHCollisionUtils::ShapeTransform SHCollisionUtils::ShapeTransform::GetInverse() const noexcept
   {
-    orientation.Invert();
+    const SHQuaternion INV_ORIENTATION = SHQuaternion::Inverse(orientation);
-    position = SHVec3::Rotate(-position, orientation);
+    return ShapeTransform { SHVec3::Rotate(-position, INV_ORIENTATION), INV_ORIENTATION };
   }
 
 

SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.h

@@ -51,7 +51,7 @@ namespace SHADE
       /* Member Functions                                                              */
       /*-------------------------------------------------------- ----------------------*/
 
-      void Invert() noexcept;
+      ShapeTransform GetInverse() const noexcept;
     };
 
     struct FaceQuery

SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp

@@ -90,106 +90,109 @@ namespace SHADE
   {
     // Convert to underlying types
     // For the convex, we only need the convex polyhedron shape since the get vertex is pure virtual.
-    //const SHSphere&           SPHERE      = dynamic_cast<const SHSphere&>(A);
+    const SHSphere&           SPHERE  = dynamic_cast<const SHSphere&>(A);
-    //const SHConvexPolyhedron& POLYHEDRON  = dynamic_cast<const SHConvexPolyhedron&>(B);
+    const SHConvexPolyhedron& POLY    = dynamic_cast<const SHConvexPolyhedron&>(B);
 
-    //const SHVec3  CENTER  = SPHERE.Center;
+    const SHVec3  CENTER  = SPHERE.Center;
-    //const float   RADIUS  = SPHERE.GetWorldRadius();
+    const float   RADIUS  = SPHERE.GetWorldRadius();
 
-    //const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
+    const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLY);
-    //if (!FACE_QUERY.colliding)
+    if (!FACE_QUERY.colliding)
-    //  return false;
+      return false;
 
-    //uint32_t    numContacts   = 0;
+    const SHCollisionUtils::ShapeTransform TF_A = { SPHERE.GetWorldCentroid(), SPHERE.GetWorldOrientation() };
-    //const float PENETRATION   = RADIUS - FACE_QUERY.bestDistance;
+    const SHCollisionUtils::ShapeTransform TF_B = { POLY.GetWorldCentroid(), POLY.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse();
+    const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse();
 
-    //SHContact contact;
+    uint32_t    numContacts   = 0;
-    //contact.featurePair.key = 0;
+    const float PENETRATION   = RADIUS - FACE_QUERY.bestDistance;
 
-    //// Check if center is inside polyhedron (below the face)
+    SHContact contact;
-    //if (FACE_QUERY.bestDistance < SHMath::EPSILON)
+    contact.featurePair.key = 0;
-    //{
-    //  manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
 
-    //  contact.penetration = PENETRATION;
+    // Check if center is inside polyhedron (below the face)
-    //  contact.position    = CENTER;
+    if (FACE_QUERY.bestDistance < SHMath::EPSILON)
+    {
+      manifold.normal = -POLY.GetNormal(FACE_QUERY.closestFace);
 
-    //  manifold.contacts[numContacts++] = contact;
+      contact.penetration = PENETRATION;
-    //  manifold.numContacts = numContacts;
 
-    //  return true;
+      // A is the sphere, B is the polyhedron
-    //}
+      contact.localPointA = SHVec3::Rotate(manifold.normal * RADIUS, INV_TF_A.orientation);
+      contact.localPointB = (INV_TF_B * CENTER) + manifold.normal * (PENETRATION - RADIUS);
 
-    //// Find closest face of polygon to circle
+      manifold.contacts[numContacts++] = contact;
-    //const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace);
+      manifold.numContacts = numContacts;
 
-    //const auto&   FACE          = POLYHEDRON.GetFace(FACE_QUERY.closestFace);
+      return true;
-    //const SHVec3& FACE_NORMAL   = POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
+    }
-    //const int32_t NUM_VERTICES  = static_cast<int32_t>(FACE.vertexIndices.size());
 
-    //// Get points and build tangents
+    // Find closest face of polygon to circle
-    //const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES;
+    const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLY, FACE_QUERY.closestFace);
-    //const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1;
 
-    //const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index);
+    const auto&   FACE          = POLY.GetFace(FACE_QUERY.closestFace);
-    //const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index);
+    const SHVec3& FACE_NORMAL   = POLY.GetNormal(FACE_QUERY.closestFace);
-    //const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index);
+    const int32_t NUM_VERTICES  = static_cast<int32_t>(FACE.vertexIndices.size());
 
-    //const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1);
+    // Get points and build tangents
-    //const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1);
+    const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES;
+    const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1;
 
-    //// Get the voronoi region it belongs in
+    const SHVec3 P1 = POLY.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index);
-    //const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2);
+    const SHVec3 P2 = POLY.GetVertex(FACE.vertexIndices[P2_INDEX].index);
-    //if (REGION == 0)
+    const SHVec3 P3 = POLY.GetVertex(FACE.vertexIndices[P3_INDEX].index);
-    //  return false;
 
-    //// Create contact information based on region
+    const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1);
+    const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1);
 
-    //const SHVec3 P1_TO_CENTER = CENTER - P1;
+    // Get the voronoi region it belongs in
-    //switch (REGION)
+    const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2);
-    //{
+    if (REGION == 0)
-    //  case 1: // Region A
+      return false;
-    //  case 2: // Region B
-    //  {
-    //    // Find closest point
-    //    const SHVec3& TANGENT     = REGION == 1 ? TANGENT_1 : TANGENT_2;
-    //    const SHVec3 CP           = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT);
-    //    const SHVec3 CP_TO_CENTER = CENTER - CP;
 
-    //    manifold.normal     = -SHVec3::Normalise(CP_TO_CENTER);
+    // Create contact information based on region
 
-    //    contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
+    const SHVec3 P1_TO_CENTER = CENTER - P1;
-    //    contact.position    = CP;
+    switch (REGION)
+    {
+      case 1: // Region A
+      case 2: // Region B
+      {
+        // Find closest point
+        const SHVec3& TANGENT     = REGION == 1 ? TANGENT_1 : TANGENT_2;
+        const SHVec3 CP           = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT);
+        const SHVec3 CP_TO_CENTER = CENTER - CP;
 
-    //    break;
+        manifold.normal     = -SHVec3::Normalise(CP_TO_CENTER);
-    //  }
+        contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
-    //  case 3: // Region C
-    //  {
-    //    manifold.normal     = -SHVec3::Normalise(P1_TO_CENTER);
 
-    //    contact.penetration = RADIUS - P1_TO_CENTER.Length();
+        break;
-    //    contact.position    = P1;
+      }
+      case 3: // Region C
+      {
+        manifold.normal     = -SHVec3::Normalise(P1_TO_CENTER);
+        contact.penetration = RADIUS - P1_TO_CENTER.Length();
 
-    //    break;
+        break;
-    //  }
+      }
-    //  case 4: // Region D
+      case 4: // Region D
-    //  {
+      {
-    //    manifold.normal     = -FACE_NORMAL;
+        manifold.normal     = -FACE_NORMAL;
+        contact.penetration = PENETRATION;
 
-    //    contact.penetration = PENETRATION;
+        break;
-    //    contact.position    = CENTER - FACE_NORMAL * RADIUS;
+      }
+      default: return false;  // Should never happen
+    }
 
-    //    break;
+    contact.localPointA = SHVec3::Rotate(manifold.normal * RADIUS, INV_TF_A.orientation);
-    //  }
+    contact.localPointB = (INV_TF_B * CENTER) + manifold.normal * (RADIUS - contact.penetration);
-    //  default: return false;  // Should never happen
-    //}
 
-    //manifold.contacts[numContacts++] = contact;
+    manifold.contacts[numContacts++] = contact;
-    //manifold.numContacts = numContacts;
+    manifold.numContacts = numContacts;
 
-    //return true;
+    return true;
-
-    return false;
   }
 
   bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
@@ -197,6 +200,10 @@ namespace SHADE
     if (SphereVsConvex(manifold, B, A))
     {
       manifold.normal = -manifold.normal;
+
+      // There can only be one contact in this scenario, so we swap the first
+      std::swap(manifold.contacts[0].localPointA, manifold.contacts[0].localPointB);
+
       return true;
     };
 
@@ -244,8 +251,8 @@ namespace SHADE
 
       if (SIGNED_DIST > faceQuery.bestDistance)
       {
-        faceQuery.bestDistance = SIGNED_DIST;
+        faceQuery.bestDistance  = SIGNED_DIST;
-        faceQuery.closestFace = i;
+        faceQuery.closestFace   = i;
       }
     }
 

SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp

@@ -66,11 +66,10 @@ namespace SHADE
     if (DISTANCE_BETWEEN_CENTERS_SQUARED > COMBINED_RADIUS_SQUARED)
       return false;
 
-    SHCollisionUtils::ShapeTransform inverseTransformA = { SPHERE_A.GetWorldCentroid(), SPHERE_A.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform TF_A = { SPHERE_A.GetWorldCentroid(), SPHERE_A.GetWorldOrientation() };
-    SHCollisionUtils::ShapeTransform inverseTransformB = { SPHERE_B.GetWorldCentroid(), SPHERE_B.GetWorldOrientation() };
+    const SHCollisionUtils::ShapeTransform TF_B = { SPHERE_B.GetWorldCentroid(), SPHERE_B.GetWorldOrientation() };
+
     
-    inverseTransformA.Invert();
-    inverseTransformB.Invert();
 
     // Only populate the manifold if there is a collision
 
@@ -84,16 +83,16 @@ namespace SHADE
     if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f))
     {
       manifold.normal     = SHVec3::UnitY;
-      contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, inverseTransformA.orientation);
+      contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, TF_A.GetInverse().orientation);
-      contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, inverseTransformB.orientation);
+      contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, TF_B.GetInverse().orientation);
 
       manifold.contacts[numContacts++] = contact;
     }
     else
     {
       manifold.normal     = SHVec3::Normalise(A_TO_B);
-      contact.localPointA = RADIUS_A * SHVec3::Normalise(inverseTransformA * CENTER_B);
+      contact.localPointA = RADIUS_A * SHVec3::Normalise(TF_A.GetInverse() * CENTER_B);
-      contact.localPointB = RADIUS_B * SHVec3::Normalise(inverseTransformB * CENTER_A);
+      contact.localPointB = RADIUS_B * SHVec3::Normalise(TF_B.GetInverse() * CENTER_A);
       
 
       manifold.contacts[numContacts++] = contact;

SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp

@@ -53,11 +53,18 @@ namespace SHADE
       };
 
       const auto* SHAPE_A = manifold.shapeA;
+      const auto* SHAPE_B = manifold.shapeB;
+
       const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() };
+      const SHCollisionUtils::ShapeTransform TF_B = { SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() };
 
       for (uint32_t i = 0; i < manifold.numContacts; ++i)
-        collisionEvent.contactPoints[i] = TF_A * manifold.contacts[i].localPointA;
+      {
+        const SHVec3 WORLD_POINT_A = TF_A * manifold.contacts[i].localPointA;
+        const SHVec3 WORLD_POINT_B = TF_B * manifold.contacts[i].localPointB;
 
+        collisionEvent.contactPoints[i] = SHVec3::ClampedLerp(WORLD_POINT_A, WORLD_POINT_B, 0.5f);
+      }
 
       collisionEvents.emplace_back(collisionEvent);
     }
@@ -77,15 +84,20 @@ namespace SHADE
         continue;
 
       const auto* SHAPE_A = manifold.shapeA;
+      const auto* SHAPE_B = manifold.shapeB;
+
       const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() };
+      const SHCollisionUtils::ShapeTransform TF_B = { SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() };
 
       for (uint32_t i = 0; i < manifold.numContacts; ++i)
       {
-        // Contact position will be the world position of localPointA
+        // Contact position will be the middle of worldPointA & worldPointB
+        const SHVec3 WORLD_POINT_A = TF_A * manifold.contacts[i].localPointA;
+        const SHVec3 WORLD_POINT_B = TF_B * manifold.contacts[i].localPointB;
 
         const ContactInfo INFO
         {
-          .position = TF_A * manifold.contacts[i].localPointA
+          .position = SHVec3::ClampedLerp(WORLD_POINT_A, WORLD_POINT_B, 0.5f)
         , .normal   = manifold.normal 
         };
 

SHADE_Engine/src/Physics/System/Routines/SHPhysicsDebugDrawRoutine.cpp

@@ -80,7 +80,7 @@ namespace SHADE
       for (auto& contactPoint : CONTACT_POINTS)
       {
         const SHMatrix TRS = SHMatrix::Transform(contactPoint.position, SHQuaternion::Identity, SHVec3{ 0.1f });
-        debugDrawSystem->DrawCube(TRS, CONTACT_COLOUR);
+        debugDrawSystem->DrawSphere(TRS, CONTACT_COLOUR);
         debugDrawSystem->DrawLine(contactPoint.position, contactPoint.position + contactPoint.normal * 0.5f, CONTACT_COLOUR, true);
       }
     }