From 3a454953cef63ab8a6ae7e13636ac1dc850c710b Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Wed, 1 Mar 2023 04:34:06 +0800 Subject: [PATCH] Solved Sphere VS Convex in local space --- Assets/Application.SHConfig | 2 +- Assets/Scenes/PhysicsSandbox.shade | 77 +-------- Assets/Scenes/SS_Playground.shade | 35 +++- .../Narrowphase/SHCollisionUtils.cpp | 6 +- .../Collision/Narrowphase/SHCollisionUtils.h | 2 +- .../Narrowphase/SHSphereVsConvex.cpp | 161 +++++++++--------- .../Narrowphase/SHSphereVsSphere.cpp | 15 +- .../src/Physics/Dynamics/SHContactManager.cpp | 20 ++- .../Routines/SHPhysicsDebugDrawRoutine.cpp | 2 +- 9 files changed, 143 insertions(+), 177 deletions(-) diff --git a/Assets/Application.SHConfig b/Assets/Application.SHConfig index 6fdc7d87..d0fa83df 100644 --- a/Assets/Application.SHConfig +++ b/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 \ No newline at end of file diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 5a4ab9fd..f984c3df 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/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 @@ -175,73 +175,4 @@ Position Offset: {x: 0, y: 0, z: 0} 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: ~ \ No newline at end of file diff --git a/Assets/Scenes/SS_Playground.shade b/Assets/Scenes/SS_Playground.shade index b3e540b5..beee48b3 100644 --- a/Assets/Scenes/SS_Playground.shade +++ b/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 - Radius: 3 + Type: Box + 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 diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.cpp index dd4b7439..c4267ca8 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.cpp +++ b/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(); - position = SHVec3::Rotate(-position, orientation); + const SHQuaternion INV_ORIENTATION = SHQuaternion::Inverse(orientation); + return ShapeTransform { SHVec3::Rotate(-position, INV_ORIENTATION), INV_ORIENTATION }; } diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.h index cef216de..f3a63cff 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.h +++ b/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 diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index 928a8184..68217d8b 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/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(A); - //const SHConvexPolyhedron& POLYHEDRON = dynamic_cast(B); + const SHSphere& SPHERE = dynamic_cast(A); + const SHConvexPolyhedron& POLY = dynamic_cast(B); - //const SHVec3 CENTER = SPHERE.Center; - //const float RADIUS = SPHERE.GetWorldRadius(); + const SHVec3 CENTER = SPHERE.Center; + const float RADIUS = SPHERE.GetWorldRadius(); - //const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON); - //if (!FACE_QUERY.colliding) - // return false; + const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLY); + if (!FACE_QUERY.colliding) + return false; - //uint32_t numContacts = 0; - //const float PENETRATION = RADIUS - FACE_QUERY.bestDistance; + const SHCollisionUtils::ShapeTransform TF_A = { SPHERE.GetWorldCentroid(), SPHERE.GetWorldOrientation() }; + 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; - //contact.featurePair.key = 0; + uint32_t numContacts = 0; + const float PENETRATION = RADIUS - FACE_QUERY.bestDistance; - //// Check if center is inside polyhedron (below the face) - //if (FACE_QUERY.bestDistance < SHMath::EPSILON) - //{ - // manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace); + SHContact contact; + contact.featurePair.key = 0; - // contact.penetration = PENETRATION; - // contact.position = CENTER; + // Check if center is inside polyhedron (below the face) + if (FACE_QUERY.bestDistance < SHMath::EPSILON) + { + manifold.normal = -POLY.GetNormal(FACE_QUERY.closestFace); - // manifold.contacts[numContacts++] = contact; - // manifold.numContacts = numContacts; + contact.penetration = PENETRATION; - // 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 - //const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace); + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; - //const auto& FACE = POLYHEDRON.GetFace(FACE_QUERY.closestFace); - //const SHVec3& FACE_NORMAL = POLYHEDRON.GetNormal(FACE_QUERY.closestFace); - //const int32_t NUM_VERTICES = static_cast(FACE.vertexIndices.size()); + return true; + } - //// Get points and build tangents - //const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES; - //const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1; + // Find closest face of polygon to circle + const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLY, FACE_QUERY.closestFace); - //const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index); - //const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index); - //const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index); + const auto& FACE = POLY.GetFace(FACE_QUERY.closestFace); + const SHVec3& FACE_NORMAL = POLY.GetNormal(FACE_QUERY.closestFace); + const int32_t NUM_VERTICES = static_cast(FACE.vertexIndices.size()); - //const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1); - //const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1); + // Get points and build tangents + 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 int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2); - //if (REGION == 0) - // return false; + const SHVec3 P1 = POLY.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index); + const SHVec3 P2 = POLY.GetVertex(FACE.vertexIndices[P2_INDEX].index); + const SHVec3 P3 = POLY.GetVertex(FACE.vertexIndices[P3_INDEX].index); - //// 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; - //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; + // Get the voronoi region it belongs in + const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2); + if (REGION == 0) + return false; - // manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); + // Create contact information based on region - // contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); - // contact.position = CP; + const SHVec3 P1_TO_CENTER = CENTER - P1; + 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; - // } - // case 3: // Region C - // { - // manifold.normal = -SHVec3::Normalise(P1_TO_CENTER); + manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); + contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); - // contact.penetration = RADIUS - P1_TO_CENTER.Length(); - // contact.position = P1; + break; + } + case 3: // Region C + { + manifold.normal = -SHVec3::Normalise(P1_TO_CENTER); + contact.penetration = RADIUS - P1_TO_CENTER.Length(); - // break; - // } - // case 4: // Region D - // { - // manifold.normal = -FACE_NORMAL; + break; + } + case 4: // Region D + { + manifold.normal = -FACE_NORMAL; + contact.penetration = PENETRATION; - // contact.penetration = PENETRATION; - // contact.position = CENTER - FACE_NORMAL * RADIUS; + break; + } + default: return false; // Should never happen + } - // break; - // } - // default: return false; // Should never happen - //} + contact.localPointA = SHVec3::Rotate(manifold.normal * RADIUS, INV_TF_A.orientation); + contact.localPointB = (INV_TF_B * CENTER) + manifold.normal * (RADIUS - contact.penetration); - //manifold.contacts[numContacts++] = contact; - //manifold.numContacts = numContacts; + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; - //return true; - - return false; + return true; } 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.closestFace = i; + faceQuery.bestDistance = SIGNED_DIST; + faceQuery.closestFace = i; } } diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp index 37bd9d32..210c4f43 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp +++ b/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() }; - SHCollisionUtils::ShapeTransform inverseTransformB = { SPHERE_B.GetWorldCentroid(), SPHERE_B.GetWorldOrientation() }; + const SHCollisionUtils::ShapeTransform TF_A = { SPHERE_A.GetWorldCentroid(), SPHERE_A.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.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, inverseTransformB.orientation); + contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, TF_A.GetInverse().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.localPointB = RADIUS_B * SHVec3::Normalise(inverseTransformB * CENTER_A); + contact.localPointA = RADIUS_A * SHVec3::Normalise(TF_A.GetInverse() * CENTER_B); + contact.localPointB = RADIUS_B * SHVec3::Normalise(TF_B.GetInverse() * CENTER_A); manifold.contacts[numContacts++] = contact; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index dc5b40f9..818ed62a 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/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 }; diff --git a/SHADE_Engine/src/Physics/System/Routines/SHPhysicsDebugDrawRoutine.cpp b/SHADE_Engine/src/Physics/System/Routines/SHPhysicsDebugDrawRoutine.cpp index 69317e59..f49a9c8a 100644 --- a/SHADE_Engine/src/Physics/System/Routines/SHPhysicsDebugDrawRoutine.cpp +++ b/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); } }