From c077575a738eeab8b4a5b2e2ab7f845af81e8adf Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Mon, 16 Jan 2023 15:01:14 +0800 Subject: [PATCH] Fixed convex-convex face detection Minor bugs with contact point detection. Will test more before pushing into main --- Assets/Scenes/PhysicsSandbox.shade | 6 +- .../Collision/Narrowphase/SHCollision.h | 2 +- .../Narrowphase/SHConvexVsConvex.cpp | 208 +++++++++++++++++- SHADE_Engine/src/Physics/SHPhysicsConstants.h | 12 +- 4 files changed, 220 insertions(+), 8 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index c9104ab3..0d2e4abe 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -267,9 +267,9 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 1, y: 2, z: 3} - Rotate: {x: 0, y: 0.785398185, z: 0.785397708} - Scale: {x: 0.999990404, y: 0.999994516, z: 0.999985456} + Translate: {x: 1.81218028, y: 2, z: 3} + Rotate: {x: 0.785398006, y: 0.785398483, z: 0.785398304} + Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337} IsActive: true RigidBody Component: Type: Dynamic diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index 8012392b..1ef75974 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -124,7 +124,7 @@ namespace SHADE 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) 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 52cc2c81..a59e8f28 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -189,8 +189,78 @@ namespace SHADE } // 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 }; - return false; + 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; } /*-----------------------------------------------------------------------------------*/ @@ -472,5 +542,141 @@ namespace SHADE return out; } + std::vector SHCollision::reduceContacts(const std::vector& in, const SHVec3& faceNormal) noexcept + { + std::vector indicesToKeep; + + // Use a map to temporarily store and track the contacts we want + std::unordered_map contactMap; + const int32_t NUM_CONTACTS = static_cast(in.size()); + for (int32_t i = 0; i < NUM_CONTACTS; ++i) + contactMap.emplace(i, &in[i]); + + // Find the furthest point in a given direction + int32_t indexToKeep = -1; + float bestDistance = std::numeric_limits::lowest(); + + for (const auto& [index, contact] : contactMap) + { + const float DIST = SHVec3::Dot(contact->position, SHVec3::One); + if (DIST > bestDistance) + { + bestDistance = DIST; + indexToKeep = index; + } + } + + indicesToKeep.emplace_back(indexToKeep); + contactMap.erase(indexToKeep); + + + indexToKeep = -1; + bestDistance = std::numeric_limits::lowest(); + + // Find point furthest away from the first index + const SHVec3& FIRST_POS = in[indicesToKeep.back()].position; + for (const auto& [index, contact] : contactMap) + { + const float DIST_SQUARED = SHVec3::DistanceSquared(FIRST_POS, contact->position); + if (DIST_SQUARED > bestDistance) + { + bestDistance = DIST_SQUARED; + indexToKeep = index; + } + } + + indicesToKeep.emplace_back(indexToKeep); + contactMap.erase(indexToKeep); + + indexToKeep = -1; + + // We compute the triangle with the largest area. + // The area can be positive or negative depending on the winding order. + + float maxArea = std::numeric_limits::lowest(); + float minArea = std::numeric_limits::max(); + + int32_t maxAreaIndex = -1; + int32_t minAreaIndex = -1; + + const SHVec3& SECOND_POS = in[indicesToKeep.back()].position; + for (const auto& [index, contact] : contactMap) + { + const SHVec3& POS = contact->position; + const SHVec3 TO_P1 = FIRST_POS - POS; + const SHVec3 TO_P2 = SECOND_POS - POS; + + const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f; + + if (AREA > maxArea) + { + maxArea = AREA; + maxAreaIndex = index; + } + + if (AREA < minArea) + { + minArea = AREA; + minAreaIndex = index; + } + } + + // Compare which triangle creates the largest area + bool isAreaPositive = false; + if (maxArea > (-minArea)) + { + isAreaPositive = true; + indexToKeep = maxAreaIndex; + } + else + { + isAreaPositive = false; + indexToKeep = minAreaIndex; + } + + indicesToKeep.emplace_back(indexToKeep); + contactMap.erase(indexToKeep); + + indexToKeep = -1; + + // For the last point, we want the point which forms the largest area that is winded opposite to the first triangle + // The areas should be inverted: If area was -ve, we want a +ve. Otherwise, vice versa. + float bestArea = 0.0f; + + const SHVec3& THIRD_POS = in[indicesToKeep.back()].position; + const SHVec3 ABC[3] = { FIRST_POS, SECOND_POS, THIRD_POS }; + + for (const auto& [index, contact] : contactMap) + { + const SHVec3& Q = contact->position; + + for (int i = 0; i < 3; ++i) + { + const int P1 = i; + const int P2 = (i + 1) % 3; + + const SHVec3 TO_P1 = ABC[P1] - Q; + const SHVec3 TO_P2 = ABC[P2] - Q; + + const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f; + + if (isAreaPositive && AREA < bestArea) + { + bestArea = AREA; + indexToKeep = index; + } + + if (!isAreaPositive && AREA > bestArea) + { + bestArea = AREA; + indexToKeep = index; + } + } + } + + indicesToKeep.emplace_back(indexToKeep); + return indicesToKeep; + } + } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/SHPhysicsConstants.h b/SHADE_Engine/src/Physics/SHPhysicsConstants.h index f252fce5..b680515e 100644 --- a/SHADE_Engine/src/Physics/SHPhysicsConstants.h +++ b/SHADE_Engine/src/Physics/SHPhysicsConstants.h @@ -34,11 +34,17 @@ namespace SHADE */ static constexpr float SHPHYSICS_RESTITUTION_THRESHOLD = 1.0f; - /** + /** * @brief * Scaling factor to control how fast overlaps are resolved.
* 1 is ideal for instant correction, but values close to 1 can lead to overshoot. - */ - static constexpr float SHPHYSICS_BAUMGARTE = 0.2f; + */ + static constexpr float SHPHYSICS_BAUMGARTE = 0.2f; + + /** + * @brief + * Distance threshold to consider two contacts as the same. + */ + static constexpr float SHPHYSICS_SAME_CONTACT_DISTANCE = 0.01; }