diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 8f439326..44e5e03f 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -45,7 +45,7 @@ NumberOfChildren: 0 Components: Camera Component: - Position: {x: 0, y: 2, z: 7} + Position: {x: 0, y: 2, z: 10} Pitch: 0 Yaw: 0 Roll: 0 @@ -62,7 +62,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: -1, y: 7.5, z: 0} + Translate: {x: -2, y: 7.5, z: 0} Rotate: {x: -0, y: 0, z: 0.785398185} Scale: {x: 1, y: 1, z: 1} IsActive: true @@ -77,7 +77,7 @@ Interpolate: true Sleeping Enabled: true Freeze Position X: false - Freeze Position Y: false + Freeze Position Y: true Freeze Position Z: false Freeze Rotation X: false Freeze Rotation Y: false @@ -267,8 +267,8 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 2, y: 2, z: 3} - Rotate: {x: 0.785398185, y: 0.785398185, z: 0.785398185} + Translate: {x: 0, y: 7, z: 0} + Rotate: {x: 0, y: 0.785398185, z: 0} Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337} IsActive: true RigidBody Component: diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h index 3dbb16d0..15142303 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h @@ -38,7 +38,7 @@ namespace SHADE { public: SHCollisionKey info; - SHCollisionState state; + SHCollisionState state = SHCollisionState::INVALID; }; /** @@ -52,7 +52,7 @@ namespace SHADE static constexpr int MAX_NUM_CONTACTS = 4; SHCollisionKey info; - SHCollisionState state; + SHCollisionState state = SHCollisionState::INVALID; SHVec3 normal; SHVec3 contactPoints[MAX_NUM_CONTACTS]; }; diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h index 3b591875..e60e5329 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h @@ -12,6 +12,7 @@ // Primary Header #include "Physics/Dynamics/SHRigidBody.h" +#include "Physics/Collision/Narrowphase/SHSATInfo.h" #include "Physics/Collision/Shapes/SHCollisionShape.h" #include "SHContact.h" #include "SHCollisionEvents.h" @@ -58,6 +59,8 @@ namespace SHADE uint32_t numContacts = 0; SHCollisionState state = SHCollisionState::INVALID; + SHSATInfo cachedSATInfo; + SHVec3 normal; SHVec3 tangents[SHContact::NUM_TANGENTS]; SHContact contacts[MAX_NUM_CONTACTS]; diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp index 23be8c79..f1b93a43 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp @@ -28,13 +28,14 @@ namespace SHADE } inline SHManifold::SHManifold(const SHManifold& rhs) noexcept - : shapeA { rhs.shapeA } - , shapeB { rhs.shapeB } - , bodyA { rhs.bodyA } - , bodyB { rhs.bodyB } - , numContacts { rhs.numContacts } - , state { rhs.state } - , normal { rhs.normal } + : shapeA { rhs.shapeA } + , shapeB { rhs.shapeB } + , bodyA { rhs.bodyA } + , bodyB { rhs.bodyB } + , numContacts { rhs.numContacts } + , state { rhs.state } + , cachedSATInfo { rhs.cachedSATInfo } + , normal { rhs.normal } { static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast(MAX_NUM_CONTACTS); memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS); @@ -44,13 +45,14 @@ namespace SHADE } inline SHManifold::SHManifold(SHManifold&& rhs) noexcept - : shapeA { rhs.shapeA } - , shapeB { rhs.shapeB } - , bodyA { rhs.bodyA } - , bodyB { rhs.bodyB } - , numContacts { rhs.numContacts } - , state { rhs.state } - , normal { rhs.normal } + : shapeA { rhs.shapeA } + , shapeB { rhs.shapeB } + , bodyA { rhs.bodyA } + , bodyB { rhs.bodyB } + , numContacts { rhs.numContacts } + , state { rhs.state } + , cachedSATInfo { rhs.cachedSATInfo } + , normal { rhs.normal } { static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast(MAX_NUM_CONTACTS); memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS); @@ -68,13 +70,14 @@ namespace SHADE if (this == &rhs) return *this; - shapeA = rhs.shapeA; - shapeB = rhs.shapeB; - bodyA = rhs.bodyA; - bodyB = rhs.bodyB; - numContacts = rhs.numContacts; - state = rhs.state; - normal = rhs.normal; + shapeA = rhs.shapeA; + shapeB = rhs.shapeB; + bodyA = rhs.bodyA; + bodyB = rhs.bodyB; + numContacts = rhs.numContacts; + state = rhs.state; + cachedSATInfo = rhs.cachedSATInfo; + normal = rhs.normal; static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast(MAX_NUM_CONTACTS); memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS); @@ -87,13 +90,14 @@ namespace SHADE inline SHManifold& SHManifold::operator=(SHManifold&& rhs) noexcept { - shapeA = rhs.shapeA; - shapeB = rhs.shapeB; - bodyA = rhs.bodyA; - bodyB = rhs.bodyB; - numContacts = rhs.numContacts; - state = rhs.state; - normal = rhs.normal; + shapeA = rhs.shapeA; + shapeB = rhs.shapeB; + bodyA = rhs.bodyA; + bodyB = rhs.bodyB; + numContacts = rhs.numContacts; + state = rhs.state; + cachedSATInfo = rhs.cachedSATInfo; + normal = rhs.normal; static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast(MAX_NUM_CONTACTS); memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS); diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index e78b33fa..060d42cc 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -139,6 +139,8 @@ namespace SHADE , const SHConvexPolyhedron& B ) noexcept; + + static EdgeQuery queryEdgeDirections ( const SHConvexPolyhedron& A @@ -207,5 +209,14 @@ namespace SHADE , const SHVec3& faceNormal ) noexcept; + // Cached Convex VS Convex + + static bool cachedConvexVSConvex + ( + SHManifold& manifold + , const SHSATInfo& cachedInfo + , const SHConvexPolyhedron& A + , const SHConvexPolyhedron& B + ) 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 71539949..f2d6fcb3 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -48,25 +48,56 @@ namespace SHADE bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - static constexpr float ABSOLUTE_TOLERANCE = 0.01f; - static constexpr float RELATIVE_TOLERANCE = 0.95f; + static constexpr float ABSOLUTE_TOLERANCE = 0.01f; // 0.0005 + static constexpr float RELATIVE_TOLERANCE = 0.95f; // 1.0002 const SHConvexPolyhedron& POLY_A = dynamic_cast(A); const SHConvexPolyhedron& POLY_B = dynamic_cast(B); - // TODO: Check against cached separating axis. + if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID) + return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B); + + SHSATInfo cachedInfo; const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B); if (FACE_QUERY_A.bestDistance > 0.0f) + { + // cache the info + cachedInfo.type = SHSATInfo::Type::FACE; + cachedInfo.info.refPoly = SHSATInfo::ShapeID::A; + cachedInfo.info.refFace = FACE_QUERY_A.closestFace; + + manifold.cachedSATInfo = cachedInfo; + return false; + } + const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A); if (FACE_QUERY_B.bestDistance > 0.0f) + { + // cache the info + cachedInfo.type = SHSATInfo::Type::FACE; + cachedInfo.info.refPoly = SHSATInfo::ShapeID::B; + cachedInfo.info.refFace = FACE_QUERY_B.closestFace; + + manifold.cachedSATInfo = cachedInfo; + return false; + } const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B); if (EDGE_QUERY.bestDistance > 0.0f) + { + // cache the info + cachedInfo.type = SHSATInfo::Type::EDGE; + cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA; + cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB; + + manifold.cachedSATInfo = cachedInfo; + return false; + } // 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. @@ -81,6 +112,10 @@ namespace SHADE referencePoly = &POLY_A; incidentPoly = &POLY_B; flipNormal = false; + + cachedInfo.type = SHSATInfo::Type::FACE; + cachedInfo.info.refPoly = SHSATInfo::ShapeID::A; + cachedInfo.info.refFace = minFaceQuery.closestFace; } else { @@ -88,9 +123,13 @@ namespace SHADE referencePoly = &POLY_B; incidentPoly = &POLY_A; flipNormal = true; + + cachedInfo.type = SHSATInfo::Type::FACE; + cachedInfo.info.refPoly = SHSATInfo::ShapeID::B; + cachedInfo.info.refFace = minFaceQuery.closestFace; } - uint32_t numContacts = 0; + // If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on @@ -114,6 +153,8 @@ namespace SHADE manifold.normal = -manifold.normal; // In this scenario, we only have one contact + uint32_t numContacts = 0; + SHContact contact; contact.featurePair.key = EDGE_QUERY.axis; contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB); @@ -122,12 +163,20 @@ namespace SHADE manifold.contacts[numContacts++] = contact; manifold.numContacts = numContacts; + // Cache the info + cachedInfo.type = SHSATInfo::Type::EDGE; + cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA; + cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB; + + manifold.cachedSATInfo = cachedInfo; + return true; } const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace); const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL); + manifold.cachedSATInfo = cachedInfo; return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal); } @@ -268,6 +317,7 @@ namespace SHADE * (LB(s) - LA(r)) /dot VB = 0 * * Where LB(s) - LA(r) is the same vector as VB X VA. + * */ const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA); @@ -291,15 +341,17 @@ namespace SHADE * c = C' /dot U * * U is either VA or VB + * + * TODO: Check if segments degenerate into a point */ const SHVec3 C = TAIL_B - TAIL_A; - 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 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 /* @@ -310,6 +362,8 @@ 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; + + // We clamp it because the factor cannot be beyond [0,1] as it would be beyond the segment if it was so. const float R = std::clamp(NUMERATOR / DENOMINATOR, 0.0f, 1.0f); // Just take a point from A since it's A -> B @@ -687,5 +741,82 @@ namespace SHADE return indicesToKeep; } + bool SHCollision::cachedConvexVSConvex(SHManifold& manifold, const SHSATInfo& cachedInfo, const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept + { + if (cachedInfo.type == SHSATInfo::Type::FACE) + { + SHASSERT(cachedInfo.info.refPoly != SHSATInfo::ShapeID::INVALID, "Attempted to perform cached SAT with an invalid cached collision!") + + // Assume the reference poly was A + const SHConvexPolyhedron* incPoly = &B; + const SHConvexPolyhedron* refPoly = &A; + bool flip = false; + + // Swap if it was B + if (cachedInfo.info.refPoly == SHSATInfo::ShapeID::B) + { + refPoly = &B; + incPoly = &A; + flip = true; + } + + const SHHalfEdgeStructure::Face& REF_FACE = refPoly->GetFace(cachedInfo.info.refFace); + const SHVec3 REF_NORMAL = refPoly->GetNormal(cachedInfo.info.refFace); + + const SHVec3 SUPPORT_POINT = incPoly->FindSupportPoint(-REF_NORMAL); + + const SHPlane REF_FACE_PLANE { refPoly->GetVertex(REF_FACE.vertexIndices[0].index), REF_NORMAL }; + const float DISTANCE = REF_FACE_PLANE.SignedDistance(SUPPORT_POINT); + + if (DISTANCE > 0.0f) + return false; + + // Find the incident face + const int32_t INCIDENT_FACE_INDEX = findIncidentFace(*incPoly, REF_NORMAL); + + return findFaceContacts(manifold, *incPoly, INCIDENT_FACE_INDEX, *refPoly, cachedInfo.info.refFace, flip); + } + + if (cachedInfo.type == SHSATInfo::Type::EDGE) + { + const float DISTANCE = distanceBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); + if (DISTANCE > 0.0f) + return false; + + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(cachedInfo.info.edgeA); + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(cachedInfo.info.edgeB); + + const SHVec3 HEAD_A = A.GetVertex(HALF_EDGE_A.headVertexIndex); + const SHVec3 TAIL_A = A.GetVertex(HALF_EDGE_A.tailVertexIndex); + const SHVec3 HEAD_B = B.GetVertex(HALF_EDGE_B.headVertexIndex); + const SHVec3 TAIL_B = B.GetVertex(HALF_EDGE_B.tailVertexIndex); + + const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A); + const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B); + + manifold.normal = SHVec3::Cross(VB, VA); + // Flip normal if need to ( A -> B) + if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f) + manifold.normal = -manifold.normal; + + // In this scenario, we only have one contact + uint32_t numContacts = 0; + + SHContact contact; + // Take feature pair key from previous manifold + contact.featurePair.key = manifold.contacts[0].featurePair.key; + contact.position = findClosestPointBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); + contact.penetration = -DISTANCE; + + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; + + return true; + } + + // Should never reach this. + return false; + } + } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSATInfo.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSATInfo.h new file mode 100644 index 00000000..25606084 --- /dev/null +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSATInfo.h @@ -0,0 +1,87 @@ +/**************************************************************************************** + * \file SHSATInfo.h + * \author Diren D Bharwani, diren.dbharwani, 390002520 + * \brief Interface for storing information of collision detection between two + * convex shapes using SAT. + * + * \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 + +#include // int32_t + +namespace SHADE +{ + /** + * @brief + * Primarily used to cached collision information between two convex polyhedrons for + * temporal coherence. + */ + struct SHSATInfo + { + public: + /*---------------------------------------------------------------------------------*/ + /* Type Definitions */ + /*---------------------------------------------------------------------------------*/ + + enum class ShapeID : int32_t + { + A = 0 + , B = 1 + + , INVALID = -1 + }; + + enum class Type + { + EDGE + , FACE + + , INVALID = -1 + }; + + union ContactInfo + { + struct // FaceContact + { + ShapeID refPoly; + int32_t refFace; + }; + + struct // Edge Contact + { + int32_t edgeA; + int32_t edgeB; + }; + }; + + /*---------------------------------------------------------------------------------*/ + /* Data Members */ + /*---------------------------------------------------------------------------------*/ + + Type type; + ContactInfo info; + + /*---------------------------------------------------------------------------------*/ + /* Constructors & Destructor */ + /*---------------------------------------------------------------------------------*/ + + SHSATInfo () noexcept; + SHSATInfo (const SHSATInfo& rhs) noexcept; + SHSATInfo (SHSATInfo&& rhs) noexcept; + ~SHSATInfo () noexcept = default; + + /*---------------------------------------------------------------------------------*/ + /* Operator Overloads */ + /*---------------------------------------------------------------------------------*/ + + SHSATInfo& operator= (const SHSATInfo& rhs) noexcept; + SHSATInfo& operator= (SHSATInfo&& rhs) noexcept; + }; + +} // namespace SHADE + +#include "SHSATInfo.hpp" diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSATInfo.hpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSATInfo.hpp new file mode 100644 index 00000000..8e51d54b --- /dev/null +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSATInfo.hpp @@ -0,0 +1,71 @@ +/**************************************************************************************** + * \file SHSATInfo.hpp + * \author Diren D Bharwani, diren.dbharwani, 390002520 + * \brief Implementation of inlined methods for cached SAT Info. + * + * \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 + +// Primary Header +#include "SHSATInfo.h" + +namespace SHADE +{ + /*-----------------------------------------------------------------------------------*/ + /* Constructors & Destructor */ + /*-----------------------------------------------------------------------------------*/ + + inline SHSATInfo::SHSATInfo() noexcept + : type { Type::INVALID } + , info {} + { + info.refPoly = ShapeID::INVALID; + info.refFace = -1; + } + + inline SHSATInfo::SHSATInfo(const SHSATInfo& rhs) noexcept + : type { rhs.type } + , info {} + { + info.refPoly = rhs.info.refPoly; + info.refFace = rhs.info.refFace; + + } + + inline SHSATInfo::SHSATInfo(SHSATInfo&& rhs) noexcept + : type { rhs.type } + , info {} + { + info.refPoly = rhs.info.refPoly; + info.refFace = rhs.info.refFace; + } + + /*---------------------------------------------------------------------------------- */ + /* Operator Overloads */ + /*---------------------------------------------------------------------------------- */ + + inline SHSATInfo& SHSATInfo::operator=(const SHSATInfo& rhs) noexcept + { + if (this == &rhs) + return *this; + + type = rhs.type; + info.refPoly = rhs.info.refPoly; + info.refFace = rhs.info.refFace; + + return *this; + } + + inline SHSATInfo& SHSATInfo::operator=(SHSATInfo&& rhs) noexcept + { + type = rhs.type; + info.refPoly = rhs.info.refPoly; + info.refFace = rhs.info.refFace; + + return *this; + } +}