GJK floating point fixes

gotta love when 0.0000001f is the difference between working and not!

Author: Sploder12

src/include/sndx/collision/gjk.hpp

@@ -287,6 +287,15 @@ namespace sndx::collision {
 			auto ac = points[2].out - points[0].out;
 			auto ao = -points[0].out;
 
+			if (glm::length2(ab) <= 0.00001f || glm::length2(ac) <= 0.00001f) {
+				// keep b and c
+				size = 2;
+				points[0] = points[1];
+				points[1] = points[2];
+				lineOrigin(newDirection);
+				return false;
+			}
+
 			auto cross = glm::cross(ab, ac);
 
 			if (detail::similarDir(glm::cross(cross, ac), ao)) {
@@ -386,10 +395,14 @@ namespace sndx::collision {
 			support = detail::gjkMinkowski(supportA, supportB, dir);
 
 			// it is okay for the 2nd point to have a negative dot product.
-			if (!detail::similarDir(support.out, dir) && iterations > 0) {
+			auto alignment = glm::dot(glm::normalize(support.out), glm::normalize(dir));
+			if (alignment < -0.05f && iterations > 0) {
 				return std::nullopt;
 			}
 
+			assert(!std::isnan(support.out.x));
+			
+
 			simplex.push_front(support);
 			if (simplex.gjkOrigin(dir)) {
 				return simplex;
@@ -473,8 +486,14 @@ namespace sndx::collision {
 			glm::vec3 b = polytope[faces[i + 1]].out;
 			glm::vec3 c = polytope[faces[i + 2]].out;
 
-			glm::vec3 normal = glm::normalize(glm::cross(b - a, c - a));
-			float distance = dot(normal, a);
+			glm::vec3 normal = glm::cross(b - a, c - a);
+			auto l = glm::length(normal);
+
+			float distance = 0.0f;
+			if (l > 0.000001f) {
+				normal /= l;
+				distance = dot(normal, a);
+			}
 
 			if (distance < 0) {
 				normal *= -1;
@@ -528,13 +547,20 @@ namespace sndx::collision {
 			minNormal = glm::vec3(normals[minFace]);
 			minDistance = normals[minFace].w;
 
-			if (i >= 1024) {
+			if (i >= 64) {
 				break;
 			}
 
 			auto support = detail::gjkMinkowski(supportA, supportB, minNormal);
 			float sDistance = glm::dot(minNormal, support.out);
 
+			for (const auto& pnt : polytope) {
+				if (glm::distance2(pnt.out, support.out) < 0.000001f) {
+					sDistance = minDistance;
+					break;
+				}
+			}
+
 			if (abs(sDistance - minDistance) > 0.0001f) {
 				std::vector<std::pair<size_t, size_t>> uniqueEdges;
 

src/tests/collision/gjk.cpp

@@ -118,6 +118,27 @@ TEST(GJK, circleAndTriangleCollide2) {
 	EXPECT_TRUE(result);
 }
 
+TEST(GJK, circleAndTriangleCollide3) {
+	Circle3D circle{ glm::vec3{7.22956371f, 0.65463f, -0.94764f}, 0.5f };
+	Tri3D triangle{ 
+		glm::vec3{1.0f, -1.0f, -1.0f},
+		glm::vec3{1.0f, 1.0f, 1.0f},
+		glm::vec3{1.0f, -1.0f, 1.0f} 
+	};
+
+	glm::mat4 transform = glm::translate(glm::mat4{ 1.0f }, glm::vec3(5.0, 1.5, 0.0));
+	transform = glm::scale(transform, glm::vec3(2.0, 1.5, 5.0));
+	auto inv = glm::inverse(transform);
+
+	auto ttri = transformSupportFn(getSupportFn(triangle), transform, inv);
+
+	auto result = gjk(getSupportFn(circle), ttri);
+	EXPECT_TRUE(result);
+
+	result = gjk(ttri, getSupportFn(circle));
+	EXPECT_TRUE(result);
+}
+
 // because EXPECT_FLOAT_EQ is WAYYYYY too strict.
 bool floatEq(float a, float b) {
 	return std::abs(a - b) <= 0.00005f;
@@ -218,7 +239,7 @@ TEST(EPA, simpleBoxesGiveCorrectDirection) {
 	auto result = epa(*simplex, getSupportFn(boxA), getSupportFn(boxB));
 	ASSERT_LE(std::abs(result.depth - 0.50001f), 0.0001f);
 
-	boxB.translate(result.normal * (result.depth + 0.0001f));
+	boxB.translate(result.normal * (result.depth + 0.004f));
 	EXPECT_FALSE(gjk(getSupportFn(boxA), getSupportFn(boxB)));
 }
 
@@ -248,4 +269,31 @@ TEST(EPA, mixedShapesGiveCorrectDirection) {
 
 	auto result = epa(*simplex, sptA, sptB);
 	EXPECT_TRUE(floatEq(result.depth, 0.5));
+}
+
+TEST(EPA, mixedShapesGiveCorrectDirection2) {
+	Circle3D circle{ glm::vec3{-6.91016197f, 0.49365893f, 1.70220768f}, 0.5f };
+	Tri3D tri{ 
+		glm::vec3{-1.0, -1.0f, 1.0f}, 
+		glm::vec3{-1.0f, 1.0f, 1.0f},
+		glm::vec3{-1.0f, 1.0f, -1.0f}
+	};
+
+	glm::mat4 t = glm::translate(glm::mat4{ 1.0f }, glm::vec3(-5.0, 0.5, 1.0));
+	t = glm::rotate(t, glm::radians(45.0f), glm::vec3(0.0, 0.0, 1.0));
+
+	auto sptA = getSupportFn(circle);
+	auto sptB = transformSupportFn(getSupportFn(tri), t, glm::inverse(t));
+
+	auto simplex = gjk(sptA, sptB);
+	ASSERT_TRUE(simplex);
+
+	auto result = epa(*simplex, sptA, sptB);
+	EXPECT_LE(result.depth, 0.004f);
+
+	simplex = gjk(sptB, sptA);
+	ASSERT_TRUE(simplex);
+
+	result = epa(*simplex, sptB, sptA);
+	EXPECT_LE(result.depth, 0.004f);
 }