From 896b47c1a01e28fb98ebc4fe6a9ecbcb577f7e9f Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Sat, 31 Dec 2022 01:11:03 +0800 Subject: [PATCH] Fixed and optimised sphere vs convex polyhedron Improved sphere vs convex polyhedron from O(n^2) to O(n). Math is amazing. --- Assets/Scenes/PhysicsSandbox.shade | 231 ++++++++++++++---- .../CollisionShapes/SHBoxCollisionShape.cpp | 35 +++ .../SHCollisionShapeLibrary.cpp | 4 +- .../Narrowphase/SHSphereVsConvex.cpp | 92 +++---- .../src/Physics/Dynamics/SHContactManager.cpp | 1 - 5 files changed, 272 insertions(+), 91 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 7360d543..f93d0cc8 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -4,9 +4,9 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: -1.80977702, y: 3, z: 0} - Rotate: {x: -0, y: 0, z: -0.506194055} - Scale: {x: 3.27252102, y: 0.999997199, z: 1} + Translate: {x: 2.58071685, y: 0.456140399, z: 0} + Rotate: {x: -0, y: 0, z: 0.469853699} + Scale: {x: 4.61071014, y: 0.999995053, z: 1} IsActive: true RigidBody Component: Type: Static @@ -19,7 +19,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 @@ -60,54 +60,13 @@ Perspective: true IsActive: true Scripts: ~ -- EID: 2 - Name: Default - IsActive: true - NumberOfChildren: 0 - Components: - Transform Component: - Translate: {x: 0, y: 0, z: 0} - Rotate: {x: -0, y: 0, z: -0} - Scale: {x: 1, 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: false - Colliders: - - Is Trigger: false - Collision Tag: 1 - Type: Sphere - Radius: 2.5 - 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: ~ - EID: 3 Name: Default IsActive: true NumberOfChildren: 0 Components: Transform Component: - Translate: {x: -1.97624588, y: 5, z: 0} + Translate: {x: -2.24715948, y: 6.47200441, z: 0} Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true @@ -141,6 +100,186 @@ Position Offset: {x: 0, y: 0, z: 0} Rotation Offset: {x: 0, y: 0, z: 0} IsActive: true + Scripts: + - Type: PhysicsTestObj + Enabled: true + forceAmount: 50 + torqueAmount: 500 +- EID: 2 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 0, y: 4.09544182, z: 0} + Rotate: {x: -0, y: 0, z: -0.437829614} + Scale: {x: 4.61071014, y: 0.999995887, z: 1} + IsActive: true + RigidBody Component: + Type: Static + Auto Mass: false + Mass: .inf + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + Use Gravity: true + Gravity Scale: 1 + Interpolate: true + Sleeping Enabled: true + Freeze Position X: true + Freeze Position Y: true + Freeze Position Z: true + 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: 50 + torqueAmount: 500 +- EID: 4 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 0, y: -1.74209237, z: 0} + Rotate: {x: -0, y: 0, z: -0} + Scale: {x: 10, y: 0.5, z: 10} + IsActive: true + RigidBody Component: + Type: Static + Auto Mass: false + Mass: .inf + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + Use Gravity: true + Gravity Scale: 1 + Interpolate: true + Sleeping Enabled: true + Freeze Position X: false + Freeze Position Y: true + 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: 50 + torqueAmount: 500 +- EID: 5 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: -4.80025721, y: 3, z: 0} + Rotate: {x: -0, y: 0, z: 1.57079601} + Scale: {x: 9.99975109, y: 0.499992192, z: 10} + IsActive: true + RigidBody Component: + Type: Static + Auto Mass: false + Mass: .inf + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + Use Gravity: true + Gravity Scale: 1 + Interpolate: true + Sleeping Enabled: true + Freeze Position X: false + Freeze Position Y: true + 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: 50 + torqueAmount: 500 +- EID: 6 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 4.80000019, y: 3, z: 0} + Rotate: {x: -0, y: 0, z: 1.57079601} + Scale: {x: 9.99975109, y: 0.499992192, z: 10} + IsActive: true + RigidBody Component: + Type: Static + Auto Mass: false + Mass: .inf + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + Use Gravity: true + Gravity Scale: 1 + Interpolate: true + Sleeping Enabled: true + Freeze Position X: false + Freeze Position Y: true + 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 diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHBoxCollisionShape.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHBoxCollisionShape.cpp index 0fa8c861..621b560b 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHBoxCollisionShape.cpp +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHBoxCollisionShape.cpp @@ -160,6 +160,41 @@ namespace SHADE throw std::invalid_argument("Index out-of-range!"); return GetVertices()[index]; + + //// Rotate half extents + //const SHVec3 ROTATED_EXTENTS = SHVec3::Rotate(Extents, Orientation); + //const SHVec3 CENTER { Center }; + + ///* + // * Front: -Z + // * + // * 3 _____ 2 + // * | | + // * | | + // * |_____| + // * 0 1 + // * + // * Back: +Z + // * + // * 7 _____ 6 + // * | | + // * | | + // * |_____| + // * 4 5 + // * + // */ + + //switch (index) + //{ + // case 0: return CENTER + SHVec3 { -ROTATED_EXTENTS.x, -ROTATED_EXTENTS.y, -ROTATED_EXTENTS.z } + // case 1: return CENTER + SHVec3 { + // case 2: return CENTER + SHVec3 { + // case 3: return CENTER + SHVec3 { + // case 4: return CENTER + SHVec3 { + // case 5: return CENTER + SHVec3 { + // case 6: return CENTER + SHVec3 { + // case 7: return CENTER + SHVec3 { + //} } SHVec3 SHBoxCollisionShape::GetNormal(int faceIndex) const diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp index e9832f9c..58af5dc4 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp @@ -126,9 +126,9 @@ namespace SHADE * * Faces: * - * Front: 0 (0,1,2,3) Normal: Z + * Front: 0 (0,1,2,3) Normal: -Z * Right: 1 (1,5,6,2) Normal: X - * Back: 2 (5,4,7,6) Normal: -Z + * Back: 2 (5,4,7,6) Normal: Z * Left: 3 (4,0,3,7) Normal: -X * Top: 4 (3,2,6,7) Normal: Y * Bottom: 5 (4,5,1,0) Normal: -Y diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index 034e4d57..152c9460 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -52,8 +52,8 @@ namespace SHADE 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; + const SHVec3 CENTER = SPHERE.GetCenter(); + const float TOTAL_RADIUS = SPHERE.GetWorldRadius() + SHConvexPolyhedronCollisionShape::RADIUS; // Find closest face of polygon to circle @@ -64,56 +64,49 @@ namespace SHADE const SHHalfEdgeDS* HALF_EDGE_STRUCTURE = CONVEX.GetHalfEdgeStructure(); /* - * Test against each face + * 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. + * 1. For each face, build a plane in point-normal form. + * 2. Find the signed distance from plane to center of sphere. + * 3. Save best distance and 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) + // Build plane equation + + const SHVec3 POINT = CONVEX.GetVertex(FACE.vertexIndices[0]); // use the first vertex to build a plane + const SHVec3& NORMAL = CONVEX.GetNormal(i); + const float D = -SHVec3::Dot(NORMAL, POINT); + + // Find signed distance of center to plane + const float SIGNED_DIST = SHVec3::Dot(NORMAL, CENTER) + D; + + // Early out: + // If face is facing away from center, signed dist is negative. + // Therefore signed distance is only positive when sphere is in front of the face. + if (SIGNED_DIST > TOTAL_RADIUS) + return false; + + if (SIGNED_DIST > bestDistance) { - // 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; - } + bestDistance = SIGNED_DIST; + closestFaceIndex = i; } } uint32_t numContacts = 0; - - // Rotate the normal into the world space - const SHVec3& BEST_NORMAL = CONVEX.GetNormal(closestFaceIndex); const float PENETRATION = TOTAL_RADIUS - bestDistance; // Check if center is inside polyhedron (below the face) if (bestDistance < SHMath::EPSILON) { - manifold.normal = -BEST_NORMAL; + manifold.normal = CONVEX.GetNormal(closestFaceIndex); SHContact newContact; newContact.penetration = PENETRATION; - newContact.position = SPHERE.GetCenter() - BEST_NORMAL * PENETRATION; + newContact.position = SPHERE.GetCenter(); newContact.featurePair.key = 0; manifold.contacts[numContacts++] = newContact; @@ -135,10 +128,25 @@ namespace SHADE * / / regionC */ - const SHHalfEdgeDS::Face& CLOSEST_FACE = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex); - const int32_t NUM_VERTICES_ON_FACE = static_cast(CLOSEST_FACE.vertexIndices.size()); + const SHHalfEdgeDS::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex); + const SHVec3& NORMAL = CONVEX.GetNormal(closestFaceIndex); - const SHVec3 C = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[closestPointIndex]); + const int32_t NUM_VERTICES_ON_FACE = static_cast(FACE.vertexIndices.size()); + + // Find closest point on face + closestPointIndex = 0; + float smallestDist = SHVec3::Dot(CENTER - CONVEX.GetVertex(FACE.vertexIndices[0]), NORMAL); + + for (int32_t i = 1; i < NUM_VERTICES_ON_FACE; ++i) + { + const SHVec3 POINT = CONVEX.GetVertex(FACE.vertexIndices[i]); + const float PROJECTION = SHVec3::Dot(CENTER - POINT, NORMAL); + + if (PROJECTION < smallestDist) + closestPointIndex = i; + } + + const SHVec3 C = CONVEX.GetVertex(FACE.vertexIndices[closestPointIndex]); const SHVec3 C_TO_CENTER = SPHERE.GetCenter() - C; const int32_t INDEX_A = (closestPointIndex + 1) % NUM_VERTICES_ON_FACE; @@ -146,8 +154,8 @@ namespace SHADE const SHVec3 POINTS[2] = { - CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[INDEX_A]) // A - , CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[INDEX_B]) // B + CONVEX.GetVertex(FACE.vertexIndices[INDEX_A]) // A + , CONVEX.GetVertex(FACE.vertexIndices[INDEX_B]) // B }; // To be inside either region A or B, 2 conditions must be satisfied @@ -170,7 +178,7 @@ namespace SHADE projection = SHVec3::Dot(C_TO_CENTER, CP_TO_CENTER); if (projection >= 0.0f) { - // Sphere Within region A + // Sphere Within region manifold.normal = CP_TO_CENTER; SHContact newContact; @@ -190,7 +198,7 @@ namespace SHADE { if (C_TO_CENTER.LengthSquared() < TOTAL_RADIUS * TOTAL_RADIUS) { - manifold.normal = SHVec3::Normalise(C_TO_CENTER); + manifold.normal = -SHVec3::Normalise(C_TO_CENTER); SHContact newContact; newContact.penetration = PENETRATION; @@ -207,11 +215,11 @@ namespace SHADE // Region D if (PENETRATION <= TOTAL_RADIUS) { - manifold.normal = -BEST_NORMAL; + manifold.normal = -NORMAL; SHContact newContact; newContact.penetration = PENETRATION; - newContact.position = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS; + newContact.position = SPHERE.GetCenter() - NORMAL * TOTAL_RADIUS; newContact.featurePair.key = 0; manifold.contacts[numContacts++] = newContact; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index c8bd2abe..34e3dabf 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp @@ -63,7 +63,6 @@ namespace SHADE const SHContactManager::ContactPoints& SHContactManager::GetContactPoints() const noexcept { static ContactPoints contactPoints; - contactPoints.clear(); for (auto& manifold : manifolds | std::views::values)