From 0c3106f15b9d770be1a2391655ecf63bcf404e36 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Sun, 22 Jan 2023 19:20:03 +0800 Subject: [PATCH] Abstracted contact derivation as setup for cached SAT --- Assets/Scenes/PhysicsSandbox.shade | 14 +- .../Collision/Narrowphase/SHCollision.h | 105 ++++++- .../Narrowphase/SHConvexVsConvex.cpp | 277 +++++++++--------- .../src/Physics/Dynamics/SHPhysicsWorld.h | 4 - 4 files changed, 243 insertions(+), 157 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index ad77de0a..8f439326 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -79,9 +79,9 @@ Freeze Position X: false Freeze Position Y: false Freeze Position Z: false - Freeze Rotation X: true - Freeze Rotation Y: true - Freeze Rotation Z: true + Freeze Rotation X: false + Freeze Rotation Y: false + Freeze Rotation Z: false IsActive: true Collider Component: DrawColliders: false @@ -90,7 +90,7 @@ Collision Tag: 1 Type: Sphere Radius: 1 - Friction: 1 + Friction: 0.400000006 Bounciness: 0 Density: 1 Position Offset: {x: 0, y: 0, z: 0} @@ -131,7 +131,7 @@ Collision Tag: 2 Type: Box Half Extents: {x: 1, y: 1, z: 1} - Friction: 1 + Friction: 0.400000006 Bounciness: 0 Density: 1 Position Offset: {x: 0, y: 0, z: 0} @@ -268,7 +268,7 @@ Components: Transform Component: Translate: {x: 2, y: 2, z: 3} - Rotate: {x: 0, y: 0, z: 0} + Rotate: {x: 0.785398185, y: 0.785398185, z: 0.785398185} Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337} IsActive: true RigidBody Component: @@ -278,7 +278,7 @@ Drag: 0.00999999978 Angular Drag: 0 Use Gravity: true - Gravity Scale: 5 + Gravity Scale: 1 Interpolate: true Sleeping Enabled: true Freeze Position X: false diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index 1ef75974..e78b33fa 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -99,9 +99,27 @@ namespace SHADE // Sphere VS Convex - static FaceQuery findClosestFace (const SHSphere& sphere, const SHConvexPolyhedron& polyhedron) noexcept; - static int32_t findClosestPoint (const SHSphere& sphere, const SHConvexPolyhedron& polyhedron, int32_t faceIndex) noexcept; - static int32_t findVoronoiRegion (const SHSphere& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2) noexcept; + static FaceQuery findClosestFace + ( + const SHSphere& sphere + , const SHConvexPolyhedron& polyhedron + ) noexcept; + + static int32_t findClosestPoint + ( + const SHSphere& sphere + , const SHConvexPolyhedron& polyhedron + , int32_t faceIndex + ) noexcept; + + static int32_t findVoronoiRegion + ( + const SHSphere& sphere + , const SHVec3& faceVertex + , const SHVec3& faceNormal + , const SHVec3& tangent1 + , const SHVec3& tangent2 + ) noexcept; // Capsule VS Convex @@ -115,16 +133,79 @@ namespace SHADE * https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp */ - static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; - static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; + static FaceQuery queryFaceDirections + ( + const SHConvexPolyhedron& A + , const SHConvexPolyhedron& B + ) noexcept; - static bool buildMinkowskiFace (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; - static bool isMinkowskiFace (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept; - static float distanceBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; - static SHVec3 findClosestPointBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; - static int32_t findIncidentFace (const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept; - static std::vector clipPolygonWithPlane (const std::vector& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept; - static std::vector reduceContacts (const std::vector& in, const SHVec3& faceNormal) noexcept; + static EdgeQuery queryEdgeDirections + ( + const SHConvexPolyhedron& A + , const SHConvexPolyhedron& B + ) noexcept; + + static bool buildMinkowskiFace + ( + const SHConvexPolyhedron& A + , const SHConvexPolyhedron& B + , int32_t edgeA + , int32_t edgeB + ) noexcept; + + static bool isMinkowskiFace + ( + const SHVec3& a + , const SHVec3& b + , const SHVec3& c + , const SHVec3& d + ) noexcept; + + static float distanceBetweenEdges + ( + const SHConvexPolyhedron& A + , const SHConvexPolyhedron& B + , int32_t edgeA + , int32_t edgeB + ) noexcept; + + static SHVec3 findClosestPointBetweenEdges + ( + const SHConvexPolyhedron& A + , const SHConvexPolyhedron& B + , int32_t edgeA + , int32_t edgeB + ) noexcept; + + static int32_t findIncidentFace + ( + const SHConvexPolyhedron& poly + , const SHVec3& normal + ) noexcept; + + static bool findFaceContacts + ( + SHManifold& manifold + , const SHConvexPolyhedron& incPoly + , int32_t incFace + , const SHConvexPolyhedron& refPoly + , int32_t refFace + , bool flip + ) noexcept; + + static std::vector clipPolygonWithPlane + ( + const std::vector& in + , int32_t numIn + , const SHPlane& plane + , int32_t planeIdx + ) noexcept; + + static std::vector reduceContacts + ( + const std::vector& in + , const SHVec3& faceNormal + ) noexcept; }; } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index a59e8f28..71539949 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -128,139 +128,7 @@ namespace SHADE const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace); const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL); - const SHHalfEdgeStructure::Face& INCIDENT_FACE = incidentPoly->GetFace(INCIDENT_FACE_IDX); - const SHHalfEdgeStructure::Face& REFERENCE_FACE = referencePoly->GetFace(minFaceQuery.closestFace); - - const int32_t NUM_INCIDENT_VERTICES = static_cast(INCIDENT_FACE.vertexIndices.size()); - const int32_t NUM_REFERENCE_VERTICES = static_cast(REFERENCE_FACE.vertexIndices.size()); - - // Build incoming vertices to clip - std::vector clipIn; - clipIn.resize(NUM_INCIDENT_VERTICES * 2, ClipVertex{}); - - int32_t numClipIn = 0; - for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i) - { - const int32_t prevI = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1; - - const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index; - - // The incoming id is the previous edge - // The outgoing id is the current edge (where this vertex is the tail of) - - ClipVertex v; - v.position = incidentPoly->GetVertex(V_INDEX); - v.featurePair.inI = INCIDENT_FACE.vertexIndices[prevI].edgeIndex; - v.featurePair.outI = INCIDENT_FACE.vertexIndices[i].edgeIndex; - v.featurePair.inR = 0; - v.featurePair.outR = 0; - - clipIn[numClipIn++] = v; - } - - // Clip the vertices against the reference face side planes. - // Number of side planes == number of edges == number of vertices - for (int32_t i = 0; i < NUM_REFERENCE_VERTICES; ++i) - { - // Side plane can be built with the vertex on the edge and the plane's normal - // Plane normal = faceNormal X tangent (v2 - v1) - - const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index; - const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index; - - const SHVec3 V1 = referencePoly->GetVertex(V1_INDEX); - const SHVec3 V2 = referencePoly->GetVertex(V2_INDEX); - - const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1); - const SHPlane CLIP_PLANE { V1, SHVec3::Cross(REFERENCE_NORMAL, TANGENT) }; - - std::vector clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex); - if (clipOut.empty()) - return false; - - // Replace the clip container's contents with the clipped points for the next clipping pass - const int32_t NUM_CLIPPED = static_cast(clipOut.size()); - for (int32_t clippedIndex = 0; clippedIndex < NUM_CLIPPED; ++clippedIndex) - { - clipIn[clippedIndex].position = clipOut[clippedIndex].position; - clipIn[clippedIndex].featurePair.key = clipOut[clippedIndex].featurePair.key; - } - numClipIn = NUM_CLIPPED; - } - - // From the final set of clipped points, only keep the points that are below the reference plane. - const SHPlane REFERENCE_PLANE{ referencePoly->GetVertex(REFERENCE_FACE.vertexIndices.front().index), REFERENCE_NORMAL }; - - std::vector contacts; - for (int32_t i = 0; i < numClipIn; ++i) - { - const SHVec3 POS = clipIn[i].position; - const float DIST = REFERENCE_PLANE.SignedDistance(POS); - if (DIST <= 0.0f) - { - SHContact contact; - contact.position = POS; - contact.penetration = -DIST; - - if (flipNormal) - { - contact.featurePair.inI = clipIn[i].featurePair.inR; - contact.featurePair.inR = clipIn[i].featurePair.inI; - contact.featurePair.outI = clipIn[i].featurePair.outR; - contact.featurePair.outR = clipIn[i].featurePair.outI; - } - else - { - contact.featurePair.key = clipIn[i].featurePair.key; - } - - contacts.emplace_back(contact); - ++numContacts; - } - } - - // Reduce contact manifold if more than 4 points - if (numContacts > 4) - { - const auto INDICES_TO_KEEP = reduceContacts(contacts, REFERENCE_NORMAL); - std::vector reducedContacts; - - const int32_t NUM_REDUCED = static_cast(INDICES_TO_KEEP.size()); - for (int32_t i = 0; i < NUM_REDUCED; ++i) - reducedContacts.emplace_back(contacts[INDICES_TO_KEEP[i]]); - - contacts.clear(); - // Copy contacts to main container - for (auto& contact : reducedContacts) - contacts.emplace_back(contact); - } - - // Remove potential duplicate contact points - // No way about this being an n^2 loop - static constexpr float THRESHOLD = SHPHYSICS_SAME_CONTACT_DISTANCE * SHPHYSICS_SAME_CONTACT_DISTANCE; - for (auto i = contacts.begin(); i != contacts.end(); ++i) - { - for (auto j = i + 1; j != contacts.end();) - { - const float D2 = SHVec3::DistanceSquared(i->position, j->position); - if (D2 < THRESHOLD) - j = contacts.erase(j); - else - ++j; - } - } - - // Copy final contacts into the manifold - numContacts = static_cast(contacts.size()); - for (int32_t i = 0; i < numContacts; ++i) - manifold.contacts[i] = contacts[i]; - - manifold.numContacts = numContacts; - manifold.normal = REFERENCE_NORMAL; - if (flipNormal) - manifold.normal = -manifold.normal; - - return true; + return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal); } /*-----------------------------------------------------------------------------------*/ @@ -442,7 +310,7 @@ namespace SHADE const float A2_OVER_A1 = VB_DOT_VA == 0.0f ? 0.0f : VB_DOT_VB / VB_DOT_VA; const float NUMERATOR = C_DOT_VA * A2_OVER_A1 - C_DOT_VB; const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1; - const float R = DENOMINATOR == 0.0f ? NUMERATOR : NUMERATOR / DENOMINATOR; + const float R = std::clamp(NUMERATOR / DENOMINATOR, 0.0f, 1.0f); // Just take a point from A since it's A -> B return TAIL_A + R * VA; @@ -470,6 +338,147 @@ namespace SHADE return bestFace; } + bool SHCollision::findFaceContacts(SHManifold& manifold, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept + { + const SHHalfEdgeStructure::Face& INCIDENT_FACE = incPoly.GetFace(incFace); + const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace); + + const int32_t NUM_INCIDENT_VERTICES = static_cast(INCIDENT_FACE.vertexIndices.size()); + const int32_t NUM_REFERENCE_VERTICES = static_cast(REFERENCE_FACE.vertexIndices.size()); + + // Build incoming vertices to clip + std::vector clipIn; + clipIn.resize(NUM_INCIDENT_VERTICES * 2, ClipVertex{}); + + int32_t numClipIn = 0; + for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i) + { + const int32_t prevI = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1; + + const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index; + + // The incoming id is the previous edge + // The outgoing id is the current edge (where this vertex is the tail of) + + ClipVertex v; + v.position = incPoly.GetVertex(V_INDEX); + v.featurePair.inI = INCIDENT_FACE.vertexIndices[prevI].edgeIndex; + v.featurePair.outI = INCIDENT_FACE.vertexIndices[i].edgeIndex; + v.featurePair.inR = 0; + v.featurePair.outR = 0; + + clipIn[numClipIn++] = v; + } + + // Clip the vertices against the reference face side planes. + // Number of side planes == number of edges == number of vertices + const SHVec3 REF_NORMAL = refPoly.GetNormal(refFace); + for (int32_t i = 0; i < NUM_REFERENCE_VERTICES; ++i) + { + // Side plane can be built with the vertex on the edge and the plane's normal + // Plane normal = faceNormal X tangent (v2 - v1) + + const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index; + const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index; + + const SHVec3 V1 = refPoly.GetVertex(V1_INDEX); + const SHVec3 V2 = refPoly.GetVertex(V2_INDEX); + + const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1); + const SHPlane CLIP_PLANE { V1, SHVec3::Cross(REF_NORMAL, TANGENT) }; + + std::vector clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex); + if (clipOut.empty()) + return false; + + // Replace the clip container's contents with the clipped points for the next clipping pass + const int32_t NUM_CLIPPED = static_cast(clipOut.size()); + for (int32_t clippedIndex = 0; clippedIndex < NUM_CLIPPED; ++clippedIndex) + { + clipIn[clippedIndex].position = clipOut[clippedIndex].position; + clipIn[clippedIndex].featurePair.key = clipOut[clippedIndex].featurePair.key; + } + numClipIn = NUM_CLIPPED; + } + + // From the final set of clipped points, only keep the points that are below the reference plane. + const SHPlane REFERENCE_PLANE{ refPoly.GetVertex(REFERENCE_FACE.vertexIndices.front().index), REF_NORMAL }; + + uint32_t numContacts = 0; + + std::vector contacts; + for (int32_t i = 0; i < numClipIn; ++i) + { + const SHVec3 POS = clipIn[i].position; + const float DIST = REFERENCE_PLANE.SignedDistance(POS); + if (DIST <= 0.0f) + { + SHContact contact; + contact.position = POS; + contact.penetration = -DIST; + + if (flip) + { + contact.featurePair.inI = clipIn[i].featurePair.inR; + contact.featurePair.inR = clipIn[i].featurePair.inI; + contact.featurePair.outI = clipIn[i].featurePair.outR; + contact.featurePair.outR = clipIn[i].featurePair.outI; + } + else + { + contact.featurePair.key = clipIn[i].featurePair.key; + } + + contacts.emplace_back(contact); + ++numContacts; + } + } + + // Reduce contact manifold if more than 4 points + if (numContacts > 4) + { + const auto INDICES_TO_KEEP = reduceContacts(contacts, REF_NORMAL); + std::vector reducedContacts; + + const int32_t NUM_REDUCED = static_cast(INDICES_TO_KEEP.size()); + for (int32_t i = 0; i < NUM_REDUCED; ++i) + reducedContacts.emplace_back(contacts[INDICES_TO_KEEP[i]]); + + contacts.clear(); + // Copy contacts to main container + for (auto& contact : reducedContacts) + contacts.emplace_back(contact); + } + + // Remove potential duplicate contact points + // No way about this being an n^2 loop + static constexpr float THRESHOLD = SHPHYSICS_SAME_CONTACT_DISTANCE * SHPHYSICS_SAME_CONTACT_DISTANCE; + for (auto i = contacts.begin(); i != contacts.end(); ++i) + { + for (auto j = i + 1; j != contacts.end();) + { + const float D2 = SHVec3::DistanceSquared(i->position, j->position); + if (D2 < THRESHOLD) + j = contacts.erase(j); + else + ++j; + } + } + + // Copy final contacts into the manifold + numContacts = static_cast(contacts.size()); + for (int32_t i = 0; i < numContacts; ++i) + manifold.contacts[i] = contacts[i]; + + manifold.numContacts = numContacts; + manifold.normal = REF_NORMAL; + if (flip) + manifold.normal = -manifold.normal; + + return true; + } + + std::vector SHCollision::clipPolygonWithPlane(const std::vector& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept { std::vector out; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h index b171f206..9ad525e8 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h @@ -139,10 +139,6 @@ namespace SHADE // TODO: Move to island when islands are set up void integrateForces (SHRigidBody& rigidBody, float dt) const noexcept; void integrateVelocities (SHRigidBody& rigidBody, float dt) const noexcept; - - - - };