don't collide with tiny triangles

src/Debug.hx

@@ -10,8 +10,15 @@ class Debug {
 	static var drawBounds:Bool = false;
 
 	static var _triangles:Array<h3d.col.Point> = [];
+	static var _spheres:Array<{
+		position:Vector,
+		radius:Float
+	}> = [];
 
 	static var debugTriangles:h3d.scene.Mesh;
+	static var debugSphere:h3d.scene.MeshBatch;
+
+	public static function init() {}
 
 	public static function update() {
 		if (_triangles.length != 0 && drawBounds) {
@@ -30,6 +37,28 @@ class Debug {
 				debugTriangles = null;
 			}
 		}
+		if (_spheres.length != 0 && drawBounds) {
+			if (debugSphere == null) {
+				var sphprim = new h3d.prim.Sphere();
+				sphprim.addUVs();
+				sphprim.addNormals();
+				debugSphere = new h3d.scene.MeshBatch(sphprim, h3d.mat.Material.create());
+				debugSphere.material.castShadows = false;
+				debugSphere.material.receiveShadows = false;
+				MarbleGame.instance.scene.addChild(debugSphere);
+			}
+			debugSphere.begin(_spheres.length);
+			for (sph in _spheres) {
+				debugSphere.setPosition(sph.position.x, sph.position.y, sph.position.z);
+				debugSphere.setScale(sph.radius);
+				debugSphere.emitInstance();
+			}
+		} else {
+			if (debugSphere != null) {
+				debugSphere.remove();
+				debugSphere = null;
+			}
+		}
 	}
 
 	public static function drawTriangle(p1:Vector, p2:Vector, p3:Vector) {
@@ -37,4 +66,8 @@ class Debug {
 		_triangles.push(p2.toPoint());
 		_triangles.push(p1.toPoint());
 	}
+
+	public static function drawSphere(centre:Vector, radius:Float) {
+		_spheres.push({position: centre.clone(), radius: radius});
+	}
 }

src/Main.hx

@@ -1,5 +1,6 @@
 package;
 
+import src.Debug;
 import src.Marbleland;
 import src.Console;
 import hxd.Key;

src/Marble.hx

@@ -1127,6 +1127,7 @@ class Marble extends GameObject {
 								found = true;
 								// iterationFound = true;
 								i += 3;
+								// Debug.drawSphere(currentFinalPos, radius);
 								continue;
 							}
 						}
@@ -1206,6 +1207,7 @@ class Marble extends GameObject {
 									lastContactPos = vertDiff.multiply(distanceAlongEdge / edgeLen).add(thisVert);
 									lastVert = thisVert;
 									found = true;
+									// Debug.drawSphere(currentFinalPos, radius);
 									// iterationFound = true;
 									continue;
 								}
@@ -1250,6 +1252,7 @@ class Marble extends GameObject {
 										currentFinalPos = position.add(relVel.multiply(finalT));
 										lastContactPos = thisVert;
 										found = true;
+										// Debug.drawSphere(currentFinalPos, radius);
 										// iterationFound = true;
 									}
 								}
@@ -1297,6 +1300,7 @@ class Marble extends GameObject {
 
 							finalT = edgeCollisionTime;
 							currentFinalPos = position.add(relVel.multiply(finalT));
+							// Debug.drawSphere(currentFinalPos, radius);
 
 							lastVert = thisVert;
 							found = true;
@@ -1503,13 +1507,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++;
 					}
 				}

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) {