xiaoqi
/
SHADE_Y3
1
0
Fork 0
Code Issues 12 Pull Requests Packages Projects Releases 9 Wiki Activity

Implemented a custom physics engine #316

Merged
direnbharwani merged 95 commits from SHPhysics into main 2023-01-23 15:55:45 +08:00
Conversation 0 Commits 95 Files Changed 144 +55 -32
3 changed files with 55 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 016f6c804d - Show all commits

2
Assets/Scenes/PhysicsSandbox.shade
View File

@ -280,7 +280,7 @@
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0.524352431, y: 11.1989822, z: 0.0463808179}
Translate: {x: 0.524352431, y: 11.5, z: 0.0463808179}
Rotate: {x: -0, y: 0.785398066, z: 0.785398185}
Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881}
IsActive: true

2
SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h
View File

@ -107,7 +107,7 @@ namespace SHADE
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, SHVec3& normal) noexcept;
static SHVec3 findClosestPointBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
/*
* TODO:

83
SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp
View File

@ -83,6 +83,8 @@ namespace SHADE
const FaceQuery& BEST_FACE_QUERY = FACE_QUERY_A.bestDistance > FACE_QUERY_B.bestDistance ? FACE_QUERY_A : FACE_QUERY_B;
uint32_t numContacts = 0;
// If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on
// each edge and use that as the contact point.
// Artificially increase the depth of this collision by a small tolerance to tend towards picking a face query in the next frame.
@ -91,22 +93,31 @@ namespace SHADE
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 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
manifold.normal = SHVec3::Cross(VB, VA);
// Flip normal if need to ( A -> B)
if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
manifold.normal = -manifold.normal;
// In this scenario, we only have one contact
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.featurePair.typeA = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
contact.featurePair.indexA = HALF_EDGE_A.tailVertexIndex;
contact.featurePair.typeB = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
contact.featurePair.indexB = HALF_EDGE_B.tailVertexIndex;
contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB, manifold.normal);
contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB);
contact.penetration = EDGE_QUERY.bestDistance;
manifold.contacts[0] = contact;
manifold.numContacts = 1;
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
return true;
}
@ -237,8 +248,22 @@ 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, SHVec3& normal) noexcept
SHVec3 SHCollision::findClosestPointBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept
{
/*
* The two edges can be parameterised in the form p + tv
*
* LA(r) = A + rVA
* LB(s) = B + sVB
*
* The vector between the closest points is the cross product of VA and VB.
* Since the cross product is orthogonal to VA and VB, we can generalise this as:
* (LB(s) - LA(r)) /dot VA = 0
* (LB(s) - LA(r)) /dot VB = 0
*
* Where LB(s) - LA(r) is the same vector as VB X VA.
*/
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA);
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB);
@ -250,37 +275,35 @@ namespace SHADE
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)
* C' = TAIL_B - TAIL_A
*
* a = VB /dot U
* b = -VA /dot U
* c = C' /dot U
*
* U is either VA or VB
*/
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;
const float VB_DOT_VA = SHVec3::Dot(VB, VA);
const float VB_DOT_VB = SHVec3::Dot(VB, VB);
const float AV_DOT_VA = SHVec3::Dot(-VA, VA);
const float AV_DOT_VB = SHVec3::Dot(-VA, VB);
const float C_DOT_VA = SHVec3::Dot(C, VA);
const float C_DOT_VB = SHVec3::Dot(C, VB);
/*
* R = -c2 / ( b2 - (a2 * (c1 + b1)) / a1 )
* S = (b1 / a1) * (c2 / ( b2 - (a2 * b1) / a1 )) - (c1 / a1)
*
* We only need to solve for R
*/
const float R = -C2 / (B2 - (A2 * (C1 + B1)) / A1);
const float R = -C_DOT_VB / (AV_DOT_VB - (VB_DOT_VB * (C_DOT_VA + AV_DOT_VA)) / VB_DOT_VA);
// Just take a point from A since it's A -> B
return TAIL_A + R * VA;