From 19bffc91247f92774083390d1e318179972cadb8 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Mon, 16 Jan 2023 02:44:27 +0800 Subject: [PATCH] First half of re-implementing face-face contact derivation --- Assets/Scenes/PhysicsSandbox.shade | 32 ++-- SHADE_Engine/src/Math/SHMathHelpers.h | 2 +- .../Physics/Collision/Contacts/SHContact.h | 12 +- .../Collision/Narrowphase/SHCollision.h | 40 ++--- .../Narrowphase/SHConvexVsConvex.cpp | 149 ++++++++++++++++-- .../src/Physics/Dynamics/SHContactSolver.cpp | 52 +++--- SHADE_Engine/src/Physics/SHPhysicsConstants.h | 2 +- 7 files changed, 202 insertions(+), 87 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 6d15850b..c9104ab3 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -45,7 +45,7 @@ NumberOfChildren: 0 Components: Camera Component: - Position: {x: 0, y: 0, z: 7} + Position: {x: 0, y: 2, z: 7} Pitch: 0 Yaw: 0 Roll: 0 @@ -62,14 +62,14 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: -1.45715916, y: 7.37748241, z: 0.227711335} - Rotate: {x: -0, y: 0, z: -0} + Translate: {x: -1.5, y: 7.5, z: 0} + Rotate: {x: -0, y: 0, z: 0.785398185} Scale: {x: 1, y: 1, z: 1} IsActive: true RigidBody Component: Type: Dynamic Auto Mass: false - Mass: 10 + Mass: 0.52359879 Drag: 0.00999999978 Angular Drag: 0 Use Gravity: true @@ -84,7 +84,7 @@ Freeze Rotation Z: false IsActive: true Collider Component: - DrawColliders: true + DrawColliders: false Colliders: - Is Trigger: false Collision Tag: 1 @@ -96,18 +96,14 @@ 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 + Scripts: ~ - EID: 2 Name: Default IsActive: true NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0, y: 4.09544182, z: 0} + Translate: {x: 0, y: 4, z: 0} Rotate: {x: -0, y: 0, z: -0.436332315} Scale: {x: 4.61071014, y: 0.999995887, z: 1} IsActive: true @@ -272,17 +268,17 @@ Components: Transform Component: Translate: {x: 1, y: 2, z: 3} - Rotate: {x: -0, y: 0.785398066, z: 0.785398185} - Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881} + Rotate: {x: 0, y: 0.785398185, z: 0.785397708} + Scale: {x: 0.999990404, y: 0.999994516, z: 0.999985456} IsActive: true RigidBody Component: Type: Dynamic Auto Mass: false Mass: 1 Drag: 0.00999999978 - Angular Drag: 0.00999999978 + Angular Drag: 0 Use Gravity: true - Gravity Scale: 0.5 + Gravity Scale: 1 Interpolate: true Sleeping Enabled: true Freeze Position X: false @@ -305,7 +301,11 @@ Position Offset: {x: 0, y: 0, z: 0} Rotation Offset: {x: 0, y: 0, z: 0} IsActive: true - Scripts: ~ + Scripts: + - Type: PhysicsTestObj + Enabled: true + forceAmount: 50 + torqueAmount: 5 - EID: 8 Name: Default IsActive: true diff --git a/SHADE_Engine/src/Math/SHMathHelpers.h b/SHADE_Engine/src/Math/SHMathHelpers.h index b053beff..1d65eb91 100644 --- a/SHADE_Engine/src/Math/SHMathHelpers.h +++ b/SHADE_Engine/src/Math/SHMathHelpers.h @@ -46,7 +46,7 @@ namespace SHADE /*---------------------------------------------------------------------------------*/ /** Standard Epsilon value for comparing Single-Precision Floating-Point values. */ - static constexpr float EPSILON = 0.001f; + static constexpr float EPSILON = 0.0001f; /** Single-Precision Floating-Point value of infinity */ static constexpr float INF = std::numeric_limits::infinity(); diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h index 0337eedc..bacf2255 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h @@ -27,20 +27,16 @@ namespace SHADE union SHContactFeatures { public: - /*---------------------------------------------------------------------------------*/ - /* Type Definit */ - /*---------------------------------------------------------------------------------*/ - /*---------------------------------------------------------------------------------*/ /* Data Members */ /*---------------------------------------------------------------------------------*/ struct { - uint8_t inI; - uint8_t outI; - uint8_t inR; - uint8_t outR; + uint8_t inI; // Incoming Incident Edge + uint8_t outI; // Outgoing Incident Edge + uint8_t inR; // Incoming Reference Edge + uint8_t outR; // Outgoing Reference Edge }; uint32_t key = std::numeric_limits::max(); diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index 2a4503ce..8012392b 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -11,6 +11,7 @@ #pragma once // Project Headers +#include "Math/Geometry/SHPlane.h" #include "Physics/Collision/Contacts/SHManifold.h" #include "Physics/Collision/Contacts/SHCollisionKey.h" #include "Physics/Collision/Shapes/SHHalfEdgeStructure.h" @@ -86,15 +87,21 @@ namespace SHADE float bestDistance = std::numeric_limits::lowest(); }; + struct ClipVertex + { + SHVec3 position; + SHContactFeatures featurePair; + }; + /*---------------------------------------------------------------------------------*/ /* Member Functions */ /*---------------------------------------------------------------------------------*/ // 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 @@ -102,29 +109,22 @@ namespace SHADE // Convex VS Convex - static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) 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; - /* - * TODO: - * - * - * - * - * - * static uint32_t clip [sutherland-hodgemann clipping] - * * ! References * https://ia801303.us.archive.org/30/items/GDC2013Gregorius/GDC2013-Gregorius.pdf * 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 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) 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 97e9dbd4..52cc2c81 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -54,6 +54,8 @@ namespace SHADE const SHConvexPolyhedron& POLY_A = dynamic_cast(A); const SHConvexPolyhedron& POLY_B = dynamic_cast(B); + // TODO: Check against cached separating axis. + const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B); if (FACE_QUERY_A.bestDistance > 0.0f) return false; @@ -78,6 +80,7 @@ namespace SHADE minFaceQuery = FACE_QUERY_A; referencePoly = &POLY_A; incidentPoly = &POLY_B; + flipNormal = false; } else { @@ -114,7 +117,7 @@ namespace SHADE SHContact contact; contact.featurePair.key = EDGE_QUERY.axis; contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB); - contact.penetration = EDGE_QUERY.bestDistance; + contact.penetration = -EDGE_QUERY.bestDistance; manifold.contacts[numContacts++] = contact; manifold.numContacts = numContacts; @@ -125,23 +128,67 @@ 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); - /* - * TODO: - * - * !! - * 4. From above, save the axis of minimum penetration (reference face) - * 5. Find the most anti-parallel face on other shape (incident face) - * 6. Clip incident face against side planes of reference face. (Sutherland-Hodgeman Clipping). - * Keep all vertices below reference face. - * 7. Reduce manifold to 4 contact points. We only need 4 contact points for a stable manifold. - * - * Remember to save IDs in queries. - * During generation of incident face, store IDs of face. - * - */ + 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. return false; } @@ -353,5 +400,77 @@ namespace SHADE return bestFace; } + std::vector SHCollision::clipPolygonWithPlane(const std::vector& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept + { + std::vector out; + + int32_t v1Index = numIn - 1; + int32_t v2Index = 0; + + float v1Distance = plane.SignedDistance(in[v1Index].position); + float v2Distance = 0.0f; + + for (int32_t i = 0; i < numIn; ++i) + { + v2Index = i; + + const SHVec3 v1Pos = in[v1Index].position; + const SHVec3 v2Pos = in[v2Index].position; + + v2Distance = plane.SignedDistance(v2Pos); + + // v1 in front, v2 behind + // keep the intersection point + if (v1Distance >= 0.0f && v2Distance < 0.0f) + { + ClipVertex intersection; + + // In case the edge is parallel, the intersection is just the start point + const bool IS_PARALLEL = SHMath::CompareFloat(SHVec3::Dot(v2Pos - v1Pos, plane.GetNormal()), 0.0f); + const float ALPHA = IS_PARALLEL ? 0.0f : v1Distance / SHVec3::Distance(v1Pos, v2Pos); + + intersection.position = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA); + intersection.featurePair.inI = in[v1Index].featurePair.outI; + intersection.featurePair.outI = 0; + intersection.featurePair.inR = 0; + intersection.featurePair.outR = planeIdx; + + out.emplace_back(intersection); + } + + // v1 behind, v2 in front + // keep intersection point & v2 + if(v1Distance < 0.0f && v2Distance >= 0.0f) + { + ClipVertex intersection; + + // In case the edge is parallel, the intersection is just the start point + const bool IS_PARALLEL = SHMath::CompareFloat(SHVec3::Dot(v2Pos - v1Pos, plane.GetNormal()), 0.0f); + const float ALPHA = IS_PARALLEL ? 0.0f : -v1Distance / SHVec3::Distance(v1Pos, v2Pos); + + intersection.position = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA); + intersection.featurePair.inI = 0; + intersection.featurePair.outI = in[v2Index].featurePair.inI; + intersection.featurePair.inR = planeIdx; + intersection.featurePair.outR = 0; + + out.emplace_back(intersection); + out.emplace_back(in[v2Index]); + } + + // both in front, keep v2 + if (v1Distance >= 0.0f && v2Distance >= 0.0f) + { + out.emplace_back(in[v2Index]); + } + + // v2 is the next v1 + v1Index = v2Index; + v1Distance = v2Distance; + } + + return out; + } + } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index b5d7c2cc..926e1fed 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -187,7 +187,7 @@ namespace SHADE * restituion bias = restitution * (relative velocity /dot normal) */ - const float ERROR_BIAS = SHPHYSICS_BAUMGARTE * INV_DT * std::min(0.0f, -contact.penetration + SHPHYSICS_LINEAR_SLOP); + const float ERROR_BIAS = -SHPHYSICS_BAUMGARTE * INV_DT * std::max(0.0f, contact.penetration - SHPHYSICS_LINEAR_SLOP); // Warm starting // Compute impulses @@ -244,6 +244,31 @@ namespace SHADE SHVec3 velocityB = vB + SHVec3::Cross(wB, contact.rB); SHVec3 relativeVelocity = velocityB - velocityA; + // Get scalar of relative velocity along the normal + const float VN = SHVec3::Dot(relativeVelocity, constraint.normal); + + // Compute true normal impulse + const float OLD_NORMAL_IMPULSE = contact.normalImpulse; + + float newNormalImpulse = -(VN + contact.bias) * contact.normalMass; + contact.normalImpulse = std::max(OLD_NORMAL_IMPULSE + newNormalImpulse, 0.0f); + newNormalImpulse = contact.normalImpulse - OLD_NORMAL_IMPULSE; + + const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal; + + // Apply impulses + vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A; + wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A; + + vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B; + wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B; + + // Solve normal impulse + // Re-compute relative velocity + velocityA = vA + SHVec3::Cross(wA, contact.rA); + velocityB = vB + SHVec3::Cross(wB, contact.rB); + relativeVelocity = velocityB - velocityA; + // Solve tangent impulse for (int j = 0; j < SHContact::NUM_TANGENTS; ++j) { @@ -269,31 +294,6 @@ namespace SHADE vB += TANGENT_IMPULSE * constraint.invMassB * LINEAR_LOCK_B; wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE) * ANGULAR_LOCK_B; } - - // Solve normal impulse - // Re-compute relative velocity - velocityA = vA + SHVec3::Cross(wA, contact.rA); - velocityB = vB + SHVec3::Cross(wB, contact.rB); - relativeVelocity = velocityB - velocityA; - - // Get scalar of relative velocity along the normal - const float VN = SHVec3::Dot(relativeVelocity, constraint.normal); - - // Compute true normal impulse - const float OLD_NORMAL_IMPULSE = contact.normalImpulse; - - float newNormalImpulse = -(VN + contact.bias) * contact.normalMass; - contact.normalImpulse = std::max(OLD_NORMAL_IMPULSE + newNormalImpulse, 0.0f); - newNormalImpulse = contact.normalImpulse - OLD_NORMAL_IMPULSE; - - const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal; - - // Apply impulses - vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A; - wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A; - - vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B; - wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B; } velocityStateA.LinearVelocity = vA; diff --git a/SHADE_Engine/src/Physics/SHPhysicsConstants.h b/SHADE_Engine/src/Physics/SHPhysicsConstants.h index a6cbd608..f252fce5 100644 --- a/SHADE_Engine/src/Physics/SHPhysicsConstants.h +++ b/SHADE_Engine/src/Physics/SHPhysicsConstants.h @@ -26,7 +26,7 @@ namespace SHADE * @brief * Linear Collision & Constraint tolerance. */ - static constexpr float SHPHYSICS_LINEAR_SLOP = 0.05f * SHPHYSICS_LENGTHS_PER_UNIT_METER; + static constexpr float SHPHYSICS_LINEAR_SLOP = 0.01f * SHPHYSICS_LENGTHS_PER_UNIT_METER; /** * @brief