Added first half of Gauss Map Optimised SAT

This commit is contained in:
Diren D Bharwani 2023-01-05 01:12:25 +08:00
parent dffdec9d9c
commit c484a088fd
7 changed files with 291 additions and 29 deletions

View File

@ -45,7 +45,7 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Camera Component: Camera Component:
Position: {x: 0, y: 4, z: 5} Position: {x: 1, y: 10, z: 3}
Pitch: 0 Pitch: 0
Yaw: 0 Yaw: 0
Roll: 0 Roll: 0
@ -62,7 +62,7 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Transform Component: Transform Component:
Translate: {x: -1.45715916, y: 7, z: 0.227711335} Translate: {x: -1.45715916, y: 7.37748241, z: 0.227711335}
Rotate: {x: -0, y: 0, z: -0} Rotate: {x: -0, y: 0, z: -0}
Scale: {x: 1, y: 1, z: 1} Scale: {x: 1, y: 1, z: 1}
IsActive: true IsActive: true
@ -77,7 +77,7 @@
Interpolate: true Interpolate: true
Sleeping Enabled: true Sleeping Enabled: true
Freeze Position X: false Freeze Position X: false
Freeze Position Y: false Freeze Position Y: true
Freeze Position Z: false Freeze Position Z: false
Freeze Rotation X: false Freeze Rotation X: false
Freeze Rotation Y: false Freeze Rotation Y: false
@ -274,3 +274,85 @@
Rotation Offset: {x: 0, y: 0, z: 0} Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true IsActive: true
Scripts: ~ Scripts: ~
- EID: 7
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0.899999976, y: 10, z: 0}
Rotate: {x: -0, y: 0, z: 0.785398185}
Scale: {x: 1, y: 1, z: 1}
IsActive: true
RigidBody Component:
Type: Dynamic
Auto Mass: false
Mass: 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: true
Freeze Position Z: false
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: false
Colliders:
- Is Trigger: true
Collision Tag: 1
Type: Box
Half Extents: {x: 1, y: 1, z: 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: ~
- EID: 8
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: -0.5, y: 10, z: 0}
Rotate: {x: -0, y: 0.785398185, 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: true
Freeze Position Z: false
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: false
Colliders:
- Is Trigger: true
Collision Tag: 1
Type: Box
Half Extents: {x: 1, y: 1, z: 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: ~

View File

@ -13,6 +13,7 @@
// Project Headers // Project Headers
#include "Physics/Collision/Contacts/SHManifold.h" #include "Physics/Collision/Contacts/SHManifold.h"
#include "Physics/Collision/Contacts/SHCollisionKey.h" #include "Physics/Collision/Contacts/SHCollisionKey.h"
#include "Physics/Collision/Shapes/SHHalfEdgeStructure.h"
namespace SHADE namespace SHADE
@ -72,11 +73,18 @@ namespace SHADE
struct FaceQuery struct FaceQuery
{ {
bool colliding = false; bool colliding = false; // Allows for early out
int32_t closestFace = -1; int32_t closestFace = -1;
float bestDistance = std::numeric_limits<float>::lowest(); float bestDistance = std::numeric_limits<float>::lowest();
}; };
struct EdgeQuery
{
int32_t halfEdgeA = -1;
int32_t halfEdgeB = -1;
float bestDistance = std::numeric_limits<float>::lowest();
};
/*---------------------------------------------------------------------------------*/ /*---------------------------------------------------------------------------------*/
/* Member Functions */ /* Member Functions */
/*---------------------------------------------------------------------------------*/ /*---------------------------------------------------------------------------------*/
@ -93,14 +101,19 @@ namespace SHADE
// Convex VS Convex // Convex VS Convex
static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
static bool buildMinkowskiFace (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
static bool isMinkowskiFace (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept; static 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;
/* /*
* TODO: * TODO:
* 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::HalfEdge& edgeA, const SHConvexPolyhedron::HalfEdge& edgeB) noexcept; *
* static float distanceBetweenEdges(const SHConvexPolyhedron::HalfEdge& edgeA, const SHConvexPolyhedron::HalfEdge& edgeB, SHConvexPolyhedron& poly) noexcept; *
* static int32_t findIncidentFace (SHConvexPolyhedron& poly, const SHVec3& normal) noexcept; * static int32_t findIncidentFace (SHConvexPolyhedron& poly, const SHVec3& normal) noexcept;
* static uint32_t clip [sutherland-hodgemann clipping] * static uint32_t clip [sutherland-hodgemann clipping]
* *

View File

@ -15,9 +15,9 @@
// Project Headers // Project Headers
#include "Math/SHMathHelpers.h" #include "Math/SHMathHelpers.h"
#include "Math/Geometry/SHPlane.h"
#include "Physics/Collision/Shapes/SHConvexPolyhedron.h" #include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
namespace SHADE namespace SHADE
{ {
/*-----------------------------------------------------------------------------------*/ /*-----------------------------------------------------------------------------------*/
@ -26,15 +26,22 @@ namespace SHADE
bool SHCollision::ConvexVsConvex(const SHCollisionShape& A, const SHCollisionShape& B) noexcept bool SHCollision::ConvexVsConvex(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
{ {
/* const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
* TODO: const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
*
* 1. Query face directiosn 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.
*/
const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B);
if (FACE_QUERY_A.bestDistance > 0.0f)
return false; return false;
const 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);
if (EDGE_QUERY.bestDistance > 0.0f)
return false;
return true;
} }
bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
@ -61,4 +68,123 @@ namespace SHADE
return false; return false;
} }
/*-----------------------------------------------------------------------------------*/
/* Private Member Functions Definitions */
/*-----------------------------------------------------------------------------------*/
SHCollision::FaceQuery SHCollision::queryFaceDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
{
FaceQuery faceQuery;
const int32_t NUM_FACES = A.GetFaceCount();
for (const int32_t i : std::views::iota(0, NUM_FACES))
{
const SHHalfEdgeStructure::Face& FACE_A = A.GetFace(i);
const SHVec3 NORMAL_A = A.GetNormal(i);
// Smallest penetration is point closest to face normal
const SHVec3 SUPPORT_POINT = B.FindSupportPoint(-NORMAL_A);
const SHVec3 VERTEX_A = A.GetVertex(FACE_A.vertexIndices[0].index);
const SHPlane FACE_PLANE { VERTEX_A, NORMAL_A };
const float DISTANCE = FACE_PLANE.SignedDistance(SUPPORT_POINT);
if (DISTANCE > faceQuery.bestDistance)
{
faceQuery.bestDistance = DISTANCE;
faceQuery.closestFace = i;
}
}
return faceQuery;
}
SHCollision::EdgeQuery SHCollision::queryEdgeDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
{
EdgeQuery edgeQuery;
const int32_t EDGE_COUNT_A = A.GetHalfEdgeCount();
const int32_t EDGE_COUNT_B = B.GetHalfEdgeCount();
for (int32_t i = 0; i < EDGE_COUNT_A; i += 2)
{
for (int32_t j = 0; j < EDGE_COUNT_B; j += 2)
{
const bool IS_MINKOWSKI_FACE = buildMinkowskiFace(A, B, i, j);
if (!IS_MINKOWSKI_FACE)
continue;
const float SEPARATION = distanceBetweenEdges(A, B, i, j);
if (SEPARATION > edgeQuery.bestDistance)
{
edgeQuery.bestDistance = SEPARATION;
edgeQuery.halfEdgeA = i;
edgeQuery.halfEdgeB = j;
}
}
}
return edgeQuery;
}
bool SHCollision::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);
const SHHalfEdgeStructure::HalfEdge& TWIN_EDGE_A = A.GetHalfEdge(HALF_EDGE_A.twinEdgeIndex);
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);
const SHVec3 NC = B.GetNormal(HALF_EDGE_B.faceIndex);
const SHVec3 ND = B.GetNormal(TWIN_EDGE_B.faceIndex);
return isMinkowskiFace(NA, NB, -NC, -ND);
}
bool SHCollision::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);
const float CBA = SHVec3::Dot(c, BXA);
const float DBA = SHVec3::Dot(d, BXA);
const float ADC = SHVec3::Dot(a, DXC);
const float BDC = SHVec3::Dot(b, DXC);
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
{
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA);
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.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 DIR_A = SHVec3::Normalise(HEAD_A - TAIL_A);
const SHVec3 DIR_B = SHVec3::Normalise(HEAD_B - TAIL_B);
// Check if the edges are parallel (abs dot product is 1)
const float DOT_BETWEEN_EDGES = std::fabs(SHVec3::Dot(DIR_A, DIR_B));
if (SHMath::CompareFloat(DOT_BETWEEN_EDGES, 1.0f))
return std::numeric_limits<float>::lowest();
SHVec3 normal = SHVec3::Cross(DIR_A, DIR_B);
// Flip normal if need to ( A -> B)
if (SHVec3::Dot(normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
normal = -normal;
return SHVec3::Dot(normal, HEAD_B - HEAD_A);
}
} // namespace SHADE } // namespace SHADE

