From abdf614083705c38024a97ec41165f19023135e2 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Wed, 1 Mar 2023 00:55:44 +0800 Subject: [PATCH] Contacts are solved locally on each body --- Assets/Scenes/PhysicsSandbox.shade | 8 +- Assets/Scenes/SS_Playground.shade | 82 ++ Assets/Scenes/SS_Playground.shade.shmeta | 3 + .../Physics/Collision/Contacts/SHContact.h | 4 +- .../Collision/Narrowphase/SHCollision.h | 148 +-- .../Narrowphase/SHCollisionUtils.cpp | 38 + .../Collision/Narrowphase/SHCollisionUtils.h | 108 +++ .../Narrowphase/SHConvexVsConvex.cpp | 894 +++++++++--------- .../Narrowphase/SHSphereVsConvex.cpp | 179 ++-- .../Narrowphase/SHSphereVsSphere.cpp | 20 +- .../src/Physics/Dynamics/SHContactManager.cpp | 14 +- .../src/Physics/Dynamics/SHContactSolver.cpp | 8 +- 12 files changed, 838 insertions(+), 668 deletions(-) create mode 100644 Assets/Scenes/SS_Playground.shade create mode 100644 Assets/Scenes/SS_Playground.shade.shmeta create mode 100644 SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.cpp create mode 100644 SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.h diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 0394e247..5a4ab9fd 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -182,9 +182,9 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0, y: 7, z: 0} - Rotate: {x: 0, y: 0, z: 0.785398185} - Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337} + Translate: {x: 0, y: 2, z: 3} + Rotate: {x: 0, y: 0.785398185, z: 0} + Scale: {x: 0.999988496, y: 0.999994099, z: 0.999984443} IsActive: true RigidBody Component: Type: Dynamic @@ -229,7 +229,7 @@ Transform Component: Translate: {x: 0, y: 0, z: 3} Rotate: {x: -0, y: 0, z: -0} - Scale: {x: 1, y: 1, z: 1} + Scale: {x: 5, y: 1, z: 5} IsActive: true Collider Component: DrawColliders: false diff --git a/Assets/Scenes/SS_Playground.shade b/Assets/Scenes/SS_Playground.shade new file mode 100644 index 00000000..ffaca467 --- /dev/null +++ b/Assets/Scenes/SS_Playground.shade @@ -0,0 +1,82 @@ +- EID: 0 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Camera Component: + Position: {x: 0, y: 2, z: 7} + Pitch: 0 + Yaw: 0 + Roll: 0 + Width: 1920 + Height: 1080 + Near: 0.00999999978 + Far: 10000 + Perspective: true + IsActive: true + Scripts: ~ +- EID: 1 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 0, y: 0, z: 0} + Rotate: {x: 0, y: 0, z: 0} + Scale: {x: 1, y: 1, z: 1} + IsActive: true + Collider Component: + DrawColliders: true + Colliders: + - Is Trigger: false + Collision Tag: 1 + Type: Sphere + Radius: 2 + Friction: 0.400000006 + Bounciness: 0 + Density: 1 + Position Offset: {x: 0, y: 0, z: 0} + Rotation Offset: {x: 0, y: 0, z: 0} + IsActive: true + Scripts: ~ +- EID: 2 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 0, y: 3, z: 0} + Rotate: {x: 0, y: 0, z: 0} + Scale: {x: 1, y: 1, z: 1} + IsActive: true + RigidBody Component: + Type: Dynamic + Auto Mass: false + Mass: 1 + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + Use Gravity: true + Gravity Scale: 1 + Interpolate: true + Sleeping Enabled: true + Freeze Position X: false + Freeze Position Y: false + Freeze Position Z: false + Freeze Rotation X: false + Freeze Rotation Y: false + Freeze Rotation Z: false + IsActive: true + Collider Component: + DrawColliders: true + Colliders: + - Is Trigger: false + Collision Tag: 1 + Type: Sphere + Radius: 1 + Friction: 0.400000006 + Bounciness: 0 + Density: 1 + Position Offset: {x: 0, y: 0, z: 0} + Rotation Offset: {x: 0, y: 0, z: 0} + IsActive: true + Scripts: ~ \ No newline at end of file diff --git a/Assets/Scenes/SS_Playground.shade.shmeta b/Assets/Scenes/SS_Playground.shade.shmeta new file mode 100644 index 00000000..3b3496a8 --- /dev/null +++ b/Assets/Scenes/SS_Playground.shade.shmeta @@ -0,0 +1,3 @@ +Name: SS_Playground +ID: 92914350 +Type: 5 diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h index bacf2255..73fd0ac6 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h @@ -62,7 +62,9 @@ namespace SHADE float tangentImpulse[NUM_TANGENTS] = { 0.0f }; // Accumulated tangent impulses float tangentMass[NUM_TANGENTS] = { 0.0f }; // Effective masses along the tangents - SHVec3 position; + // We store points locally for each contact + SHVec3 localPointA; + SHVec3 localPointB; SHVec3 rA; // Vector from COM of A to the contact SHVec3 rB; // Vector from COM of B to the contact SHContactFeatures featurePair; diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index 060d42cc..9a3ef9a2 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -14,7 +14,6 @@ #include "Math/Geometry/SHPlane.h" #include "Physics/Collision/Contacts/SHManifold.h" #include "Physics/Collision/Contacts/SHCollisionKey.h" -#include "Physics/Collision/Shapes/SHHalfEdgeStructure.h" namespace SHADE @@ -68,155 +67,10 @@ namespace SHADE [[nodiscard]] static bool ConvexVsConvex (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept; private: - /*---------------------------------------------------------------------------------*/ - /* Type Definitions */ - /*---------------------------------------------------------------------------------*/ - - struct FaceQuery - { - bool colliding = false; // Allows for early out - int32_t closestFace = -1; - float bestDistance = std::numeric_limits::lowest(); - }; - - struct EdgeQuery - { - int32_t halfEdgeA = -1; - int32_t halfEdgeB = -1; - int32_t axis = -1; - 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; - - // Capsule VS Convex - - // TODO: Capsule VS Convex uses the same gauss map optimisation as convex vs convex - - // Convex VS Convex - - /* - * ! 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 bool findFaceContacts - ( - SHManifold& manifold - , const SHConvexPolyhedron& incPoly - , int32_t incFace - , const SHConvexPolyhedron& refPoly - , int32_t refFace - , bool flip - ) 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 - , const SHVec3& faceNormal - ) noexcept; - - // Cached Convex VS Convex - - static bool cachedConvexVSConvex - ( - SHManifold& manifold - , const SHSATInfo& cachedInfo - , const SHConvexPolyhedron& A - , const SHConvexPolyhedron& B - ) noexcept; + 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/SHCollisionUtils.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.cpp new file mode 100644 index 00000000..dd4b7439 --- /dev/null +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.cpp @@ -0,0 +1,38 @@ +/**************************************************************************************** + * \file SHCollisionUtils.cpp + * \author Diren D Bharwani, diren.dbharwani, 390002520 + * \brief Implementation for some objects to assist with collision detection + * + * \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. +****************************************************************************************/ + +#include + +// Primary Header +#include "SHCollisionUtils.h" + +namespace SHADE +{ + /*-----------------------------------------------------------------------------------*/ + /* Operator Overload Definitions */ + /*-----------------------------------------------------------------------------------*/ + + SHVec3 SHCollisionUtils::ShapeTransform::operator*(const SHVec3& rhs) const noexcept + { + return SHVec3::Rotate(rhs, orientation) + position; + } + + /*-----------------------------------------------------------------------------------*/ + /* Public Member Functions Definitions */ + /*-----------------------------------------------------------------------------------*/ + + void SHCollisionUtils::ShapeTransform::Invert() noexcept + { + orientation.Invert(); + position = SHVec3::Rotate(-position, orientation); + } + + +} // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.h new file mode 100644 index 00000000..cef216de --- /dev/null +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollisionUtils.h @@ -0,0 +1,108 @@ +/**************************************************************************************** + * \file SHCollisionUtils.h + * \author Diren D Bharwani, diren.dbharwani, 390002520 + * \brief Interface for some objects to assist with collision detection + * + * \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 Headers +#include "Math/Vector/SHVec3.h" +#include "Physics/Collision/Contacts/SHContact.h" + +namespace SHADE +{ + /*-----------------------------------------------------------------------------------*/ + /* Type Definitions */ + /*-----------------------------------------------------------------------------------*/ + + /** + * @brief + * Defines a bunch of helper objects for collision detection. + */ + class SHCollisionUtils + { + public: + /*---------------------------------------------------------------------------------*/ + /* Type Definitions */ + /*---------------------------------------------------------------------------------*/ + + struct ShapeTransform + { + public: + /*-------------------------------------------------------- ----------------------*/ + /* Data Members */ + /*-------------------------------------------------------- ----------------------*/ + + SHVec3 position; + SHQuaternion orientation; + + /*-------------------------------------------------------- ----------------------*/ + /* Operator Overloads */ + /*-------------------------------------------------------- ----------------------*/ + + SHVec3 operator* (const SHVec3& rhs) const noexcept; + + /*-------------------------------------------------------- ----------------------*/ + /* Member Functions */ + /*-------------------------------------------------------- ----------------------*/ + + void Invert() noexcept; + }; + + struct FaceQuery + { + public: + /*-------------------------------------------------------- ----------------------*/ + /* Data Members */ + /*-------------------------------------------------------- ----------------------*/ + + bool colliding = false; // Allows for early out + int32_t closestFace = -1; + float bestDistance = std::numeric_limits::lowest(); + }; + + struct EdgeQuery + { + public: + /*-------------------------------------------------------- ----------------------*/ + /* Data Members */ + /*-------------------------------------------------------- ----------------------*/ + + int32_t halfEdgeA = -1; + int32_t halfEdgeB = -1; + int32_t axis = -1; + float bestDistance = std::numeric_limits::lowest(); + }; + + struct EdgeContacts + { + public: + /*-------------------------------------------------------- ----------------------*/ + /* Data Members */ + /*-------------------------------------------------------- ----------------------*/ + + SHVec3 closestPointA; + SHVec3 closestPointB; + }; + + struct ClipVertex + { + public: + /*-------------------------------------------------------- ----------------------*/ + /* Data Members */ + /*-------------------------------------------------------- ----------------------*/ + + SHVec3 position; + SHContactFeatures featurePair; + }; + + using ClipVertices = std::vector; + }; + + +} // namespace SHAD diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index 76b948a3..9e7685fc 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -14,6 +14,7 @@ #include "SHCollision.h" // Project Headers +#include "SHCollisionUtils.h" #include "Math/SHMathHelpers.h" #include "Math/Geometry/SHPlane.h" #include "Physics/Collision/Shapes/SHConvexPolyhedron.h" @@ -22,6 +23,24 @@ namespace SHADE { + + /* + * ! References + * https://ia801303.us.archive.org/30/items/GDC2013Gregorius/GDC2013-Gregorius.pdf + * https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp + */ + + SHCollisionUtils::FaceQuery queryFaceDirections (const SHConvexPolyhedron&, const SHConvexPolyhedron&) noexcept; + SHCollisionUtils::EdgeQuery queryEdgeDirections (const SHConvexPolyhedron&, const SHConvexPolyhedron&) noexcept; + bool buildMinkowskiFace (const SHConvexPolyhedron&, const SHConvexPolyhedron&, int32_t edgeA, int32_t edgeB) noexcept; + bool isMinkowskiFace (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept; + float distanceBetweenEdges (const SHConvexPolyhedron&, const SHConvexPolyhedron&, int32_t edgeA, int32_t edgeB) noexcept; + SHCollisionUtils::EdgeContacts findClosestPointBetweenEdges (const SHConvexPolyhedron&, const SHConvexPolyhedron&, int32_t edgeA, int32_t edgeB) noexcept; + int32_t findIncidentFace (const SHConvexPolyhedron&, const SHVec3& normal) noexcept; + bool findFaceContacts (SHManifold&, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept; + SHCollisionUtils::ClipVertices clipPolygonWithPlane (const SHCollisionUtils::ClipVertices& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept; + //std::vector reduceContacts (const std::vector& in, const SHVec3& faceNormal) noexcept; + /*-----------------------------------------------------------------------------------*/ /* Public Member Functions Definitions */ /*-----------------------------------------------------------------------------------*/ @@ -31,15 +50,15 @@ namespace SHADE const SHConvexPolyhedron& POLY_A = dynamic_cast(A); const SHConvexPolyhedron& POLY_B = dynamic_cast(B); - const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B); + const SHCollisionUtils::SHCollisionUtils::FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B); if (FACE_QUERY_A.bestDistance > 0.0f) return false; - const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A); + const SHCollisionUtils::SHCollisionUtils::FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A); if (FACE_QUERY_B.bestDistance > 0.0f) return false; - const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B); + const SHCollisionUtils::EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B); if (EDGE_QUERY.bestDistance > 0.0f) return false; @@ -48,145 +67,149 @@ namespace SHADE bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - static constexpr float ABSOLUTE_TOLERANCE = 0.01f; // 0.0005 - static constexpr float RELATIVE_TOLERANCE = 0.95f; // 1.0002 + //static constexpr float ABSOLUTE_TOLERANCE = 0.01f; + //static constexpr float RELATIVE_TOLERANCE = 0.95f; - const SHConvexPolyhedron& POLY_A = dynamic_cast(A); - const SHConvexPolyhedron& POLY_B = dynamic_cast(B); + //const SHConvexPolyhedron& POLY_A = dynamic_cast(A); + //const SHConvexPolyhedron& POLY_B = dynamic_cast(B); - if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID) - return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B); + //if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID) + // return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B); - SHSATInfo cachedInfo; + //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; + //const SHCollisionUtils::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; + // manifold.cachedSATInfo = cachedInfo; - return false; - } - + // 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; + //const SHCollisionUtils::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; + // manifold.cachedSATInfo = cachedInfo; - return false; - } + // 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; + //const SHCollisionUtils::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; + // manifold.cachedSATInfo = cachedInfo; - return false; - } + // 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. - bool flipNormal = false; - const SHConvexPolyhedron* referencePoly = nullptr; - const SHConvexPolyhedron* incidentPoly = nullptr; + //// 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 + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE) - { - minFaceQuery = FACE_QUERY_A; - referencePoly = &POLY_A; - incidentPoly = &POLY_B; - flipNormal = false; + //SHCollisionUtils::FaceQuery minFaceQuery; + //if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE) + //{ + // minFaceQuery = FACE_QUERY_A; + // 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 - { - minFaceQuery = FACE_QUERY_B; - referencePoly = &POLY_B; - incidentPoly = &POLY_A; - flipNormal = true; + // cachedInfo.type = SHSATInfo::Type::FACE; + // cachedInfo.info.refPoly = SHSATInfo::ShapeID::A; + // cachedInfo.info.refFace = minFaceQuery.closestFace; + //} + //else + //{ + // minFaceQuery = FACE_QUERY_B; + // referencePoly = &POLY_B; + // incidentPoly = &POLY_A; + // flipNormal = true; - cachedInfo.type = SHSATInfo::Type::FACE; - cachedInfo.info.refPoly = SHSATInfo::ShapeID::B; - cachedInfo.info.refFace = minFaceQuery.closestFace; - } + // cachedInfo.type = SHSATInfo::Type::FACE; + // cachedInfo.info.refPoly = SHSATInfo::ShapeID::B; + // cachedInfo.info.refFace = minFaceQuery.closestFace; + //} - + // - // 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. - if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_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); + //// 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. + //if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_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); - const SHVec3 HEAD_A = POLY_A.GetVertex(HALF_EDGE_A.headVertexIndex); - const SHVec3 TAIL_A = POLY_A.GetVertex(HALF_EDGE_A.tailVertexIndex); - const SHVec3 HEAD_B = POLY_B.GetVertex(HALF_EDGE_B.headVertexIndex); - const SHVec3 TAIL_B = POLY_B.GetVertex(HALF_EDGE_B.tailVertexIndex); + // const SHVec3 HEAD_A = POLY_A.GetVertex(HALF_EDGE_A.headVertexIndex); + // const SHVec3 TAIL_A = POLY_A.GetVertex(HALF_EDGE_A.tailVertexIndex); + // const SHVec3 HEAD_B = POLY_B.GetVertex(HALF_EDGE_B.headVertexIndex); + // const SHVec3 TAIL_B = POLY_B.GetVertex(HALF_EDGE_B.tailVertexIndex); - const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A); - const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B); + // 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; + // 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; + // // 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); - contact.penetration = -EDGE_QUERY.bestDistance; + // const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(POLY_A, POLY_B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); - manifold.contacts[numContacts++] = contact; - manifold.numContacts = numContacts; + // SHContact contact; + // contact.featurePair.key = EDGE_QUERY.axis; + // contact.position = SHVec3::ClampedLerp(CONTACTS.closestPointA, CONTACTS.closestPointB, 0.5f); + // contact.penetration = -EDGE_QUERY.bestDistance; - // Cache the info - cachedInfo.type = SHSATInfo::Type::EDGE; - cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA; - cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB; + // manifold.contacts[numContacts++] = contact; + // manifold.numContacts = numContacts; - manifold.cachedSATInfo = cachedInfo; + // // Cache the info + // cachedInfo.type = SHSATInfo::Type::EDGE; + // cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA; + // cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB; - return true; - } + // manifold.cachedSATInfo = cachedInfo; - const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace); - const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL); + // return true; + //} - manifold.cachedSATInfo = cachedInfo; - return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal); + //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); + + return false; } /*-----------------------------------------------------------------------------------*/ /* Private Member Functions Definitions */ /*-----------------------------------------------------------------------------------*/ - SHCollision::FaceQuery SHCollision::queryFaceDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept + SHCollisionUtils::FaceQuery queryFaceDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept { - FaceQuery faceQuery; + SHCollisionUtils::FaceQuery faceQuery; const int32_t NUM_FACES = A.GetFaceCount(); for (const int32_t i : std::views::iota(0, NUM_FACES)) @@ -212,9 +235,9 @@ namespace SHADE return faceQuery; } - SHCollision::EdgeQuery SHCollision::queryEdgeDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept + SHCollisionUtils::EdgeQuery queryEdgeDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept { - EdgeQuery edgeQuery; + SHCollisionUtils::EdgeQuery edgeQuery; const int32_t EDGE_COUNT_A = A.GetHalfEdgeCount(); const int32_t EDGE_COUNT_B = B.GetHalfEdgeCount(); @@ -244,7 +267,7 @@ namespace SHADE return edgeQuery; } - bool SHCollision::buildMinkowskiFace(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept + bool buildMinkowskiFace(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept { // Get Half Edge from both polygons const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA); @@ -253,8 +276,6 @@ namespace SHADE const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB); const SHHalfEdgeStructure::HalfEdge& TWIN_EDGE_B = B.GetHalfEdge(HALF_EDGE_B.twinEdgeIndex); - - // Get normals from face and twin edge face const SHVec3 NA = A.GetNormal(HALF_EDGE_A.faceIndex); const SHVec3 NB = A.GetNormal(TWIN_EDGE_A.faceIndex); @@ -264,7 +285,7 @@ namespace SHADE return isMinkowskiFace(NA, NB, -NC, -ND); } - bool SHCollision::isMinkowskiFace(const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept + bool isMinkowskiFace(const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept { const SHVec3 BXA = SHVec3::Cross(b, a); const SHVec3 DXC = SHVec3::Cross(d, c); @@ -277,7 +298,7 @@ namespace SHADE return CBA * DBA < 0.0f && ADC * BDC < 0.0f && CBA * BDC > 0.0f; } - float SHCollision::distanceBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept + float distanceBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept { const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA); const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB); @@ -303,65 +324,84 @@ namespace SHADE return SHVec3::Dot(normal, HEAD_B - HEAD_A); } - SHVec3 SHCollision::findClosestPointBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept + SHCollisionUtils::EdgeContacts findClosestPointBetweenEdges(const SHConvexPolyhedron& polyA, const SHConvexPolyhedron& polyB, int32_t edgeA, int32_t edgeB) noexcept { /* * A more robust solution can be found in Real-Time Collision Detection by * Christer Ericson, page 148-149. */ - const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA); - const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB); + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = polyA.GetHalfEdge(edgeA); + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = polyB.GetHalfEdge(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 HEAD_A = polyA.GetVertex(HALF_EDGE_A.headVertexIndex); + const SHVec3 TAIL_A = polyA.GetVertex(HALF_EDGE_A.tailVertexIndex); + const SHVec3 HEAD_B = polyB.GetVertex(HALF_EDGE_B.headVertexIndex); + const SHVec3 TAIL_B = polyB.GetVertex(HALF_EDGE_B.tailVertexIndex); - const SHVec3 D1 = TAIL_A - HEAD_A; - const SHVec3 D2 = TAIL_B - HEAD_B; - const SHVec3 R = HEAD_A - HEAD_B; + const SHVec3 D1 = HEAD_A - TAIL_A; + const SHVec3 D2 = HEAD_B - TAIL_B; + const SHVec3 R = TAIL_A - TAIL_B; - const float D1_LENSQ = D1.LengthSquared(); // a - const float D2_LENSQ = D2.LengthSquared(); // e + const float A = D1.LengthSquared(); // a + const float E = D2.LengthSquared(); // e - const float D1_DOT_D2 = SHVec3::Dot(D1, D2); // b - const float D1_DOT_R = SHVec3::Dot(D1, R); // c - const float D2_DOT_R = SHVec3::Dot(D2, R); // f + const float B = SHVec3::Dot(D1, D2); // b + const float C = SHVec3::Dot(D1, R); // c + const float F = SHVec3::Dot(D2, R); // f float s = 0.0f; float t = 0.0f; + SHCollisionUtils::EdgeContacts result + { + .closestPointA = TAIL_A, + .closestPointB = TAIL_B + }; + // If both segment degenerates into points - if (D1_LENSQ <= SHMath::EPSILON && D2_LENSQ <= SHMath::EPSILON) - return TAIL_A; + if (A <= SHMath::EPSILON && E <= SHMath::EPSILON) + return result; + // Segment A degenerates into a point - if (D1_LENSQ <= SHMath::EPSILON) - t = std::clamp(D2_DOT_R / D2_LENSQ, 0.0f, 1.0f); + if (A <= SHMath::EPSILON) + t = std::clamp(F / E, 0.0f, 1.0f); + else + { + // Segment B degenerates into a point + if (E <= SHMath::EPSILON) + s = std::clamp(-C / A, 0.0f, 1.0f); + else + { + const float DENOMINATOR = A * E - B * B; + if (!SHMath::CompareFloat(DENOMINATOR, 0.0f)) + s = std::clamp((B * F - C * E) / DENOMINATOR, 0.0f, 1.0f); + else + s = 0.0f; - // Segment B degenerates into a point - if (D2_LENSQ <= SHMath::EPSILON) - s = std::clamp(-D1_DOT_R / D1_LENSQ, 0.0f, 1.0f); + // Clamp closest point to segment B + t = (B * s + F) / E; + if (t < 0.0f) + { + t = 0.0f; + s = std::clamp(-C / A, 0.0f, 1.0f); + } + if (t > 1.0) + { + t = 1.0f; + s = std::clamp((B - C) / A, 0.0f, 1.0f); + } + } + } - const float DENOMINATOR = D1_LENSQ * D2_LENSQ - D1_DOT_D2 * D1_DOT_D2; - if (!SHMath::CompareFloat(DENOMINATOR, 0.0f)) - s = std::clamp((D1_DOT_D2 * D2_DOT_R - D1_DOT_R * D2_LENSQ) / DENOMINATOR, 0.0f, 1.0f); - - t = (D1_DOT_D2 * s + D2_DOT_R) / D2_LENSQ; - - // We just need s since we are taking the point in A - if (t < 0.0f) - s = std::clamp(-D1_DOT_R / D1_LENSQ, 0.0f, 1.0f); - else if (t > 1.0f) - s = std::clamp((D1_DOT_D2 - D1_DOT_R) / D1_LENSQ, 0.0f, 1.0f); - - // Get the point on A - return HEAD_A + s * SHVec3::Normalise(D1); + result.closestPointA = TAIL_A + D1 * s; + result.closestPointB = TAIL_B + D2 * t; + return result; } - int32_t SHCollision::findIncidentFace(const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept + int32_t findIncidentFace(const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept { // Get the most anti-parallel face to the normal int32_t bestFace = 0; @@ -383,150 +423,152 @@ namespace SHADE return bestFace; } - bool SHCollision::findFaceContacts(SHManifold& manifold, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept + bool findFaceContacts(SHManifold& manifold, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept { - const SHHalfEdgeStructure::Face& INCIDENT_FACE = incPoly.GetFace(incFace); - const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace); + //const SHHalfEdgeStructure::Face& INCIDENT_FACE = incPoly.GetFace(incFace); + //const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace); - const int32_t NUM_INCIDENT_VERTICES = static_cast(INCIDENT_FACE.vertexIndices.size()); - const int32_t NUM_REFERENCE_VERTICES = static_cast(REFERENCE_FACE.vertexIndices.size()); + //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{}); + //// Build incoming vertices to clip + //SHCollisionUtils::ClipVertices clipIn; + //clipIn.resize(NUM_INCIDENT_VERTICES * 2, SHCollisionUtils::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; + //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; + // 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) + // // 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 = incPoly.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; + // SHCollisionUtils::ClipVertex v; + // v.position = incPoly.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; - } + // clipIn[numClipIn++] = v; + //} - // Clip the vertices against the reference face side planes. - // Number of side planes == number of edges == number of vertices - const SHVec3 REF_NORMAL = refPoly.GetNormal(refFace); - 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) + //// Clip the vertices against the reference face side planes. + //// Number of side planes == number of edges == number of vertices + //const SHVec3 REF_NORMAL = refPoly.GetNormal(refFace); + //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 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 = refPoly.GetVertex(V1_INDEX); - const SHVec3 V2 = refPoly.GetVertex(V2_INDEX); + // const SHVec3 V1 = refPoly.GetVertex(V1_INDEX); + // const SHVec3 V2 = refPoly.GetVertex(V2_INDEX); - const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1); - const SHPlane CLIP_PLANE { V1, SHVec3::Cross(REF_NORMAL, TANGENT) }; + // const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1); + // const SHPlane CLIP_PLANE { V1, SHVec3::Cross(REF_NORMAL, TANGENT) }; - std::vector clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex); - if (clipOut.empty()) - return false; + // SHCollisionUtils::ClipVertices 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; - } + // // 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. - const SHPlane REFERENCE_PLANE{ refPoly.GetVertex(REFERENCE_FACE.vertexIndices.front().index), REF_NORMAL }; + //// From the final set of clipped points, only keep the points that are below the reference plane. + //const SHPlane REFERENCE_PLANE{ refPoly.GetVertex(REFERENCE_FACE.vertexIndices.front().index), REF_NORMAL }; - uint32_t numContacts = 0; + //uint32_t numContacts = 0; - 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; + //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 (flip) - { - 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; - } + // if (flip) + // { + // 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; - } - } + // contacts.emplace_back(contact); + // ++numContacts; + // } + //} - // Reduce contact manifold if more than 4 points - if (numContacts > 4) - { - const auto INDICES_TO_KEEP = reduceContacts(contacts, REF_NORMAL); - std::vector reducedContacts; + //// Reduce contact manifold if more than 4 points + //if (numContacts > 4) + //{ + // const auto INDICES_TO_KEEP = reduceContacts(contacts, REF_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]]); + // 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); - } + // 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; - } - } + //// 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]; + //// 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 = REF_NORMAL; - if (flip) - manifold.normal = -manifold.normal; + //manifold.numContacts = numContacts; + //manifold.normal = REF_NORMAL; + //if (flip) + // manifold.normal = -manifold.normal; - return true; + //return true; + + return false; } - std::vector SHCollision::clipPolygonWithPlane(const std::vector& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept + SHCollisionUtils::ClipVertices clipPolygonWithPlane(const SHCollisionUtils::ClipVertices& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept { - std::vector out; + SHCollisionUtils::ClipVertices out; int32_t v1Index = numIn - 1; int32_t v2Index = 0; @@ -547,7 +589,7 @@ namespace SHADE // keep the intersection point if (v1Distance >= 0.0f && v2Distance < 0.0f) { - ClipVertex intersection; + SHCollisionUtils::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); @@ -566,7 +608,7 @@ namespace SHADE // keep intersection point & v2 if(v1Distance < 0.0f && v2Distance >= 0.0f) { - ClipVertex intersection; + SHCollisionUtils::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); @@ -596,214 +638,216 @@ namespace SHADE return out; } - std::vector SHCollision::reduceContacts(const std::vector& in, const SHVec3& faceNormal) noexcept - { - std::vector indicesToKeep; + //std::vector 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]); + // // 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(); + // // 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; - } - } + // 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); + // indicesToKeep.emplace_back(indexToKeep); + // contactMap.erase(indexToKeep); - indexToKeep = -1; - bestDistance = std::numeric_limits::lowest(); + // 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; - } - } + // // 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); + // indicesToKeep.emplace_back(indexToKeep); + // contactMap.erase(indexToKeep); - indexToKeep = -1; + // indexToKeep = -1; - // We compute the triangle with the largest area. - // The area can be positive or negative depending on the winding order. + // // 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(); + // float maxArea = std::numeric_limits::lowest(); + // float minArea = std::numeric_limits::max(); - int32_t maxAreaIndex = -1; - int32_t minAreaIndex = -1; + // 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 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; + // const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f; - if (AREA > maxArea) - { - maxArea = AREA; - maxAreaIndex = index; - } + // if (AREA > maxArea) + // { + // maxArea = AREA; + // maxAreaIndex = index; + // } - if (AREA < minArea) - { - minArea = AREA; - minAreaIndex = 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; - } + // // 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); + // indicesToKeep.emplace_back(indexToKeep); + // contactMap.erase(indexToKeep); - indexToKeep = -1; + // 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; + // // 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 }; + // 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 (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; + // 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 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; + // 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; + // } - if (!isAreaPositive && AREA > bestArea) - { - bestArea = AREA; - indexToKeep = index; - } - } - } + // if (!isAreaPositive && AREA > bestArea) + // { + // bestArea = AREA; + // indexToKeep = index; + // } + // } + // } - indicesToKeep.emplace_back(indexToKeep); - return indicesToKeep; - } + // indicesToKeep.emplace_back(indexToKeep); + // 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!") + //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; + // // 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; - } + // // 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 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 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); + // 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; + // if (DISTANCE > 0.0f) + // return false; - // Find the incident face - const int32_t INCIDENT_FACE_INDEX = findIncidentFace(*incPoly, REF_NORMAL); + // // 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); - } + // 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; + //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 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 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); + // 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; + // 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; + // // 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; + // const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); - manifold.contacts[numContacts++] = contact; - manifold.numContacts = numContacts; + // SHContact contact; + // // Take feature pair key from previous manifold + // contact.featurePair.key = manifold.contacts[0].featurePair.key; + // contact.position = SHVec3::ClampedLerp(CONTACTS.closestPointA, CONTACTS.closestPointB, 0.5f); + // contact.penetration = -DISTANCE; - return true; - } + // manifold.contacts[numContacts++] = contact; + // manifold.numContacts = numContacts; + + // return true; + //} // Should never reach this. return false; diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index 07466ab1..928a8184 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -15,14 +15,27 @@ #include "SHCollision.h" // Project Headers +#include "SHCollisionUtils.h" #include "Math/Geometry/SHPlane.h" #include "Math/SHMathHelpers.h" #include "Physics/Collision/Shapes/SHSphere.h" #include "Physics/Collision/Shapes/SHConvexPolyhedron.h" #include "Physics/SHPhysicsConstants.h" + + + namespace SHADE { + /*-----------------------------------------------------------------------------------*/ + /* Local Functions Declarations */ + /*-----------------------------------------------------------------------------------*/ + + SHCollisionUtils::FaceQuery findClosestFace (const SHSphere&, const SHConvexPolyhedron&) noexcept; + int32_t findClosestPoint (const SHSphere&, const SHConvexPolyhedron&, int32_t faceIndex) noexcept; + int32_t findVoronoiRegion (const SHSphere&, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& t1, const SHVec3& t2) noexcept; + + /*-----------------------------------------------------------------------------------*/ /* Public Member Functions Definitions */ /*-----------------------------------------------------------------------------------*/ @@ -36,7 +49,7 @@ namespace SHADE const float RADIUS = SPHERE.GetWorldRadius(); // Find closest face - const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON); + const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON); if (!FACE_QUERY.colliding) return false; @@ -77,104 +90,106 @@ namespace SHADE { // Convert to underlying types // For the convex, we only need the convex polyhedron shape since the get vertex is pure virtual. - const SHSphere& SPHERE = dynamic_cast(A); - const SHConvexPolyhedron& POLYHEDRON = dynamic_cast(B); + //const SHSphere& SPHERE = dynamic_cast(A); + //const SHConvexPolyhedron& POLYHEDRON = dynamic_cast(B); - const SHVec3 CENTER = SPHERE.Center; - const float RADIUS = SPHERE.GetWorldRadius(); + //const SHVec3 CENTER = SPHERE.Center; + //const float RADIUS = SPHERE.GetWorldRadius(); - const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON); - if (!FACE_QUERY.colliding) - return false; + //const SHCollisionUtils::FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON); + //if (!FACE_QUERY.colliding) + // return false; - uint32_t numContacts = 0; - const float PENETRATION = RADIUS - FACE_QUERY.bestDistance; + //uint32_t numContacts = 0; + //const float PENETRATION = RADIUS - FACE_QUERY.bestDistance; - SHContact contact; - contact.featurePair.key = 0; + //SHContact contact; + //contact.featurePair.key = 0; - // Check if center is inside polyhedron (below the face) - if (FACE_QUERY.bestDistance < SHMath::EPSILON) - { - manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace); + //// Check if center is inside polyhedron (below the face) + //if (FACE_QUERY.bestDistance < SHMath::EPSILON) + //{ + // manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace); - contact.penetration = PENETRATION; - contact.position = CENTER; + // contact.penetration = PENETRATION; + // contact.position = CENTER; - manifold.contacts[numContacts++] = contact; - manifold.numContacts = numContacts; + // manifold.contacts[numContacts++] = contact; + // manifold.numContacts = numContacts; - return true; - } + // return true; + //} - // Find closest face of polygon to circle - const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace); + //// Find closest face of polygon to circle + //const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace); - const auto& FACE = POLYHEDRON.GetFace(FACE_QUERY.closestFace); - const SHVec3& FACE_NORMAL = POLYHEDRON.GetNormal(FACE_QUERY.closestFace); - const int32_t NUM_VERTICES = static_cast(FACE.vertexIndices.size()); + //const auto& FACE = POLYHEDRON.GetFace(FACE_QUERY.closestFace); + //const SHVec3& FACE_NORMAL = POLYHEDRON.GetNormal(FACE_QUERY.closestFace); + //const int32_t NUM_VERTICES = static_cast(FACE.vertexIndices.size()); - // Get points and build tangents - const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES; - const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1; + //// Get points and build tangents + //const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES; + //const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1; - const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index); - const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index); - const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index); + //const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index); + //const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index); + //const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index); - const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1); - const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1); + //const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1); + //const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1); - // Get the voronoi region it belongs in - const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2); - if (REGION == 0) - return false; + //// Get the voronoi region it belongs in + //const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2); + //if (REGION == 0) + // return false; - // Create contact information based on region + //// Create contact information based on region - const SHVec3 P1_TO_CENTER = CENTER - P1; - switch (REGION) - { - case 1: // Region A - case 2: // Region B - { - // Find closest point - const SHVec3& TANGENT = REGION == 1 ? TANGENT_1 : TANGENT_2; - const SHVec3 CP = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT); - const SHVec3 CP_TO_CENTER = CENTER - CP; + //const SHVec3 P1_TO_CENTER = CENTER - P1; + //switch (REGION) + //{ + // case 1: // Region A + // case 2: // Region B + // { + // // Find closest point + // const SHVec3& TANGENT = REGION == 1 ? TANGENT_1 : TANGENT_2; + // const SHVec3 CP = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT); + // const SHVec3 CP_TO_CENTER = CENTER - CP; - manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); + // manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); - contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); - contact.position = CP; + // contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); + // contact.position = CP; - break; - } - case 3: // Region C - { - manifold.normal = -SHVec3::Normalise(P1_TO_CENTER); + // break; + // } + // case 3: // Region C + // { + // manifold.normal = -SHVec3::Normalise(P1_TO_CENTER); - contact.penetration = RADIUS - P1_TO_CENTER.Length(); - contact.position = P1; + // contact.penetration = RADIUS - P1_TO_CENTER.Length(); + // contact.position = P1; - break; - } - case 4: // Region D - { - manifold.normal = -FACE_NORMAL; + // break; + // } + // case 4: // Region D + // { + // manifold.normal = -FACE_NORMAL; - contact.penetration = PENETRATION; - contact.position = CENTER - FACE_NORMAL * RADIUS; + // contact.penetration = PENETRATION; + // contact.position = CENTER - FACE_NORMAL * RADIUS; - break; - } - default: return false; // Should never happen - } + // break; + // } + // default: return false; // Should never happen + //} - manifold.contacts[numContacts++] = contact; - manifold.numContacts = numContacts; + //manifold.contacts[numContacts++] = contact; + //manifold.numContacts = numContacts; - return true; + //return true; + + return false; } bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept @@ -192,15 +207,15 @@ namespace SHADE /* Private Member Functions Definitions */ /*-----------------------------------------------------------------------------------*/ - SHCollision::FaceQuery SHCollision::findClosestFace + SHCollisionUtils::FaceQuery findClosestFace ( const SHSphere& sphere , const SHConvexPolyhedron& polyhedron ) noexcept { - FaceQuery faceQuery; + SHCollisionUtils::FaceQuery faceQuery; - const SHVec3 CENTER = sphere.Center; + const SHVec3 CENTER = sphere.GetWorldCentroid(); const float RADIUS = sphere.GetWorldRadius(); /* @@ -238,7 +253,7 @@ namespace SHADE return faceQuery; } - int32_t SHCollision::findClosestPoint + int32_t findClosestPoint ( const SHSphere& sphere , const SHConvexPolyhedron& polyhedron @@ -248,7 +263,7 @@ namespace SHADE // Find closest point on face int32_t closestPointIndex = -1; - const SHVec3 CENTER = sphere.Center; + const SHVec3 CENTER = sphere.GetWorldCentroid(); const SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(faceIndex); const int32_t NUM_VERITICES = static_cast(FACE.vertexIndices.size()); @@ -269,7 +284,7 @@ namespace SHADE return closestPointIndex; } - int32_t SHCollision::findVoronoiRegion + int32_t findVoronoiRegion ( const SHSphere& sphere , const SHVec3& faceVertex @@ -294,8 +309,8 @@ namespace SHADE * / / regionC * */ - - const SHVec3 CENTER = sphere.Center; + + const SHVec3 CENTER = sphere.GetWorldCentroid(); const float RADIUS = sphere.GetWorldRadius(); const SHVec3 TANGENTS [NUM_TANGENTS] { tangent1, tangent2 }; diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp index 1a76924e..37bd9d32 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp @@ -14,6 +14,7 @@ #include "SHCollision.h" // Project Headers +#include "SHCollisionUtils.h" #include "Math/SHMathHelpers.h" #include "Physics/Collision/Shapes/SHSphere.h" @@ -23,7 +24,7 @@ namespace SHADE /* Public Member Functions Definitions */ /*-----------------------------------------------------------------------------------*/ - bool SHCollision::SphereVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept + bool SHCollision::SphereVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept { const SHSphere& SPHERE_A = dynamic_cast(A); const SHSphere& SPHERE_B = dynamic_cast(B); @@ -65,26 +66,35 @@ namespace SHADE if (DISTANCE_BETWEEN_CENTERS_SQUARED > COMBINED_RADIUS_SQUARED) return false; + SHCollisionUtils::ShapeTransform inverseTransformA = { SPHERE_A.GetWorldCentroid(), SPHERE_A.GetWorldOrientation() }; + SHCollisionUtils::ShapeTransform inverseTransformB = { SPHERE_B.GetWorldCentroid(), SPHERE_B.GetWorldOrientation() }; + + inverseTransformA.Invert(); + inverseTransformB.Invert(); + // Only populate the manifold if there is a collision uint32_t numContacts = 0; SHContact contact; contact.featurePair.key = 0; + contact.penetration = COMBINED_RADIUS - A_TO_B.Length(); + // Degenerate case if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f)) { manifold.normal = SHVec3::UnitY; - contact.position = CENTER_A; - contact.penetration = RADIUS_B; + contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, inverseTransformA.orientation); + contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, inverseTransformB.orientation); manifold.contacts[numContacts++] = contact; } else { manifold.normal = SHVec3::Normalise(A_TO_B); - contact.position = CENTER_B - manifold.normal * RADIUS_B; - contact.penetration = COMBINED_RADIUS - A_TO_B.Length(); + contact.localPointA = RADIUS_A * SHVec3::Normalise(inverseTransformA * CENTER_B); + contact.localPointB = RADIUS_B * SHVec3::Normalise(inverseTransformB * CENTER_A); + manifold.contacts[numContacts++] = contact; } diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index 7434953e..dc5b40f9 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp @@ -17,6 +17,7 @@ // Project Headers #include "Math/SHMathHelpers.h" #include "Physics/Collision/Narrowphase/SHCollisionDispatch.h" +#include "Physics/Collision/Narrowphase/SHCollisionUtils.h" namespace SHADE { @@ -51,8 +52,12 @@ namespace SHADE , .normal = manifold.normal }; + const auto* SHAPE_A = manifold.shapeA; + const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() }; + for (uint32_t i = 0; i < manifold.numContacts; ++i) - collisionEvent.contactPoints[i] = manifold.contacts[i].position; + collisionEvent.contactPoints[i] = TF_A * manifold.contacts[i].localPointA; + collisionEvents.emplace_back(collisionEvent); } @@ -71,11 +76,16 @@ namespace SHADE if (manifold.state == SHCollisionState::EXIT) continue; + const auto* SHAPE_A = manifold.shapeA; + const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() }; + for (uint32_t i = 0; i < manifold.numContacts; ++i) { + // Contact position will be the world position of localPointA + const ContactInfo INFO { - .position = manifold.contacts[i].position + .position = TF_A * manifold.contacts[i].localPointA , .normal = manifold.normal }; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index f16d7f1e..ff4212b6 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -16,6 +16,7 @@ // Project Headers #include "Math/SHMathHelpers.h" #include "Physics/SHPhysicsConstants.h" +#include "Physics/Collision/Narrowphase/SHCollisionUtils.h" namespace SHADE { @@ -108,11 +109,14 @@ namespace SHADE memcpy_s(newConstraint.tangents, TANGENTS_SIZE, manifold.tangents, TANGENTS_SIZE); memcpy_s(newConstraint.contacts, CONTACTS_SIZE, manifold.contacts, CONTACTS_SIZE); + const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() }; + const SHCollisionUtils::ShapeTransform TF_B = { SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() }; + // Compute rA & rB for contacts for (uint32_t i = 0; i < newConstraint.numContacts; ++i) { - newConstraint.contacts[i].rA = newConstraint.contacts[i].position - newConstraint.centerOfMassA; - newConstraint.contacts[i].rB = newConstraint.contacts[i].position - newConstraint.centerOfMassB; + newConstraint.contacts[i].rA = (TF_A * newConstraint.contacts[i].localPointA) - newConstraint.centerOfMassA; + newConstraint.contacts[i].rB = (TF_B * newConstraint.contacts[i].localPointB) - newConstraint.centerOfMassB; } }