implement gjk collision for dts

compile.hxml

@@ -1,5 +1,4 @@
 -cp src
--lib headbutt
 -lib heaps
 -lib hlsdl
 -lib polygonal-ds

src/DifBuilder.hx

@@ -1,5 +1,6 @@
 package src;
 
+import h3d.mat.Material;
 import src.ResourceLoader;
 import src.PathedInterior;
 import h3d.Vector;
@@ -213,6 +214,9 @@ class DifBuilder {
 		itr.collider = collider;
 
 		function canFindTex(tex:String) {
+			if (["NULL"].contains(tex)) {
+				return false;
+			}
 			if (tex.indexOf('/') != -1) {
 				tex = tex.split('/')[1];
 			}
@@ -289,9 +293,15 @@ class DifBuilder {
 			prim.uvs = uvs;
 			prim.normals = normals;
 
-			var texture:Texture = loader.load(tex(grp)).toImage().toTexture();
-			texture.wrap = Wrap.Repeat;
-			var material = h3d.mat.Material.create(texture);
+			var material:Material;
+			var texture:Texture;
+			if (canFindTex(grp)) {
+				texture = loader.load(tex(grp)).toImage().toTexture();
+				texture.wrap = Wrap.Repeat;
+				material = h3d.mat.Material.create(texture);
+			} else {
+				material = Material.create();
+			}
 			// material.mainPass.wireframe = true;
 
 			var mesh = new Mesh(prim, material, itr);

src/DtsObject.hx

@@ -1,5 +1,8 @@
 package src;
 
+import collision.CollisionHull;
+import collision.CollisionSurface;
+import collision.CollisionEntity;
 import hxd.FloatBuffer;
 import h3d.prim.DynamicPrimitive;
 import h3d.scene.Trail;
@@ -76,6 +79,7 @@ class DtsObject extends Object {
 	var fs:Loader;
 
 	var rootObject:Object;
+	var colliders:Array<CollisionEntity>;
 
 	public function new() {
 		super();
@@ -89,6 +93,7 @@ class DtsObject extends Object {
 
 		var graphNodes = [];
 		var rootNodesIdx = [];
+		colliders = [];
 
 		for (i in 0...this.dts.nodes.length) {
 			graphNodes.push(new Object());
@@ -105,7 +110,6 @@ class DtsObject extends Object {
 
 		this.graphNodes = graphNodes;
 		// this.rootGraphNodes = graphNodes.filter(node -> node.parent == null);
-		this.updateNodeTransforms();
 
 		var affectedBySequences = this.dts.sequences.length > 0 ? (this.dts.sequences[0].rotationMatters.length < 0 ? 0 : this.dts.sequences[0].rotationMatters[0]) | (this.dts.sequences[0].translationMatters.length > 0 ? this.dts.sequences[0].translationMatters[0] : 0) : 0;
 
@@ -127,8 +131,8 @@ class DtsObject extends Object {
 					if (mesh == null)
 						continue;
 
-					var vertices = mesh.vertices.map(v -> new Vector(v.x, v.y, v.z));
-					var vertexNormals = mesh.normals.map(v -> new Vector(v.x, v.y, v.z));
+					var vertices = mesh.vertices.map(v -> new Vector(-v.x, v.y, v.z));
+					var vertexNormals = mesh.normals.map(v -> new Vector(-v.x, v.y, v.z));
 
 					var geometry = this.generateMaterialGeometry(mesh, vertices, vertexNormals);
 					for (k in 0...geometry.length) {
@@ -145,6 +149,41 @@ class DtsObject extends Object {
 			}
 		}
 
+		for (i in 0...dts.nodes.length) {
+			var objects = dts.objects.filter(object -> object.node == i);
+			var meshSurfaces = [];
+			var collider = new CollisionHull();
+
+			for (object in objects) {
+				var isCollisionObject = dts.names[object.name].substr(0, 3).toLowerCase() == "col";
+
+				if (isCollisionObject) {
+					for (j in object.firstMesh...(object.firstMesh + object.numMeshes)) {
+						if (j >= this.dts.meshes.length)
+							continue;
+
+						var mesh = this.dts.meshes[j];
+						if (mesh == null)
+							continue;
+
+						var vertices = mesh.vertices.map(v -> new Vector(v.x, v.y, v.z));
+						var vertexNormals = mesh.normals.map(v -> new Vector(v.x, v.y, v.z));
+
+						var surfaces = this.generateCollisionGeometry(mesh, vertices, vertexNormals);
+						for (surface in surfaces)
+							collider.addSurface(surface);
+						meshSurfaces = meshSurfaces.concat(surfaces);
+					}
+				}
+			}
+			if (meshSurfaces.length != 0)
+				colliders.push(collider);
+			else
+				colliders.push(null);
+		}
+
+		this.updateNodeTransforms();
+
 		for (i in 0...this.dts.meshes.length) {
 			var mesh = this.dts.meshes[i];
 			if (mesh == null)
@@ -189,7 +228,7 @@ class DtsObject extends Object {
 			rootObject.addChild(this.skinMeshData.geometry);
 		}
 
-		rootObject.scaleX = -1;
+		// rootObject.scaleX = -1;
 	}
 
 	function computeMaterials() {
@@ -256,9 +295,58 @@ class DtsObject extends Object {
 			quat.toMatrix(mat);
 			mat.setPosition(new Vector(translation.x, translation.y, translation.z));
 			this.graphNodes[i].setTransform(mat);
+			var absTform = this.graphNodes[i].getAbsPos().clone();
+			if (this.colliders[i] != null)
+				// this.colliders[i].setTransform(Matrix.I());
+				this.colliders[i].setTransform(absTform);
 		}
 	}
 
+	function generateCollisionGeometry(dtsMesh:dts.Mesh, vertices:Array<Vector>, vertexNormals:Array<Vector>) {
+		var surfaces = this.materials.map(x -> new CollisionSurface());
+		for (surface in surfaces) {
+			surface.points = [];
+			surface.normals = [];
+			surface.indices = [];
+		}
+		for (primitive in dtsMesh.primitives) {
+			var k = 0;
+			var geometrydata = surfaces[primitive.matIndex];
+
+			for (i in primitive.firstElement...(primitive.firstElement + primitive.numElements - 2)) {
+				var i1 = dtsMesh.indices[i];
+				var i2 = dtsMesh.indices[i + 1];
+				var i3 = dtsMesh.indices[i + 2];
+
+				if (k % 2 == 0) {
+					// Swap the first and last index to mainting correct winding order
+					var temp = i1;
+					i1 = i3;
+					i3 = temp;
+				}
+
+				for (index in [i1, i2, i3]) {
+					var vertex = vertices[index];
+					geometrydata.points.push(new Vector(-vertex.x, vertex.y, vertex.z));
+
+					var normal = vertexNormals[index];
+					geometrydata.normals.push(new Vector(-normal.x, normal.y, normal.z));
+				}
+
+				geometrydata.indices.push(geometrydata.indices.length);
+				geometrydata.indices.push(geometrydata.indices.length);
+				geometrydata.indices.push(geometrydata.indices.length);
+
+				k++;
+			}
+		}
+		for (surface in surfaces) {
+			surface.generateBoundingBox();
+			// surface.generateNormals();
+		}
+		return surfaces;
+	}
+
 	function generateMaterialGeometry(dtsMesh:dts.Mesh, vertices:Array<Vector>, vertexNormals:Array<Vector>) {
 		var materialGeometry:Array<MaterialGeometry> = this.materials.map(x -> {
 			vertices: [],
@@ -490,8 +578,6 @@ class DtsObject extends Object {
 					if (prim.buffer != null) {
 						prim.addNormals();
 						prim.flush();
-						// prim.buffer.uploadBytes()
-						// prim.buffer.dispose();
 					}
 					mesh.primitive = prim;
 					mesh = cast info.geometry.children[meshIndex];
@@ -501,35 +587,28 @@ class DtsObject extends Object {
 						vbuffer = prim.getBuffer(prim.points.length);
 					}
 				}
-				// if (prim.buffer == null) {
 				var vertex = info.vertices[i];
 				var normal = info.normals[i];
 				prim.points[pos] = vertex.toPoint();
 				if (prim.buffer != null) {
 					prim.dirtyFlags[pos] = true;
 				}
-				// } else {
-				// vbuffer[pos * 8] = info.vertices[i].x;
-				// vbuffer[(pos * 8) + 1] = info.vertices[i].y;
-				// vbuffer[(pos * 8) + 2] = info.vertices[i].z;
-				// var fb = new FloatBuffer();
-				// fb.push(info.vertices[i].x);
-				// fb.push(info.vertices[i].y);
-				// fb.push(info.vertices[i].z);
-				// prim.buffer.uploadVector(fb, pos * 8, 1);
-				// }
-				// prim.normals[pos] = normal.toPoint();
 				pos++;
 			}
 			if (prim.buffer != null) {
 				prim.addNormals();
 				prim.flush();
-				// prim.buffer.dispose();
 			}
 			if (_regenNormals) {
 				_regenNormals = false;
 			}
 		}
+
+		for (i in 0...this.colliders.length) {
+			var absTform = this.graphNodes[i].getAbsPos().clone();
+			if (this.colliders[i] != null)
+				this.colliders[i].setTransform(absTform);
+		}
 	}
 
 	public function getMountTransform(mountPoint:Int) {

src/InteriorGeometry.hx

@@ -2,9 +2,6 @@ package src;
 
 import h3d.Matrix;
 import collision.CollisionEntity;
-import headbutt.threed.Headbutt;
-import headbutt.threed.shapes.Sphere;
-import glm.Vec3;
 import dif.math.Point3F;
 import h3d.scene.Mesh;
 import h3d.col.Bounds;

src/Main.hx

@@ -34,49 +34,53 @@ class Main extends hxd.App {
 		var loader = new Loader(fileSystem);
 
 		dtsObj = new DtsObject();
-		dtsObj.dtsPath = "data/shapes/hazards/tornado.dts";
+		dtsObj.dtsPath = "data/shapes/items/timetravel.dts";
 		dtsObj.isCollideable = false;
 		dtsObj.isTSStatic = false;
 		dtsObj.fs = loader;
+		// dtsObj.setRotation(Math.PI / 2, 0, 0);
 
 		world = new MarbleWorld(s3d);
 
 		var db = new InteriorGeometry();
 		DifBuilder.loadDif("data/interiors/beginner/training_friction.dif", loader, db);
 		world.addInterior(db);
+		var tform = db.getTransform();
+		tform.setPosition(new Vector(0, 0, 7));
+		db.setTransform(tform);
 
-		var pi = new PathedInterior();
-		DifBuilder.loadDif("data/interiors/addon/smallplatform.dif", loader, pi);
-		var pim = pi.getTransform();
-		pim.setPosition(new Vector(5, 0, 0));
-		pi.setTransform(pim);
+		// var pi = new PathedInterior();
+		// DifBuilder.loadDif("data/interiors/addon/smallplatform.dif", loader, pi);
+		// var pim = pi.getTransform();
+		// pim.setPosition(new Vector(5, 0, 0));
+		// pi.setTransform(pim);
 
-		var cube = new Cube();
-		cube.addUVs();
-		cube.addNormals();
-		var mat = Material.create();
+		// var cube = new Cube();
+		// cube.addUVs();
+		// cube.addNormals();
+		// var mat = Material.create();
 
-		var m1 = new PathedInteriorMarker();
-		m1.msToNext = 5;
-		m1.position = new Vector(0, 0, 0);
-		m1.smoothingType = "";
-		m1.rotation = new Quat();
+		// var m1 = new PathedInteriorMarker();
+		// m1.msToNext = 5;
+		// m1.position = new Vector(0, 0, 0);
+		// m1.smoothingType = "";
+		// m1.rotation = new Quat();
 
-		var m2 = new PathedInteriorMarker();
-		m2.msToNext = 3;
-		m2.position = new Vector(0, 0, 5);
-		m2.smoothingType = "";
-		m2.rotation = new Quat();
+		// var m2 = new PathedInteriorMarker();
+		// m2.msToNext = 3;
+		// m2.position = new Vector(0, 0, 5);
+		// m2.smoothingType = "";
+		// m2.rotation = new Quat();
 
-		var m3 = new PathedInteriorMarker();
-		m3.msToNext = 5;
-		m3.position = new Vector(0, 0, 0);
-		m3.smoothingType = "";
-		m3.rotation = new Quat();
+		// var m3 = new PathedInteriorMarker();
+		// m3.msToNext = 5;
+		// m3.position = new Vector(0, 0, 0);
+		// m3.smoothingType = "";
+		// m3.rotation = new Quat();
 
-		pi.markerData = [m1, m2, m3];
+		// pi.markerData = [m1, m2, m3];
 
-		world.addPathedInterior(pi);
+		// world.addPathedInterior(pi);
 
 		world.addDtsObject(dtsObj);
 
@@ -103,11 +107,11 @@ class Main extends hxd.App {
 		var marble = new Marble();
 		marble.controllable = true;
 		world.addMarble(marble);
-		marble.setPosition(0, 0, 2);
+		marble.setPosition(5, 0, 5);
 
-		var marble2 = new Marble();
-		world.addMarble(marble2);
-		marble2.setPosition(5, 0, 5);
+		// var marble2 = new Marble();
+		// world.addMarble(marble2);
+		// marble2.setPosition(5, 0, 5);
 		// marble.setPosition(-10, -5, 5);
 	}
 

src/MarbleWorld.hx

@@ -44,6 +44,10 @@ class MarbleWorld {
 		this.dtsObjects.push(obj);
 		this.scene.addChild(obj);
 		obj.init();
+		for (collider in obj.colliders) {
+			if (collider != null)
+				this.collisionWorld.addEntity(collider);
+		}
 	}
 
 	public function addMarble(marble:Marble) {

src/collision/CollisionEntity.hx

@@ -76,6 +76,14 @@ class CollisionEntity implements IOctreeObject {
 		var tform = transform.clone();
 		tform.setPosition(tform.getPosition().add(this.velocity.multiply(dt)));
 
+		function toDifPoint(pt:Vector) {
+			return new Point3F(pt.x, pt.y, pt.z);
+		}
+
+		function fromDifPoint(pt:Point3F) {
+			return new Vector(pt.x, pt.y, pt.z);
+		}
+
 		var contacts = [];
 
 		for (obj in surfaces) {
@@ -91,8 +99,8 @@ class CollisionEntity implements IOctreeObject {
 
 				var res = Collision.IntersectTriangleSphere(v0, v, v2, surfacenormal, position, radius);
 				var closest = res.point;
-				// Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surface.normals[surface.indices[i]]);
-				if (res.result) {
+				// closest = Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surface.normals[surface.indices[i]]);
+				if (closest != null) {
 					if (position.sub(closest).lengthSq() < radius * radius) {
 						var normal = res.normal;
 
@@ -100,7 +108,7 @@ class CollisionEntity implements IOctreeObject {
 							normal.normalize();
 
 							var cinfo = new CollisionInfo();
-							cinfo.normal = res.normal; // surface.normals[surface.indices[i]];
+							cinfo.normal = normal; // surface.normals[surface.indices[i]];
 							cinfo.point = closest;
 							// cinfo.collider = this;
 							cinfo.velocity = this.velocity;

src/collision/CollisionHull.hx

@@ -0,0 +1,57 @@
+package collision;
+
+import h3d.col.Bounds;
+import collision.gjk.GJK;
+import collision.gjk.ConvexHull;
+import h3d.Vector;
+
+class CollisionHull extends CollisionEntity {
+	var hull:ConvexHull;
+
+	public function new() {
+		super();
+	}
+
+	public override function sphereIntersection(collisionEntity:SphereCollisionEntity, dt:Float):Array<CollisionInfo> {
+		var bbox = this.boundingBox;
+		var box = new Bounds();
+		var pos = collisionEntity.transform.getPosition();
+		box.xMin = pos.x - collisionEntity.radius;
+		box.yMin = pos.y - collisionEntity.radius;
+		box.zMin = pos.z - collisionEntity.radius;
+		box.xMax = pos.x + collisionEntity.radius;
+		box.yMax = pos.y + collisionEntity.radius;
+		box.zMax = pos.z + collisionEntity.radius;
+
+		if (bbox.collide(box)) {
+			var sph = new collision.gjk.Sphere();
+			sph.position = pos;
+			sph.radius = collisionEntity.radius;
+			var newTform = this.transform.clone();
+			var newpos = this.transform.getPosition().add(this.velocity.multiply(dt));
+			newTform.setPosition(newpos);
+			hull.setTransform(newTform);
+
+			var pt = GJK.gjk(sph, this.hull);
+			if (pt != null) {
+				var cinfo = new CollisionInfo();
+				cinfo.normal = pt.normalized();
+				cinfo.point = sph.position.sub(pt);
+				cinfo.velocity = velocity;
+				cinfo.contactDistance = sph.radius + pt.length();
+				cinfo.restitution = 1;
+				cinfo.friction = 1;
+				cinfo.force = 0;
+				return [cinfo];
+			}
+		}
+		return [];
+	}
+
+	public override function addSurface(surface:CollisionSurface) {
+		super.addSurface(surface);
+		if (hull == null)
+			hull = new ConvexHull([]);
+		hull.vertices = hull.vertices.concat(surface.points);
+	}
+}

src/collision/CollisionSurface.hx

@@ -24,6 +24,21 @@ class CollisionSurface implements IOctreeObject {
 		return 2;
 	}
 
+	public function generateNormals() {
+		var i = 0;
+		normals = [for (n in points) null];
+		while (i < indices.length) {
+			var p1 = points[indices[i]];
+			var p2 = points[indices[i + 1]];
+			var p3 = points[indices[i + 2]];
+			var n = p2.sub(p1).cross(p3.sub(p1)).normalized().multiply(-1);
+			normals[indices[i]] = n;
+			normals[indices[i + 1]] = n;
+			normals[indices[i + 2]] = n;
+			i += 3;
+		}
+	}
+
 	public function generateBoundingBox() {
 		var boundingBox = new Bounds();
 		boundingBox.xMin = 10e8;

src/collision/CollisionWorld.hx

@@ -44,9 +44,9 @@ class CollisionWorld {
 		box.xMin = center.x - radius;
 		box.yMin = center.y - radius;
 		box.zMin = center.z - radius;
-		box.xMax = center.x - radius;
-		box.yMax = center.y - radius;
-		box.zMax = center.z - radius;
+		box.xMax = center.x + radius;
+		box.yMax = center.y + radius;
+		box.zMax = center.z + radius;
 
 		var contacts:Array<CollisionEntity> = [];
 

src/collision/gjk/ConvexHull.hx

@@ -0,0 +1,58 @@
+package collision.gjk;
+
+import h3d.Vector;
+import h3d.Matrix;
+
+class ConvexHull {
+	public var vertices:Array<Vector>;
+
+	public var transform:Matrix;
+
+	public var centre(get, never):Vector;
+
+	var _centercache:Vector;
+
+	function get_centre():Vector {
+		if (_centercache != null)
+			return _centercache;
+		else {
+			var sum = new Vector();
+			for (v in vertices) {
+				sum = sum.add(v.transformed(this.transform));
+			}
+			sum = sum.multiply(1 / vertices.length);
+			_centercache = sum;
+			return _centercache;
+		}
+	}
+
+	public function new(vertices:Array<Vector>) {
+		this.transform = Matrix.I();
+		this.vertices = vertices;
+	}
+
+	public function setTransform(matrix:Matrix):Void {
+		if (this.transform != matrix) {
+			this.transform = matrix;
+			this._centercache = null;
+		}
+	}
+
+	public function support(direction:Vector):Vector {
+		var furthestDistance:Float = Math.NEGATIVE_INFINITY;
+		var furthestVertex:Vector = new Vector();
+
+		for (v in vertices) {
+			var v2 = v.transformed(transform);
+			var distance:Float = v2.dot(direction);
+			if (distance > furthestDistance) {
+				furthestDistance = distance;
+				furthestVertex.x = v2.x;
+				furthestVertex.y = v2.y;
+				furthestVertex.z = v2.z;
+			}
+		}
+
+		return furthestVertex;
+	}
+}

src/collision/gjk/GJK.hx

@@ -0,0 +1,215 @@
+package collision.gjk;
+
+import h3d.Vector;
+
+// Code taken from https://github.com/kevinmoran/GJK/blob/master/main.cpp
+class GJK {
+	public static var maxIterations = 64;
+	public static var maxEpaFaces = 64;
+	public static var epaTolerance = 0.0001;
+	public static var maxEpaLooseEdges = 64;
+	public static var maxEpaIterations = 64;
+
+	public static function gjk(sphere:Sphere, hull:ConvexHull) {
+		var searchDir = sphere.position.sub(hull.centre);
+		var a = new Vector();
+		var b = new Vector();
+		var c = new Vector();
+		var d = new Vector();
+
+		c = hull.support(searchDir).sub(sphere.support(searchDir.multiply(-1)));
+		searchDir = c.multiply(-1);
+		b = hull.support(searchDir).sub(sphere.support(searchDir.multiply(-1)));
+		if (b.dot(searchDir) < 0)
+			return null;
+
+		searchDir = c.sub(b).cross(b.multiply(-1)).cross(c.sub(b));
+		if (searchDir.length() == 0) {
+			searchDir = c.sub(b).cross(new Vector(1, 0, 0));
+			if (searchDir.length() == 0.00)
+				searchDir = c.sub(b).cross(new Vector(0, 0, -1));
+		}
+
+		var simpDim = 2;
+
+		for (i in 0...maxIterations) {
+			a = hull.support(searchDir).sub(sphere.support(searchDir.multiply(-1)));
+			if (a.dot(searchDir) < 0) {
+				return null;
+			}
+
+			simpDim++;
+			if (simpDim == 3) {
+				var n = b.sub(a).cross(c.sub(a));
+				var ao = a.multiply(-1);
+
+				simpDim = 2;
+				if (b.sub(a).cross(n).dot(ao) > 0) {
+					c = a;
+					searchDir = b.sub(a).cross(ao).cross(b.sub(a));
+				} else if (n.cross(c.sub(a)).dot(ao) > 0) {
+					b = a;
+					searchDir = c.sub(a).cross(ao).cross(c.sub(a));
+				} else {
+					simpDim = 3;
+					if (n.dot(ao) > 0) {
+						d = c;
+						c = b;
+						b = a;
+						searchDir = n;
+					} else {
+						d = b;
+						b = a;
+						searchDir = n.multiply(-1);
+					}
+				}
+			} else {
+				var abc = b.sub(a).cross(c.sub(a));
+				var acd = c.sub(a).cross(d.sub(a));
+				var adb = d.sub(a).cross(b.sub(a));
+				var ao = a.multiply(-1);
+				simpDim = 3;
+				if (abc.dot(ao) > 0) {
+					d = c;
+					c = b;
+					b = a;
+					searchDir = abc;
+				} else if (acd.dot(ao) > 0) {
+					b = a;
+					searchDir = acd;
+				} else if (adb.dot(ao) > 0) {
+					c = d;
+					d = b;
+					b = a;
+					searchDir = adb;
+				} else {
+					return epa(a, b, c, d, sphere, hull);
+				}
+			}
+		}
+		return null;
+	}
+
+	public static function epa(a:Vector, b:Vector, c:Vector, d:Vector, sphere:Sphere, hull:ConvexHull) {
+		var faces = [];
+		for (i in 0...maxEpaFaces)
+			faces.push([new Vector(), new Vector(), new Vector(), new Vector()]);
+
+		faces[0][0] = a;
+		faces[0][1] = b;
+		faces[0][2] = c;
+		faces[0][3] = b.sub(a).cross(c.sub(a)).normalized(); // ABC
+		faces[1][0] = a;
+		faces[1][1] = c;
+		faces[1][2] = d;
+		faces[1][3] = c.sub(a).cross(d.sub(a)).normalized();
+		faces[2][0] = a;
+		faces[2][1] = d;
+		faces[2][2] = b;
+		faces[2][3] = d.sub(a).cross(b.sub(a)).normalized();
+		faces[3][0] = b;
+		faces[3][1] = d;
+		faces[3][2] = c;
+		faces[3][3] = d.sub(b).cross(c.sub(b)).normalized();
+
+		var numFaces = 4;
+		var closestFace = 0;
+
+		for (iteration in 0...maxEpaIterations) {
+			// Find face that's closest to origin
+			var min_dist = faces[0][0].dot(faces[0][3]);
+			closestFace = 0;
+			for (i in 1...numFaces) {
+				var dist = faces[i][0].dot(faces[i][3]);
+				if (dist < min_dist) {
+					min_dist = dist;
+					closestFace = i;
+				}
+			}
+
+			// search normal to face that's closest to origin
+			var search_dir = faces[closestFace][3];
+			var p = hull.support(search_dir).sub(sphere.support(search_dir.multiply(-1)));
+			if (p.dot(search_dir) - min_dist < epaTolerance) {
+				// Convergence (new point is not significantly further from origin)
+				return faces[closestFace][3].multiply(p.dot(search_dir)); // dot vertex with normal to resolve collision along normal!
+			}
+			var loose_edges = [];
+			for (i in 0...maxEpaLooseEdges)
+				loose_edges.push([new Vector(), new Vector()]);
+
+			var num_loose_edges = 0;
+
+			// Find all triangles that are facing p
+			var i = 0;
+			while (i < numFaces) {
+				if (faces[i][3].dot(p.sub(faces[i][0])) > 0) // triangle i faces p, remove it
+				{
+					// Add removed triangle's edges to loose edge list.
+					// If it's already there, remove it (both triangles it belonged to are gone)
+					for (j in 0...3) // Three edges per face
+					{
+						var current_edge = [faces[i][j], faces[i][(j + 1) % 3]];
+						var found_edge = false;
+						for (k in 0...num_loose_edges) // Check if current edge is already in list
+						{
+							if (loose_edges[k][1] == current_edge[0] && loose_edges[k][0] == current_edge[1]) {
+								// Edge is already in the list, remove it
+								// THIS ASSUMES EDGE CAN ONLY BE SHARED BY 2 TRIANGLES (which should be true)
+								// THIS ALSO ASSUMES SHARED EDGE WILL BE REVERSED IN THE TRIANGLES (which
+								// should be true provided every triangle is wound CCW)
+								loose_edges[k][0] = loose_edges[num_loose_edges - 1][0]; // Overwrite current edge
+								loose_edges[k][1] = loose_edges[num_loose_edges - 1][1]; // with last edge in list
+								num_loose_edges--;
+								found_edge = true;
+								break;
+								// exit loop because edge can only be shared once
+							}
+						} // endfor loose_edges
+
+						if (!found_edge) { // add current edge to list
+							// assert(num_loose_edges<EPA_MAX_NUM_LOOSE_EDGES);
+							if (num_loose_edges >= maxEpaLooseEdges)
+								break;
+							loose_edges[num_loose_edges][0] = current_edge[0];
+							loose_edges[num_loose_edges][1] = current_edge[1];
+							num_loose_edges++;
+						}
+					}
+
+					// Remove triangle i from list
+					faces[i][0] = faces[numFaces - 1][0];
+					faces[i][1] = faces[numFaces - 1][1];
+					faces[i][2] = faces[numFaces - 1][2];
+					faces[i][3] = faces[numFaces - 1][3];
+					numFaces--;
+					i--;
+				} // endif p can see triangle i
+
+				i++;
+			} // endfor num_faces
+
+			// Reconstruct polytope with p added
+			for (i in 0...num_loose_edges) {
+				// assert(num_faces<EPA_MAX_NUM_FACES);
+				if (numFaces >= maxEpaFaces)
+					break;
+				faces[numFaces][0] = loose_edges[i][0];
+				faces[numFaces][1] = loose_edges[i][1];
+				faces[numFaces][2] = p;
+				faces[numFaces][3] = loose_edges[i][0].sub(loose_edges[i][1]).cross(loose_edges[i][0].sub(p)).normalized();
+
+				// Check for wrong normal to maintain CCW winding
+				var bias = 0.000001; // in case dot result is only slightly < 0 (because origin is on face)
+				if (faces[numFaces][0].dot(faces[numFaces][3]) + bias < 0) {
+					var temp = faces[numFaces][0];
+					faces[numFaces][0] = faces[numFaces][1];
+					faces[numFaces][1] = temp;
+					faces[numFaces][3] = faces[numFaces][3].multiply(-1);
+				}
+				numFaces++;
+			}
+		}
+		return faces[closestFace][3].multiply(faces[closestFace][0].dot(faces[closestFace][3]));
+	}
+}

src/collision/gjk/Sphere.hx

@@ -0,0 +1,19 @@
+package collision.gjk;
+
+import h3d.Vector;
+
+@:publicFields
+class Sphere {
+	var position:Vector;
+	var radius:Float;
+
+	public function new() {}
+
+	public function support(direction:Vector) {
+		var c = position.clone();
+		var d = direction.normalized();
+		d = d.multiply(radius);
+		c = c.add(d);
+		return c;
+	}
+}