From 402e7a98939ad78cc843cf4ae2fd7ee45e30577a Mon Sep 17 00:00:00 2001
From: RandomityGuy <31925790+RandomityGuy@users.noreply.github.com>
Date: Sun, 19 Feb 2023 18:42:28 +0530
Subject: [PATCH] don't collide with tiny triangles

---
 src/Marble.hx              | 23 ++++++++++++++++++++++-
 src/collision/Collision.hx | 10 ++++++++--
 2 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/src/Marble.hx b/src/Marble.hx
index 85657cce..06295b40 100644
--- a/src/Marble.hx
+++ b/src/Marble.hx
@@ -892,6 +892,7 @@ class Marble extends GameObject {
 								found = true;
 								// iterationFound = true;
 								i += 3;
+								// Debug.drawSphere(currentFinalPos, radius);
 								continue;
 							}
 						}
@@ -971,6 +972,7 @@ class Marble extends GameObject {
 									lastContactPos = vertDiff.multiply(distanceAlongEdge / edgeLen).add(thisVert);
 									lastVert = thisVert;
 									found = true;
+									// Debug.drawSphere(currentFinalPos, radius);
 									// iterationFound = true;
 									continue;
 								}
@@ -1015,6 +1017,7 @@ class Marble extends GameObject {
 										currentFinalPos = position.add(relVel.multiply(finalT));
 										lastContactPos = thisVert;
 										found = true;
+										// Debug.drawSphere(currentFinalPos, radius);
 										// iterationFound = true;
 									}
 								}
@@ -1062,6 +1065,7 @@ class Marble extends GameObject {
 
 							finalT = edgeCollisionTime;
 							currentFinalPos = position.add(relVel.multiply(finalT));
+							// Debug.drawSphere(currentFinalPos, radius);
 
 							lastVert = thisVert;
 							found = true;
@@ -1268,13 +1272,30 @@ class Marble extends GameObject {
 		do {
 			var resolved = 0;
 			for (testTri in concernedContacts) {
+				// Check if we are on wrong side of the triangle
+				if (testTri.n.dot(position) - testTri.n.dot(testTri.v[0]) < 0) {
+					continue;
+				}
+
+				var t1 = testTri.v[1].sub(testTri.v[0]);
+				var t2 = testTri.v[2].sub(testTri.v[0]);
+				var tarea = Math.abs(t1.cross(t2).length()) / 2.0;
+
+				// Check if our triangle is too small to be collided with
+				if (tarea < 0.001) {
+					continue;
+				}
+
 				var tsi = Collision.TriangleSphereIntersection(testTri.v[0], testTri.v[1], testTri.v[2], testTri.n, position, radius, testTri.edge,
 					testTri.concavity);
 				if (tsi.result) {
+					var separatingDistance = position.sub(tsi.point).normalized();
 					var distToContactPlane = tsi.point.distance(position);
 					if (radius - 0.005 - distToContactPlane > 0.0001) {
 						// Nudge to the surface of the contact plane
-						position = position.add(tsi.normal.multiply(radius - distToContactPlane - 0.005));
+						Debug.drawTriangle(testTri.v[0], testTri.v[1], testTri.v[2]);
+						Debug.drawSphere(position, radius);
+						position = position.add(separatingDistance.multiply(radius - distToContactPlane - 0.005));
 						resolved++;
 					}
 				}
diff --git a/src/collision/Collision.hx b/src/collision/Collision.hx
index 1cf314a5..583fcd8f 100644
--- a/src/collision/Collision.hx
+++ b/src/collision/Collision.hx
@@ -24,6 +24,7 @@ typedef ITSResult = {
 	var result:Bool;
 	var normal:Vector;
 	var point:Vector;
+	var resIdx:Int;
 }
 
 class Collision {
@@ -67,7 +68,8 @@ class Collision {
 		var res:ITSResult = {
 			result: false,
 			point: null,
-			normal: null
+			normal: null,
+			resIdx: 0
 		};
 
 		var pnorm = normal.clone();
@@ -146,7 +148,8 @@ class Collision {
 		var res:ITSResult = {
 			result: false,
 			point: null,
-			normal: null
+			normal: null,
+			resIdx: -1
 		};
 
 		var v0 = A;
@@ -217,6 +220,7 @@ class Collision {
 			res.result = true;
 			res.normal = N.clone();
 			res.point = P.sub(N.multiply(P.sub(v0).dot(N)));
+			res.resIdx = 0;
 		} else {
 			var closestPt = P.sub(N.multiply(P.sub(v0).dot(N)));
 			var r1 = ClosestPointLine(v0, v1, closestPt);
@@ -242,6 +246,8 @@ class Collision {
 			res.normal = P.sub(res.point).normalized();
 			res.result = true;
 
+			res.resIdx = chosenEdge;
+
 			// if (res.normal.dot(N) > 0.8) {
 			// 	// Internal edge
 			// 	if (chosenEdge & edgeData > 0) {