View File

@ -291,5 +291,29 @@ namespace SHADE
return SHAABB{ CENTROID, HALF_EXTENTS }; return SHAABB{ CENTROID, HALF_EXTENTS };
} }
SHVec3 SHBox::FindSupportPoint(const SHVec3& direction) const noexcept
{
float bestDistance = std::numeric_limits<float>::lowest();
SHVec3 vertices[NUM_VERTICES];
GetCorners(vertices);
// No reason to put the center really..
SHVec3 bestPoint = Center;
for (auto& vertex : vertices)
{
const float PROJECTION = SHVec3::Dot(vertex, direction);
if (PROJECTION > bestDistance)
{
bestDistance = PROJECTION;
bestPoint = vertex;
}
}
return bestPoint;
}
} // namespace SHADE } // namespace SHADE

View File

@ -114,6 +114,7 @@ namespace SHADE
[[nodiscard]] SHRaycastResult Raycast (const SHRay& ray) const noexcept override; [[nodiscard]] SHRaycastResult Raycast (const SHRay& ray) const noexcept override;
[[nodiscard]] SHMatrix GetTRS () const noexcept override; [[nodiscard]] SHMatrix GetTRS () const noexcept override;
[[nodiscard]] SHAABB ComputeAABB () const noexcept override; [[nodiscard]] SHAABB ComputeAABB () const noexcept override;
[[nodiscard]] SHVec3 FindSupportPoint (const SHVec3& direction) const noexcept override;
private: private:
/*---------------------------------------------------------------------------------*/ /*---------------------------------------------------------------------------------*/

