From 8ca4045d555d3002d66ff6d358c758dd606b0145 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Thu, 5 Jan 2023 17:53:48 +0800 Subject: [PATCH] R for retard --- Assets/Scenes/PhysicsSandbox.shade | 4 +- .../Collision/Narrowphase/SHCollision.h | 3 +- .../Narrowphase/SHConvexVsConvex.cpp | 109 ++++++++++++------ .../Narrowphase/SHSphereVsConvex.cpp | 1 + .../src/Physics/Dynamics/SHContactSolver.cpp | 5 +- .../src/Physics/Dynamics/SHContactSolver.h | 3 - SHADE_Engine/src/Physics/SHPhysicsConstants.h | 44 +++++++ 7 files changed, 128 insertions(+), 41 deletions(-) create mode 100644 SHADE_Engine/src/Physics/SHPhysicsConstants.h diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 0b732f64..d30c8f07 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -280,7 +280,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0.524352431, y: 11.5, z: 0.0463808179} + Translate: {x: 0.524352431, y: 13.5, z: 0.0463808179} Rotate: {x: -0, y: 0.785398066, z: 0.785398185} Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881} IsActive: true @@ -291,7 +291,7 @@ Drag: 0.00999999978 Angular Drag: 0.00999999978 Use Gravity: true - Gravity Scale: 1 + Gravity Scale: 0.5 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 0fe0b3f7..980914c1 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -108,6 +108,7 @@ namespace SHADE 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: @@ -115,7 +116,7 @@ namespace SHADE * * * - * static int32_t findIncidentFace (SHConvexPolyhedron& poly, const SHVec3& normal) noexcept; + * * static uint32_t clip [sutherland-hodgemann clipping] * * ! References diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index 5924ee73..be43af8f 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -17,6 +17,7 @@ #include "Math/SHMathHelpers.h" #include "Math/Geometry/SHPlane.h" #include "Physics/Collision/Shapes/SHConvexPolyhedron.h" +#include "Physics/SHPhysicsConstants.h" #include "Tools/Utilities/SHUtilities.h" namespace SHADE @@ -47,24 +48,7 @@ namespace SHADE bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - /* - * TODO: - * - * 1. Query face directions of a to b. Exit early if separation found. - * 2. Query face directions of b to a. Exit early if separation found. - * 3. Query edge directions of a & b. Exit early if separation found. - * - * (*)!! Apply weight to improve frame coherence of normal directions. DONT FORGET FLIP FLOP! - * 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. - * - */ + static constexpr float TOLERANCE = 0.1f + SHPHYSICS_LINEAR_SLOP; const SHConvexPolyhedron& POLY_A = dynamic_cast(A); const SHConvexPolyhedron& POLY_B = dynamic_cast(B); @@ -81,14 +65,33 @@ namespace SHADE if (EDGE_QUERY.bestDistance > 0.0f) return false; - const FaceQuery& BEST_FACE_QUERY = FACE_QUERY_A.bestDistance > FACE_QUERY_B.bestDistance ? FACE_QUERY_A : FACE_QUERY_B; + // Apply weight to improve frame coherence of normal directions. + // We want a normal in the direction from A -> B, so we flip the normal if needed. + bool flipNormal = false; + const SHConvexPolyhedron* referencePoly = nullptr; + const SHConvexPolyhedron* incidentPoly = nullptr; + + FaceQuery minFaceQuery; + if (FACE_QUERY_A.bestDistance + TOLERANCE > FACE_QUERY_B.bestDistance) + { + minFaceQuery = FACE_QUERY_A; + referencePoly = &POLY_A; + incidentPoly = &POLY_B; + } + else + { + minFaceQuery = FACE_QUERY_B; + referencePoly = &POLY_B; + incidentPoly = &POLY_A; + flipNormal = true; + } uint32_t numContacts = 0; + // If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on // each edge and use that as the contact point. - // Artificially increase the depth of this collision by a small tolerance to tend towards picking a face query in the next frame. - if (EDGE_QUERY.bestDistance > BEST_FACE_QUERY.bestDistance) + if (EDGE_QUERY.bestDistance > minFaceQuery.bestDistance + TOLERANCE) { const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA); const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB); @@ -122,7 +125,24 @@ namespace SHADE return true; } - // Use a bias to favour a normal in the direction of A -> B + const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace); + const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL); + + + /* + * 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. + * + */ @@ -289,25 +309,48 @@ namespace SHADE const SHVec3 C = TAIL_B - TAIL_A; - const float VB_DOT_VA = SHVec3::Dot(VB, VA); - const float VB_DOT_VB = SHVec3::Dot(VB, VB); - const float AV_DOT_VA = SHVec3::Dot(-VA, VA); - const float AV_DOT_VB = SHVec3::Dot(-VA, VB); - const float C_DOT_VA = SHVec3::Dot(C, VA); - const float C_DOT_VB = SHVec3::Dot(C, VB); + const float VB_DOT_VA = SHVec3::Dot(VB, VA); // a1 + const float VB_DOT_VB = SHVec3::Dot(VB, VB); // a2 + const float AV_DOT_VA = SHVec3::Dot(-VA, VA); // b1 + const float AV_DOT_VB = SHVec3::Dot(-VA, VB); // b2 + const float C_DOT_VA = SHVec3::Dot(C, VA); // c1 + const float C_DOT_VB = SHVec3::Dot(C, VB); // c2 /* - * R = -c2 / ( b2 - (a2 * (c1 + b1)) / a1 ) - * S = (b1 / a1) * (c2 / ( b2 - (a2 * b1) / a1 )) - (c1 / a1) - * * We only need to solve for R + * R = (c1a2 / a1 - c2) / ( b2 - a2b1/a1 ) */ - - const float R = -C_DOT_VB / (AV_DOT_VB - (VB_DOT_VB * (C_DOT_VA + AV_DOT_VA)) / VB_DOT_VA); + + const float A2_OVER_A1 = 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 = NUMERATOR / DENOMINATOR; // Just take a point from A since it's A -> B return TAIL_A + R * VA; } + int32_t SHCollision::findIncidentFace(const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept + { + // Get the most anti-parallel face to the normal + int32_t bestFace = 0; + float bestProjection = std::numeric_limits::max(); + + const int32_t NUM_FACES = poly.GetFaceCount(); + for (const int32_t i : std::views::iota(0, NUM_FACES)) + { + const SHVec3 INC_NORMAL = poly.GetNormal(i); + const float PROJECTION = SHVec3::Dot(INC_NORMAL, normal); + + if (PROJECTION < bestProjection) + { + bestProjection = PROJECTION; + bestFace = i; + } + } + + return bestFace; + } + } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index 2b08313b..e6a961e0 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -19,6 +19,7 @@ #include "Math/SHMathHelpers.h" #include "Physics/Collision/Shapes/SHSphere.h" #include "Physics/Collision/Shapes/SHConvexPolyhedron.h" +#include "Physics/SHPhysicsConstants.h" namespace SHADE { diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index ed4c9aa8..fa6f1266 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -15,6 +15,7 @@ // Project Headers #include "Math/SHMathHelpers.h" +#include "Physics/SHPhysicsConstants.h" namespace SHADE { @@ -190,8 +191,8 @@ namespace SHADE const SHVec3 RV_B = vB + SHVec3::Cross(wB, contact.rB); const float RV_N = SHVec3::Dot(RV_B - RV_A, constraint.normal); - const float ERROR_BIAS = BAUMGARTE_FACTOR * INV_DT * std::min(0.0f, -contact.penetration + PENETRATION_SLOP); - const float RESTITUTION_BIAS = -constraint.restitution * RV_N; + const float ERROR_BIAS = SHPHYSICS_BAUMGARTE * INV_DT * std::min(0.0f, -contact.penetration + SHPHYSICS_LINEAR_SLOP); + const float RESTITUTION_BIAS = std::fabs(RV_N) > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f; contact.bias = ERROR_BIAS + RESTITUTION_BIAS; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h index 56955f74..3146a743 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h @@ -78,9 +78,6 @@ namespace SHADE /* Data Members */ /*---------------------------------------------------------------------------------*/ - static constexpr float BAUMGARTE_FACTOR = 0.2f; - static constexpr float PENETRATION_SLOP = 0.05f; - VelocityStates velocityStates; ContactConstraints contactConstraints; diff --git a/SHADE_Engine/src/Physics/SHPhysicsConstants.h b/SHADE_Engine/src/Physics/SHPhysicsConstants.h new file mode 100644 index 00000000..0d8f6fc8 --- /dev/null +++ b/SHADE_Engine/src/Physics/SHPhysicsConstants.h @@ -0,0 +1,44 @@ +/**************************************************************************************** + * \file SHPhysicsConstants.h + * \author Diren D Bharwani, diren.dbharwani, 390002520 + * \brief Definitions for constants used in physics simulations + * + * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or + * disclosure of this file or its contents without the prior written consent + * of DigiPen Institute of Technology is prohibited. +****************************************************************************************/ + +#pragma once + +// Project Includes +#include "Math/SHMathHelpers.h" + +namespace SHADE +{ + /** + * @brief + * The number of simulations length for every real world unit meter.
+ * Modify this to change the global scale of the simulation. + */ + static constexpr float SHPHYSICS_LENGTHS_PER_UNIT_METER = 1.0f; + + /** + * @brief + * Linear Collision & Constraint tolerance. + */ + static constexpr float SHPHYSICS_LINEAR_SLOP = 0.005f * SHPHYSICS_LENGTHS_PER_UNIT_METER; + + /** + * @brief + * Velocity threshold for restitution to be applied. + */ + 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; + +}