From 6ad40d0927dcddc0dbac263ab588bf36e698b9ac Mon Sep 17 00:00:00 2001
From: Patrick Kelley <49526549+MortimerSnerd@users.noreply.github.com>
Date: Fri, 15 Sep 2023 01:09:02 -0400
Subject: [PATCH] Fix for NaNs in normal for scene capsule collision. (#745)

For wi::scene::Scene::Intersects(Capsule), if the overlap depth
is >= the capsule radius, and the line segment that makes up the
spine of the capsule intersects a triangle, then the reported
contact normal would be (NaN, NaN, Nan).  In this special case, the closest
point on the capsule line segment to the triangle ("Center" in the code)
would always be the same as the "bestPoint", which leads up to a divide
by zero in the normal vector calculation.

This fix detects this case, and:
  1) reports the triangle normal when this special case prevents
     calculation of the normal.
  2) corrects the depth calculation for this special case to take into
     the distance from the "Center" point on the spine to the actual
     penetrating endpoint of the spine, projected onto the triangle
     normal.
---
 WickedEngine/wiScene.cpp | 49 ++++++++++++++++++++++++++++++++++++----
 1 file changed, 45 insertions(+), 4 deletions(-)

diff --git a/WickedEngine/wiScene.cpp b/WickedEngine/wiScene.cpp
index 040e1f1c2..aa6af6224 100644
--- a/WickedEngine/wiScene.cpp
+++ b/WickedEngine/wiScene.cpp
@@ -5107,7 +5107,8 @@ namespace wi::scene
 					// Find the nearest feature on the triangle to the sphere.
 					XMVECTOR Dist = XMVector3Dot(XMVectorSubtract(Center, p0), N);
 
-					if (!mesh->IsDoubleSided() && XMVectorGetX(Dist) > 0)
+					bool onBackside = XMVectorGetX(Dist) > 0;
+					if (!mesh->IsDoubleSided() && onBackside)
 						return; // pass through back faces
 
 					// If the center of the sphere is farther from the plane of the triangle than
@@ -5191,13 +5192,53 @@ namespace wi::scene
 						XMVECTOR intersectionVec = Center - bestPoint;
 						XMVECTOR intersectionVecLen = XMVector3Length(intersectionVec);
 
-						float depth = capsule.radius - XMVectorGetX(intersectionVecLen);
+						float lenX = XMVectorGetX(intersectionVecLen);
+						float depth = capsule.radius - lenX;
 						if (depth > result.depth)
 						{
 							result.entity = entity;
-							result.depth = depth;
 							XMStoreFloat3(&result.position, bestPoint);
-							XMStoreFloat3(&result.normal, intersectionVec / intersectionVecLen);
+							if (lenX > std::numeric_limits<float>::epsilon())
+							{
+								result.depth = depth;
+								XMStoreFloat3(&result.normal, intersectionVec / intersectionVecLen);
+							} 
+							else 
+							{
+								// The line segment that makes the spine of the capsule has 
+								// intersected the triangle plane, so interSectionVec ~= Zero, 
+								// and depth ~= capsule.radius.  Use the triangle normal.
+								XMVECTOR CandNorm; 
+								if (onBackside)
+								{
+									CandNorm = N;
+								} else 
+								{
+									CandNorm = XMVectorNegate(N);
+								}
+								XMStoreFloat3(&result.normal, CandNorm);
+
+								// If the capsule has penetrated enough to intersect the spine, the
+								// depth is calculated from closest point on the spine, not from the
+								// actual endpoint, so the real depth may be greater, depending on the
+								// orientation of the capsule relative to the triangle normal.
+								// For simplicity, we assume the penetrating endpoint is the one closest
+								// to Center, and we project the distance from Center to the closest endpoint
+								// onto the normal.
+								XMVECTOR A_C = XMVector3LengthSq(Center - A);
+								XMVECTOR B_C = XMVector3LengthSq(Center - B);
+								XMVECTOR CDiff;
+								if (XMVector3Less(A_C, B_C)) 
+								{
+									CDiff = XMVectorSubtract(A, Center);
+								}
+								else 
+								{
+									CDiff = XMVectorSubtract(B, Center);
+								}
+								XMVECTOR CDiffOnN = XMVectorMultiply(XMVector3Dot(CDiff, N), CDiff);
+								result.depth = depth + XMVectorGetX(XMVector3Length(CDiffOnN));
+							}
 
 							XMVECTOR vel = bestPoint - XMVector3Transform(XMVector3Transform(bestPoint, objectMat_Inverse), objectMatPrev);
 							XMStoreFloat3(&result.velocity, vel);