View File

@ -150,11 +150,11 @@ namespace SHADE
const int32_t FACE_VERTICES[NUM_FACES][NUM_VERTICES_PER_FACE] const int32_t FACE_VERTICES[NUM_FACES][NUM_VERTICES_PER_FACE]
{ {
{ 0, 1, 2, 3 } { 0, 1, 2, 3 }
, { 1, 5, 6, 2 } , { 5, 6, 2, 1 }
, { 5, 4, 7, 6 } , { 5, 4, 7, 6 }
, { 4, 0, 3, 7 } , { 0, 3, 7, 4 }
, { 3, 2, 6, 7 } , { 2, 6, 7, 3 }
, { 4, 5, 1, 0 } , { 5, 1, 0, 4 }
}; };
for (int i = 0; i < NUM_FACES; ++i) for (int i = 0; i < NUM_FACES; ++i)

View File

@ -63,9 +63,25 @@ namespace SHADE
[[nodiscard]] int32_t GetHalfEdgeCount () const noexcept; [[nodiscard]] int32_t GetHalfEdgeCount () const noexcept;
[[nodiscard]] const SHHalfEdgeStructure::HalfEdge& GetHalfEdge (int index) const; [[nodiscard]] const SHHalfEdgeStructure::HalfEdge& GetHalfEdge (int index) const;
// Virtual Methods
[[nodiscard]] virtual SHVec3 GetVertex (int index) const = 0; [[nodiscard]] virtual SHVec3 GetVertex (int index) const = 0;
[[nodiscard]] virtual SHVec3 GetNormal (int faceIndex) const = 0; [[nodiscard]] virtual SHVec3 GetNormal (int faceIndex) const = 0;
/*---------------------------------------------------------------------------------*/
/* Member Functions */
/*---------------------------------------------------------------------------------*/
/**
* @brief
* Finds the most extreme point on the polygon in a given direction.
* @param direction
* The direction to find the support point in.
* @return
* The most extreme vertex in the given direction.
*/
[[nodiscard]] virtual SHVec3 FindSupportPoint (const SHVec3& direction) const noexcept = 0;
protected: protected:
/*---------------------------------------------------------------------------------*/ /*---------------------------------------------------------------------------------*/
/* Data Members */ /* Data Members */