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

Fixed edge-edge contact points being beyond the range of the edge

This commit is contained in:
Diren D Bharwani 2023-01-26 01:17:14 +08:00
parent 6b9a64233e
commit 9d8d1ee19d
1 changed files with 36 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -306,18 +306,8 @@ namespace SHADE
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.
*
* 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);
@ -328,46 +318,47 @@ namespace SHADE
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);
/*
* Find a1, a2, b1, b2, c1, c2 to solve the simultaneous equation
*
* C' = TAIL_B - TAIL_A
*
* a = VB /dot U
* b = -VA /dot U
* c = C' /dot U
*
* U is either VA or VB
*
* TODO: Check if segments degenerate into a point
*/
const SHVec3 D1 = TAIL_A - HEAD_A;
const SHVec3 D2 = TAIL_B - HEAD_B;
const SHVec3 R = HEAD_A - HEAD_B;
const SHVec3 C = TAIL_B - TAIL_A;
const float D1_LENSQ = D1.LengthSquared(); // a
const float D2_LENSQ = D2.LengthSquared(); // e
const float VB_DOT_VA = SHVec3::Dot(VB, VA); // a1
const float VB_DOT_VB = SHVec3::Dot(VB, VB); // a2
const float AV_DOT_VA = SHVec3::Dot(-VA, VA); // b1
const float AV_DOT_VB = SHVec3::Dot(-VA, VB); // b2
const float C_DOT_VA = SHVec3::Dot(C, VA); // c1
const float C_DOT_VB = SHVec3::Dot(C, VB); // c2
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
/*
* We only need to solve for R
* R = (c1a2 / a1 - c2) / ( b2 - a2b1/a1 )
*/
float s = 0.0f;
float t = 0.0f;
const float A2_OVER_A1 = VB_DOT_VA == 0.0f ? 0.0f : VB_DOT_VB / VB_DOT_VA;
const float NUMERATOR = C_DOT_VA * A2_OVER_A1 - C_DOT_VB;
const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1;
// If both segment degenerates into points
if (D1_LENSQ <= SHMath::EPSILON && D2_LENSQ <= SHMath::EPSILON)
return TAIL_A;
// We clamp it because the factor cannot be beyond [0,1] as it would be beyond the segment if it was so.
const float R = DENOMINATOR == 0.0f ? NUMERATOR : NUMERATOR / DENOMINATOR;
// Segment A degenerates into a point
if (D1_LENSQ <= SHMath::EPSILON)
t = std::clamp(D2_DOT_R / D2_LENSQ, 0.0f, 1.0f);
// Just take a point from A since it's A -> B
return TAIL_A + R * VA;
// Segment B degenerates into a point
if (D2_LENSQ <= SHMath::EPSILON)
s = std::clamp(-D1_DOT_R / D1_LENSQ, 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);
}
int32_t SHCollision::findIncidentFace(const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept