From 8d8a300a606ad855ed45f0ef73689e97e742aaa6 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Sat, 7 Jan 2023 02:19:26 +0800 Subject: [PATCH] Fixed temporal coherency of polyhedron contacts --- Assets/Scenes/PhysicsSandbox.shade | 2 +- .../Collision/Narrowphase/SHCollision.h | 2 +- .../Narrowphase/SHConvexVsConvex.cpp | 37 ++++++++++++------- .../src/Physics/Dynamics/SHContactManager.cpp | 9 +++++ 4 files changed, 34 insertions(+), 16 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index d8f194b8..63933bd2 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -280,7 +280,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: -0.5, y: 13.5, z: 0} + Translate: {x: 0.400000006, y: 13.5, z: 0} Rotate: {x: -0, y: 0, z: 0} Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881} IsActive: true diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index 8edc58c6..25796e42 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -116,7 +116,7 @@ namespace SHADE 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 clipPolygonAgainstPlane (const std::vector& in, const SHPlane& plane) noexcept; + static std::vector clipPolygonAgainstPlane (const std::vector& in, const SHPlane& plane, int32_t refIdx) noexcept; static std::vector reduceContacts (const std::vector& in, const SHVec3& normal) noexcept; // TODO: Reduce Manifold diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index 0da225ad..152dede2 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -151,10 +151,11 @@ namespace SHADE // Number of vertices == number of edges on face // Clip against each edge of face + int32_t numClipped = 0; + const SHHalfEdgeStructure::Face& REFERENCE_FACE = referencePoly->GetFace(minFaceQuery.closestFace); const int32_t NUM_EDGES = static_cast(INCIDENT_FACE.vertexIndices.size()); - const size_t COPY_SIZE = sizeof(SHContact) * MAX_NUM_CONTACTS; for (const int32_t i : std::views::iota(0, NUM_EDGES)) { // Build a plane on the reference poly normal X tangent and the tail of the tangent @@ -167,8 +168,17 @@ namespace SHADE const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1); const SHPlane PLANE{ V1, SHVec3::Cross(REFERENCE_NORMAL, TANGENT) }; - auto clipped = clipPolygonAgainstPlane(incidentPolygon, PLANE); - memcpy_s(incidentPolygon.data(), COPY_SIZE, clipped.data(), COPY_SIZE); + auto clipped = clipPolygonAgainstPlane(incidentPolygon, PLANE, V1_IDX); + numClipped = static_cast(clipped.size()); + + // Resize the reused container to hold all the clipped points. + if (incidentPolygon.size() < clipped.size()) + { + incidentPolygon.clear(); + incidentPolygon.resize(numClipped); + } + + memcpy_s(incidentPolygon.data(), sizeof(SHContact) * numClipped, clipped.data(), sizeof(SHContact) * numClipped); } // From clipped polygon, only keep points that are below the reference face @@ -183,8 +193,9 @@ namespace SHADE if (DEPTH <= 0.0f) { SHContact contact; - contact.position = cp.position; - contact.penetration = -DEPTH; + contact.position = cp.position; + contact.penetration = -DEPTH; + contact.featurePair.key = cp.featurePair.key; if (flipNormal) { @@ -211,8 +222,6 @@ namespace SHADE memcpy_s(manifold.contacts, sizeof(SHContact) * numContacts, finalContacts.data(), sizeof(SHContact) * numContacts); } - - manifold.normal = flipNormal ? -REFERENCE_NORMAL : REFERENCE_NORMAL; manifold.numContacts = numContacts; @@ -423,7 +432,7 @@ namespace SHADE return bestFace; } - std::vector SHCollision::clipPolygonAgainstPlane(const std::vector& in, const SHPlane& plane) noexcept + std::vector SHCollision::clipPolygonAgainstPlane(const std::vector& in, const SHPlane& plane, int32_t refIdx) noexcept { static constexpr float THRESHOLD = -SHMath::EPSILON; @@ -466,10 +475,10 @@ namespace SHADE { SHContact interpolated; - interpolated.featurePair.indexA = TAIL.featurePair.indexA; + interpolated.featurePair.indexA = refIdx; interpolated.featurePair.indexB = TAIL.featurePair.indexB; - interpolated.featurePair.typeA = TAIL.featurePair.typeB; - interpolated.featurePair.typeB = TAIL.featurePair.typeA; + interpolated.featurePair.typeA = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX); + interpolated.featurePair.typeB = SHUtilities::ConvertEnum(SHContactFeatures::Type::FACE); const SHVec3 DIR = SHVec3::Normalise(HEAD.position - TAIL.position); interpolated.position = TAIL.position + DIR * std::fabs(TAIL_DISTANCE); @@ -484,10 +493,10 @@ namespace SHADE SHContact interpolated; - interpolated.featurePair.indexA = TAIL.featurePair.indexA; + interpolated.featurePair.indexA = refIdx; interpolated.featurePair.indexB = TAIL.featurePair.indexB; - interpolated.featurePair.typeA = TAIL.featurePair.typeB; - interpolated.featurePair.typeB = TAIL.featurePair.typeA; + interpolated.featurePair.typeA = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX); + interpolated.featurePair.typeB = SHUtilities::ConvertEnum(SHContactFeatures::Type::FACE); const SHVec3 DIR = SHVec3::Normalise(HEAD.position - TAIL.position); interpolated.position = TAIL.position + DIR * std::fabs(TAIL_DISTANCE); diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index 34e3dabf..a13e47ff 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp @@ -271,6 +271,15 @@ namespace SHADE { const SHContact& OLD_CONTACT = oldManifold.contacts[j]; + SHLOGV_INFO_D + ( + "Old vs New \n indexA: {} vs {} \n typeA: {} vs {} \n indexB: {} vs {} \n typeB: {} vs {}" + , OLD_CONTACT.featurePair.indexA, contact.featurePair.indexA + , OLD_CONTACT.featurePair.typeA, contact.featurePair.typeA + , OLD_CONTACT.featurePair.indexB, contact.featurePair.indexB + , OLD_CONTACT.featurePair.typeB, contact.featurePair.typeB + ) + if (OLD_CONTACT.featurePair.key == contact.featurePair.key) { // If contact features persists, re-project old solution