attempt some internal edge collision fix

src/DifBuilder.hx

@@ -1,5 +1,6 @@
 package src;
 
+import dif.Edge;
 import h3d.shader.pbr.PropsValues;
 import h3d.mat.Material;
 import src.ResourceLoader;
@@ -40,6 +41,24 @@ class DifBuilderTriangle {
 	public function new() {}
 }
 
+class TriangleEdge {
+	public var index1:Int;
+	public var index2:Int;
+
+	public var surfaceIndex:Int;
+
+	public function new(i1:Int, i2:Int, surfaceIndex:Int) {
+		if (i2 < i1) {
+			index1 = i2;
+			index2 = i1;
+		} else {
+			index1 = i1;
+			index2 = i2;
+		}
+		this.surfaceIndex = surfaceIndex;
+	}
+}
+
 typedef VertexBucket = {
 	var referenceNormal:Point3F;
 	var triangleIndices:Array<Int>;
@@ -106,6 +125,9 @@ class DifBuilder {
 
 		var vertexBuckets = new Map<Point3F, Array<VertexBucket>>();
 
+		var edges = [];
+		var colliderSurfaces = [];
+
 		for (i in 0...hulls.length) {
 			var hullTris = [];
 			var hull = hulls[i];
@@ -137,6 +159,10 @@ class DifBuilder {
 				colliderSurface.points = [];
 				colliderSurface.normals = [];
 				colliderSurface.indices = [];
+				colliderSurface.edgeData = [];
+				colliderSurface.edgeDots = [];
+				colliderSurface.originalIndices = [];
+				colliderSurface.originalSurfaceIndex = surfaceindex;
 
 				for (k in (surface.windingStart + 2)...(surface.windingStart + surface.windingCount)) {
 					var p1, p2, p3;
@@ -144,12 +170,26 @@ class DifBuilder {
 						p1 = points[geo.windings[k]];
 						p2 = points[geo.windings[k - 1]];
 						p3 = points[geo.windings[k - 2]];
+						colliderSurface.originalIndices.push(geo.windings[k]);
+						colliderSurface.originalIndices.push(geo.windings[k - 1]);
+						colliderSurface.originalIndices.push(geo.windings[k - 2]);
 					} else {
 						p1 = points[geo.windings[k - 2]];
 						p2 = points[geo.windings[k - 1]];
 						p3 = points[geo.windings[k]];
+						colliderSurface.originalIndices.push(geo.windings[k - 2]);
+						colliderSurface.originalIndices.push(geo.windings[k - 1]);
+						colliderSurface.originalIndices.push(geo.windings[k]);
 					}
 
+					var e1 = new TriangleEdge(geo.windings[k], geo.windings[k - 1], surfaceindex);
+					var e2 = new TriangleEdge(geo.windings[k - 1], geo.windings[k - 2], surfaceindex);
+					var e3 = new TriangleEdge(geo.windings[k], geo.windings[k - 2], surfaceindex);
+
+					edges.push(e1);
+					edges.push(e2);
+					edges.push(e3);
+
 					var texgen = geo.texGenEQs[surface.texGenIndex];
 
 					var uv1 = new Point2F(p1.x * texgen.planeX.x
@@ -237,6 +277,157 @@ class DifBuilder {
 
 				colliderSurface.generateBoundingBox();
 				collider.addSurface(colliderSurface);
+				colliderSurfaces.push(colliderSurface);
+			}
+		}
+
+		var edgeMap:Map<Int, TriangleEdge> = new Map();
+		var internalEdges:Map<Int, Bool> = new Map();
+
+		var difEdges:Map<Int, Edge> = [];
+
+		for (edge in edges) {
+			var edgeHash = edge.index1 >= edge.index2 ? edge.index1 * edge.index1 + edge.index1 + edge.index2 : edge.index1 + edge.index2 * edge.index2;
+
+			if (internalEdges.exists(edgeHash))
+				continue;
+
+			if (edgeMap.exists(edgeHash)) {
+				if (edgeMap[edgeHash].surfaceIndex == edge.surfaceIndex) {
+					// Internal edge
+					internalEdges.set(edgeHash, true);
+					edgeMap.remove(edgeHash);
+					// trace('Removing internal edge: ${edge.index1} ${edge.index2}');
+				} else {
+					var difEdge = new Edge(edge.index1, edge.index2, edge.surfaceIndex, edgeMap[edgeHash].surfaceIndex);
+					difEdges.set(edgeHash, difEdge); // Literal edge
+				}
+			} else {
+				edgeMap.set(edgeHash, edge);
+			}
+		}
+
+		function hashEdge(i1:Int, i2:Int) {
+			return i1 >= i2 ? i1 * i1 + i1 + i2 : i1 + i2 * i2;
+		}
+
+		function getEdgeDot(edge:Edge) {
+			var edgeSurface0 = edge.surfaceIndex0;
+			var surface0 = geo.surfaces[edgeSurface0];
+
+			var planeindex = surface0.planeIndex;
+
+			var planeFlipped = (planeindex & 0x8000) == 0x8000;
+			if (planeFlipped)
+				planeindex &= ~0x8000;
+
+			var plane = geo.planes[planeindex];
+			var normal0 = geo.normals[plane.normalIndex];
+
+			if (planeFlipped)
+				normal0 = normal0.scalar(-1);
+
+			var edgeSurface1 = edge.surfaceIndex1;
+			var surface1 = geo.surfaces[edgeSurface1];
+
+			planeindex = surface1.planeIndex;
+
+			planeFlipped = (planeindex & 0x8000) == 0x8000;
+			if (planeFlipped)
+				planeindex &= ~0x8000;
+
+			plane = geo.planes[planeindex];
+			var normal1 = geo.normals[plane.normalIndex];
+
+			if (planeFlipped)
+				normal1 = normal1.scalar(-1);
+
+			var dot = normal0.dot(normal1);
+
+			return dot;
+		}
+
+		function getEdgeNormal(edge:Edge) {
+			var edgeSurface0 = edge.surfaceIndex0;
+			var surface0 = geo.surfaces[edgeSurface0];
+
+			var planeindex = surface0.planeIndex;
+
+			var planeFlipped = (planeindex & 0x8000) == 0x8000;
+			if (planeFlipped)
+				planeindex &= ~0x8000;
+
+			var plane = geo.planes[planeindex];
+			var normal0 = geo.normals[plane.normalIndex];
+
+			if (planeFlipped)
+				normal0 = normal0.scalar(-1);
+
+			var edgeSurface1 = edge.surfaceIndex1;
+			var surface1 = geo.surfaces[edgeSurface1];
+
+			planeindex = surface1.planeIndex;
+
+			planeFlipped = (planeindex & 0x8000) == 0x8000;
+			if (planeFlipped)
+				planeindex &= ~0x8000;
+
+			plane = geo.planes[planeindex];
+			var normal1 = geo.normals[plane.normalIndex];
+
+			if (planeFlipped)
+				normal1 = normal1.scalar(-1);
+
+			var norm = normal0.add(normal1).scalarDiv(2).normalized();
+
+			var vec = new Vector(norm.x, norm.y, norm.z);
+
+			return vec;
+		}
+
+		for (colliderSurface in colliderSurfaces) {
+			var i = 0;
+			while (i < colliderSurface.indices.length) {
+				var e1e2 = hashEdge(colliderSurface.originalIndices[i], colliderSurface.originalIndices[i + 1]);
+				var e2e3 = hashEdge(colliderSurface.originalIndices[i + 1], colliderSurface.originalIndices[i + 2]);
+				var e1e3 = hashEdge(colliderSurface.originalIndices[i], colliderSurface.originalIndices[i + 2]);
+
+				var edgeData = 0;
+				if (difEdges.exists(e1e2)) {
+					if (getEdgeDot(difEdges[e1e2]) < Math.cos(Math.PI / 12)) {
+						edgeData |= 1;
+					}
+					colliderSurface.edgeDots.push(getEdgeDot(difEdges[e1e2]));
+					// colliderSurface.edgeNormals.push(getEdgeNormal(difEdges[e1e2]));
+				} else {
+					colliderSurface.edgeDots.push(0);
+					// colliderSurface.edgeNormals.push(new Vector(0, 0, 0));
+				}
+				if (difEdges.exists(e2e3)) {
+					if (getEdgeDot(difEdges[e2e3]) < Math.cos(Math.PI / 12)) {
+						edgeData |= 2;
+					}
+					colliderSurface.edgeDots.push(getEdgeDot(difEdges[e2e3]));
+					// colliderSurface.edgeNormals.push(getEdgeNormal(difEdges[e2e3]));
+					// colliderSurface.edgeDots.push(dot);
+				} else {
+					colliderSurface.edgeDots.push(0);
+					// colliderSurface.edgeNormals.push(new Vector(0, 0, 0));
+				}
+				if (difEdges.exists(e1e3)) {
+					if (getEdgeDot(difEdges[e1e3]) < Math.cos(Math.PI / 12)) {
+						edgeData |= 4;
+					}
+					colliderSurface.edgeDots.push(getEdgeDot(difEdges[e1e3]));
+					// colliderSurface.edgeNormals.push(getEdgeNormal(difEdges[e1e3]));
+					// colliderSurface.edgeDots.push(dot);
+				} else {
+					colliderSurface.edgeDots.push(0);
+					// colliderSurface.edgeNormals.push(new Vector(0, 0, 0));
+				}
+
+				colliderSurface.edgeData.push(edgeData);
+				i += 3;
 			}
 		}
 
@@ -272,6 +463,7 @@ class DifBuilder {
 			}
 		}
 
+		collider.difEdgeMap = difEdges;
 		collider.generateBoundingBox();
 		itr.collider = collider;
 

src/collision/Collision.hx

@@ -1,5 +1,6 @@
 package collision;
 
+import haxe.Exception;
 import dif.math.Point3F;
 import dif.math.PlaneF;
 import h3d.col.Plane;
@@ -55,7 +56,12 @@ class Collision {
 		return p;
 	}
 
-	public static function IntersectTriangleSphere(v0:Vector, v1:Vector, v2:Vector, normal:Vector, center:Vector, radius:Float) {
+	// EdgeData is bitfield
+	// 001b: v0v1 is edge
+	// 010b: v1v2 is edge
+	// 100b: v0v2 is edge
+	public static function IntersectTriangleSphere(v0:Vector, v1:Vector, v2:Vector, normal:Vector, center:Vector, radius:Float, edgeData:Int,
+			edgeDots:Array<Float>) {
 		var radiusSq = radius * radius;
 
 		var res:ITSResult = {
@@ -94,19 +100,49 @@ class Collision {
 		var r2 = ClosestPointLine(v1, v2, center);
 		var r3 = ClosestPointLine(v2, v0, center);
 
+		var chosenEdge = 0; // Bitfield
+
 		var chosenPt:Vector;
-		if (r1.distanceSq(center) < r2.distanceSq(center))
+		if (r1.distanceSq(center) < r2.distanceSq(center)) {
 			chosenPt = r1;
-		else
+			chosenEdge = 1;
+		} else {
 			chosenPt = r2;
+			chosenEdge = 2;
+		}
 		if (chosenPt.distanceSq(center) < r3.distanceSq(center))
 			res.point = chosenPt;
-		else
+		else {
+			chosenEdge = 4;
 			res.point = r3;
+		}
 
 		if (res.point.distanceSq(center) <= radiusSq) {
 			res.result = true;
-			res.normal = center.sub(res.point).normalized();
+
+			if (chosenEdge & edgeData > 0) {
+				res.normal = center.sub(res.point).normalized();
+			} else { // We hit an internal edge
+				chosenEdge -= 1;
+				if (chosenEdge > 2)
+					chosenEdge--;
+				// if (edgeNormals[chosenEdge].length() < 0.5) {
+				//	res.normal = center.sub(res.point).normalized();
+				// } else
+				var edgeDotAng = Math.acos(edgeDots[chosenEdge]);
+				if (edgeDotAng < Math.PI / 12) {
+					if (edgeDotAng == 0) {
+						res.normal = center.sub(res.point).normalized();
+					} else {
+						res.normal = normal; // edgeNormals[chosenEdge];
+					}
+				} else {
+					res.result = false;
+					res.normal = center.sub(res.point).normalized();
+				}
+				// trace("Internal Edge Collision");
+			}
+
 			return res;
 		}
 
@@ -131,7 +167,6 @@ class Collision {
 		// 	res.normal = center.sub(r3).normalized();
 		// 	return res;
 		// }
-
 		// Check points
 		// if (center.sub(v0).lengthSq() < radiusSq) {
 		// 	res.result = true;
@@ -144,17 +179,14 @@ class Collision {
 		// 	res.result = true;
 		// 	res.point = v1;
 		// 	res.normal = center.sub(v1).normalized();
-
 		// 	return res;
 		// }
 		// if (center.sub(v2).lengthSq() < radiusSq) {
 		// 	res.result = true;
 		// 	res.point = v2;
 		// 	res.normal = center.sub(v2).normalized();
-
 		// 	return res;
 		// }
-
 		// Check plane
 		// var p = PlaneF.ThreePoints(toDifPoint(v0), toDifPoint(v1), toDifPoint(v2));
 		return res;
@@ -291,7 +323,8 @@ class Collision {
 		return !(u.dot(w) < 0);
 	}
 
-	public static function IntersectTriangleCapsule(start:Vector, end:Vector, radius:Float, p1:Vector, p2:Vector, p3:Vector, normal:Vector) {
+	public static function IntersectTriangleCapsule(start:Vector, end:Vector, radius:Float, p1:Vector, p2:Vector, p3:Vector, normal:Vector, edgeData:Int,
+			edgeDots:Array<Float>) {
 		var dir = end.sub(start);
 		var d = -(p1.dot(normal));
 		var t = -(start.dot(normal) - d) / dir.dot(normal);
@@ -300,7 +333,7 @@ class Collision {
 		if (t < 0)
 			t = 0;
 		var tracePoint = start.add(dir.multiply(t));
-		return IntersectTriangleSphere(p1, p2, p3, normal, tracePoint, radius);
+		return IntersectTriangleSphere(p1, p2, p3, normal, tracePoint, radius, edgeData, edgeDots);
 	}
 
 	private static function GetLowestRoot(a:Float, b:Float, c:Float, max:Float):Null<Float> {

src/collision/CollisionEntity.hx

@@ -32,6 +32,8 @@ class CollisionEntity implements IOctreeObject {
 
 	public var userData:Int;
 
+	public var difEdgeMap:Map<Int, dif.Edge>;
+
 	public function new(go:GameObject) {
 		this.go = go;
 		this.octree = new Octree();
@@ -116,6 +118,10 @@ class CollisionEntity implements IOctreeObject {
 			return new Vector(pt.x, pt.y, pt.z);
 		}
 
+		function hashEdge(i1:Int, i2:Int) {
+			return i1 >= i2 ? i1 * i1 + i1 + i2 : i1 + i2 * i2;
+		}
+
 		var contacts = [];
 
 		for (obj in surfaces) {
@@ -130,9 +136,28 @@ class CollisionEntity implements IOctreeObject {
 				var v = surface.points[surface.indices[i + 1]].transformed(tform);
 				var v2 = surface.points[surface.indices[i + 2]].transformed(tform);
 
+				// var e1e2 = hashEdge(surface.originalIndices[i], surface.originalIndices[i + 1]);
+				// var e2e3 = hashEdge(surface.originalIndices[i + 1], surface.originalIndices[i + 2]);
+				// var e1e3 = hashEdge(surface.originalIndices[i], surface.originalIndices[i + 3]);
+
+				// var edgeData = 0;
+				// if (this.difEdgeMap.exists(e1e2)) {
+				// 	edgeData |= 1;
+				// }
+				// if (this.difEdgeMap.exists(e2e3)) {
+				// 	edgeData |= 2;
+				// }
+				// if (this.difEdgeMap.exists(e1e3)) {
+				// 	edgeData |= 4;
+				// }
+
+				var edgeData = surface.edgeData[Math.floor(i / 3)];
+
+				var edgeDots = surface.edgeDots.slice(Math.floor(i / 3), Math.floor(i / 3) + 3);
+
 				var surfacenormal = surface.normals[surface.indices[i]].transformed3x3(transform).normalized();
 
-				var res = Collision.IntersectTriangleSphere(v0, v, v2, surfacenormal, position, radius);
+				var res = Collision.IntersectTriangleSphere(v0, v, v2, surfacenormal, position, radius, edgeData, edgeDots);
 				var closest = res.point;
 				// var closest = Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surfacenormal);
 				if (closest != null) {
@@ -144,23 +169,24 @@ class CollisionEntity implements IOctreeObject {
 							normal.normalize();
 
 							// We find the normal that is closest to the surface normal, sort of fixes weird edge cases of when colliding with
-							var testDot = normal.dot(surfacenormal);
+							// var testDot = normal.dot(surfacenormal);
-							if (testDot > bestDot) {
+							// if (testDot > bestDot) {
-								bestDot = testDot;
+							// 	bestDot = testDot;
 
-								var cinfo = new CollisionInfo();
+							var cinfo = new CollisionInfo();
-								cinfo.normal = normal;
+							cinfo.normal = normal;
-								cinfo.point = closest;
+							cinfo.point = closest;
-								// cinfo.collider = this;
+							// cinfo.collider = this;
-								cinfo.velocity = this.velocity.clone();
+							cinfo.velocity = this.velocity.clone();
-								cinfo.contactDistance = Math.sqrt(contactDist);
+							cinfo.contactDistance = Math.sqrt(contactDist);
-								cinfo.otherObject = this.go;
+							cinfo.otherObject = this.go;
-								// cinfo.penetration = radius - (position.sub(closest).dot(normal));
+							// cinfo.penetration = radius - (position.sub(closest).dot(normal));
-								cinfo.restitution = surface.restitution;
+							cinfo.restitution = surface.restitution;
-								cinfo.force = surface.force;
+							cinfo.force = surface.force;
-								cinfo.friction = surface.friction;
+							cinfo.friction = surface.friction;
-								surfaceBestContact = cinfo;
+							contacts.push(cinfo);
-							}
+							// surfaceBestContact = cinfo;
+							// }
 						}
 					}
 				}
@@ -168,8 +194,8 @@ class CollisionEntity implements IOctreeObject {
 				i += 3;
 			}
 
-			if (surfaceBestContact != null)
+			// if (surfaceBestContact != null)
-				contacts.push(surfaceBestContact);
+			// contacts.push(surfaceBestContact);
 		}
 
 		return contacts;

src/collision/CollisionSurface.hx

@@ -19,6 +19,13 @@ class CollisionSurface implements IOctreeObject {
 	public var restitution:Float = 1;
 	public var force:Float = 0;
 
+	public var edgeData:Array<Int>;
+
+	public var edgeDots:Array<Float>;
+	public var originalIndices:Array<Int>;
+
+	public var originalSurfaceIndex:Int;
+
 	public function new() {}
 
 	public function getElementType() {