Added edge vs edge contacts for convex polyhedron collisions

This commit is contained in:
Diren D Bharwani 2023-01-05 13:42:17 +08:00
parent c484a088fd
commit 68e11ba48e
3 changed files with 119 additions and 18 deletions

View File

@ -45,9 +45,9 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Camera Component: Camera Component:
Position: {x: 1, y: 10, z: 3} Position: {x: -0.5, y: 10, z: -3}
Pitch: 0 Pitch: 0
Yaw: 0 Yaw: 180
Roll: 0 Roll: 0
Width: 1920 Width: 1920
Height: 1080 Height: 1080
@ -84,7 +84,7 @@
Freeze Rotation Z: false Freeze Rotation Z: false
IsActive: true IsActive: true
Collider Component: Collider Component:
DrawColliders: false DrawColliders: true
Colliders: Colliders:
- Is Trigger: false - Is Trigger: false
Collision Tag: 1 Collision Tag: 1
@ -95,7 +95,7 @@
Density: 1 Density: 1
Position Offset: {x: 0, y: 0, z: 0} Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0} Rotation Offset: {x: 0, y: 0, z: 0}
- Is Trigger: false - Is Trigger: true
Collision Tag: 1 Collision Tag: 1
Type: Sphere Type: Sphere
Radius: 0.5 Radius: 0.5
@ -280,9 +280,9 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Transform Component: Transform Component:
Translate: {x: 0.899999976, y: 10, z: 0} Translate: {x: 0.524352431, y: 11.1989822, z: 0.0463808179}
Rotate: {x: -0, y: 0, z: 0.785398185} Rotate: {x: -0, y: 0.785398066, z: 0.785398185}
Scale: {x: 1, y: 1, z: 1} Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881}
IsActive: true IsActive: true
RigidBody Component: RigidBody Component:
Type: Dynamic Type: Dynamic
@ -295,7 +295,7 @@
Interpolate: true Interpolate: true
Sleeping Enabled: true Sleeping Enabled: true
Freeze Position X: false Freeze Position X: false
Freeze Position Y: true Freeze Position Y: false
Freeze Position Z: false Freeze Position Z: false
Freeze Rotation X: false Freeze Rotation X: false
Freeze Rotation Y: false Freeze Rotation Y: false
@ -304,7 +304,7 @@
Collider Component: Collider Component:
DrawColliders: false DrawColliders: false
Colliders: Colliders:
- Is Trigger: true - Is Trigger: false
Collision Tag: 1 Collision Tag: 1
Type: Box Type: Box
Half Extents: {x: 1, y: 1, z: 1} Half Extents: {x: 1, y: 1, z: 1}
@ -322,13 +322,13 @@
Components: Components:
Transform Component: Transform Component:
Translate: {x: -0.5, y: 10, z: 0} Translate: {x: -0.5, y: 10, z: 0}
Rotate: {x: -0, y: 0.785398185, 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
RigidBody Component: RigidBody Component:
Type: Dynamic Type: Static
Auto Mass: false Auto Mass: false
Mass: 1 Mass: .inf
Drag: 0.00999999978 Drag: 0.00999999978
Angular Drag: 0.00999999978 Angular Drag: 0.00999999978
Use Gravity: true Use Gravity: true
@ -345,7 +345,7 @@
Collider Component: Collider Component:
DrawColliders: false DrawColliders: false
Colliders: Colliders:
- Is Trigger: true - Is Trigger: false
Collision Tag: 1 Collision Tag: 1
Type: Box Type: Box
Half Extents: {x: 1, y: 1, z: 1} Half Extents: {x: 1, y: 1, z: 1}

View File

@ -101,12 +101,13 @@ namespace SHADE
// Convex VS Convex // Convex VS Convex
static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
static EdgeQuery queryEdgeDirections (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 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; 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, SHVec3& normal) noexcept;
/* /*
* TODO: * TODO:

View File

@ -17,6 +17,7 @@
#include "Math/SHMathHelpers.h" #include "Math/SHMathHelpers.h"
#include "Math/Geometry/SHPlane.h" #include "Math/Geometry/SHPlane.h"
#include "Physics/Collision/Shapes/SHConvexPolyhedron.h" #include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
#include "Tools/Utilities/SHUtilities.h"
namespace SHADE namespace SHADE
{ {
@ -65,6 +66,55 @@ namespace SHADE
* *
*/ */
const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
const 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);
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;
const FaceQuery& BEST_FACE_QUERY = FACE_QUERY_A.bestDistance > FACE_QUERY_B.bestDistance ? FACE_QUERY_A : FACE_QUERY_B;
// If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on
// each edge and use that as the contact point.
// Artificially increase the depth of this collision by a small tolerance to tend towards picking a face query in the next frame.
if (EDGE_QUERY.bestDistance > BEST_FACE_QUERY.bestDistance)
{
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA);
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB);
// In this scenario, we only have one contact
SHContactFeatures featurePair;
featurePair.typeA = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
featurePair.indexA = HALF_EDGE_A.tailVertexIndex;
featurePair.typeB = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
featurePair.indexB = HALF_EDGE_B.tailVertexIndex;
SHContact contact;
contact.featurePair = featurePair;
contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB, manifold.normal);
contact.penetration = EDGE_QUERY.bestDistance;
manifold.contacts[0] = contact;
manifold.numContacts = 1;
return true;
}
// Use a bias to favour a normal in the direction of A -> B
return false; return false;
} }
@ -187,4 +237,54 @@ namespace SHADE
return SHVec3::Dot(normal, HEAD_B - HEAD_A); return SHVec3::Dot(normal, HEAD_B - HEAD_A);
} }
SHVec3 SHCollision::findClosestPointBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB, SHVec3& normal) 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 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
normal = SHVec3::Cross(VB, VA);
// Flip normal if need to ( A -> B)
if (SHVec3::Dot(normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
normal = -normal;
/*
* Find a1, a2, b1, b2, c1, c2 to solve the simultaneous equation
* C' = TAIL_B - TAIL_A
* U = VA / VB
*
* a = VBxUx + VByUy + VBzUz
* b = -VAxUx - VAyUy - VAzUz
* c = (Cx'Ux + Cy'Uy + Cz'Uz)
*/
const SHVec3 C = TAIL_B - TAIL_A;
const float A1 = VB.x * VA.x + VB.y * VA.y + VB.z * VA.z;
const float A2 = VB.x * VB.x + VB.y * VB.y + VB.z * VB.z;
const float B1 = -VA.x * VA.x + -VA.y * VA.y + -VA.z * VA.z;
const float B2 = -VA.x * VB.x + -VA.y * VB.y + -VA.z * VB.z;
const float C1 = C.x * VA.x + C.y + VA.y + C.z + VA.z;
const float C2 = C.x * VB.x + C.y + VB.y + C.z + VB.z;
/*
* R = -c2 / ( b2 - (a2 * (c1 + b1)) / a1 )
* S = (b1 / a1) * (c2 / ( b2 - (a2 * b1) / a1 )) - (c1 / a1)
*/
const float R = -C2 / (B2 - (A2 * (C1 + B1)) / A1);
// Just take a point from A since it's A -> B
return TAIL_A + R * VA;
}
} // namespace SHADE } // namespace SHADE