speed up *some* collision detection

src/DifBuilder.hx

@@ -464,7 +464,7 @@ class DifBuilder {
 		}
 
 		collider.difEdgeMap = difEdges;
-		collider.generateBoundingBox();
+		collider.finalize();
 		itr.collider = collider;
 
 		function canFindTex(tex:String) {

src/Marble.hx

@@ -780,7 +780,7 @@ class Marble extends GameObject {
 				+ relLocalVel.z * deltaT * 2,
 				radius * 1.1);
 
-			var surfaces = obj.octree.boundingSearch(boundThing);
+			var surfaces = obj.grid == null ? obj.octree.boundingSearch(boundThing).map(x -> cast x) : obj.grid.boundingSearch(boundThing);
 
 			for (surf in surfaces) {
 				var surface:CollisionSurface = cast surf;
@@ -1065,7 +1065,7 @@ class Marble extends GameObject {
 			boundThing.addSpherePos(localpos.x + relLocalVel.x * dt * 2, localpos.y + relLocalVel.y * dt * 2, localpos.z + relLocalVel.z * dt * 2,
 				radius * 1.1);
 
-			var surfaces = obj.octree.boundingSearch(boundThing);
+			var surfaces = obj.grid == null ? obj.octree.boundingSearch(boundThing).map(x -> cast x) : obj.grid.boundingSearch(boundThing);
 
 			var tform = obj.transform.clone();
 

src/ProfilerUI.hx

@@ -15,27 +15,27 @@ class ProfilerUI {
 			return;
 
 		instance = this;
-		// debugProfiler = new h3d.impl.Benchmark(s2d);
-		// debugProfiler.y = 40;
+		debugProfiler = new h3d.impl.Benchmark(s2d);
+		debugProfiler.y = 40;
 
-		// fpsCounter = new Text(DefaultFont.get(), s2d);
-		// fpsCounter.y = 80;
-		// fpsCounter.color = new Vector(1, 1, 1, 1);
+		fpsCounter = new Text(DefaultFont.get(), s2d);
+		fpsCounter.y = 80;
+		fpsCounter.color = new Vector(1, 1, 1, 1);
 	}
 
 	public static function begin() {
-		// instance.debugProfiler.begin();
+		instance.debugProfiler.begin();
 	}
 
 	public static function measure(name:String) {
-		// instance.debugProfiler.measure(name);
+		instance.debugProfiler.measure(name);
 	}
 
 	public static function end() {
-		// instance.debugProfiler.end();
+		instance.debugProfiler.end();
 	}
 
 	public static function update(fps:Float) {
-		// instance.fpsCounter.text = "FPS: " + fps;
+		instance.fpsCounter.text = "FPS: " + fps;
 	}
 }

src/collision/CollisionEntity.hx

@@ -18,6 +18,8 @@ class CollisionEntity implements IOctreeObject {
 
 	public var octree:Octree;
 
+	public var grid:Grid;
+
 	public var surfaces:Array<CollisionSurface>;
 
 	public var priority:Int;
@@ -49,6 +51,15 @@ class CollisionEntity implements IOctreeObject {
 		}
 	}
 
+	// Generates the grid
+	public function finalize() {
+		this.generateBoundingBox();
+		this.grid = new Grid(this.boundingBox);
+		for (surface in this.surfaces) {
+			this.grid.insert(surface);
+		}
+	}
+
 	public function setTransform(transform:Matrix) {
 		if (this.transform == transform)
 			return;
@@ -72,15 +83,26 @@ class CollisionEntity implements IOctreeObject {
 		var rStart = rayOrigin.clone();
 		rStart.transform(invMatrix);
 		var rDir = rayDirection.transformed3x3(invMatrix);
-		var intersections = octree.raycast(rStart, rDir);
-		var iData:Array<RayIntersectionData> = [];
-		for (i in intersections) {
-			i.point.transform(transform);
-			i.normal.transform3x3(transform);
-			i.normal.normalize();
-			iData.push({point: i.point, normal: i.normal, object: i.object});
+		if (grid == null) {
+			var intersections = octree.raycast(rStart, rDir);
+			var iData:Array<RayIntersectionData> = [];
+			for (i in intersections) {
+				i.point.transform(transform);
+				i.normal.transform3x3(transform);
+				i.normal.normalize();
+				iData.push({point: i.point, normal: i.normal, object: i.object});
+			}
+			return iData;
+		} else {
+			var intersections = this.grid.rayCast(rStart, rDir);
+			for (i in intersections) {
+				i.point.transform(transform);
+				i.normal.transform3x3(transform);
+				i.normal.normalize();
+			}
+
+			return intersections;
 		}
-		return iData;
 	}
 
 	public function getElementType() {
@@ -105,7 +127,7 @@ class CollisionEntity implements IOctreeObject {
 		sphereBounds.addSpherePos(position.x, position.y, position.z, radius * 1.1);
 		sphereBounds.transform(invMatrix);
 		sphereBounds.addSpherePos(localPos.x, localPos.y, localPos.z, radius * 1.1);
-		var surfaces = octree.boundingSearch(sphereBounds);
+		var surfaces = grid == null ? octree.boundingSearch(sphereBounds).map(x -> cast x) : grid.boundingSearch(sphereBounds);
 
 		var tform = transform.clone();
 		// tform.setPosition(tform.getPosition().add(this.velocity.multiply(timeState.dt)));

src/collision/CollisionSurface.hx

@@ -26,6 +26,8 @@ class CollisionSurface implements IOctreeObject {
 
 	public var originalSurfaceIndex:Int;
 
+	public var key:Bool = false;
+
 	public function new() {}
 
 	public function getElementType() {

src/collision/Grid.hx

@@ -0,0 +1,214 @@
+package collision;
+
+import haxe.Exception;
+import h3d.Vector;
+import h3d.col.Bounds;
+
+class Grid {
+	public var bounds:Bounds; // The bounds of the grid
+
+	public var cellSize:Vector; // The dimensions of one cell
+
+	public static var CELL_DIV = new Vector(16, 16, 16); // split the bounds into cells of dimensions 1/16th of the corresponding dimensions of the bounds
+
+	var map:Map<Int, Array<Int>> = new Map();
+
+	var surfaces:Array<CollisionSurface> = [];
+
+	public function new(bounds:Bounds) {
+		this.bounds = bounds;
+
+		this.cellSize = new Vector(bounds.xSize / CELL_DIV.x, bounds.ySize / CELL_DIV.y, bounds.zSize / CELL_DIV.z);
+	}
+
+	public function insert(surface:CollisionSurface) {
+		// Assuming surface has built a bounding box already
+		if (this.bounds.containsBounds(surface.boundingBox)) {
+			var idx = this.surfaces.length;
+			this.surfaces.push(surface);
+
+			var xStart = Math.floor((surface.boundingBox.xMin - bounds.xMin) / this.cellSize.x);
+			var yStart = Math.floor((surface.boundingBox.yMin - bounds.yMin) / this.cellSize.y);
+			var zStart = Math.floor((surface.boundingBox.zMin - bounds.zMin) / this.cellSize.z);
+			var xEnd = Math.ceil((surface.boundingBox.xMax - bounds.xMin) / this.cellSize.x) + 1;
+			var yEnd = Math.ceil((surface.boundingBox.yMax - bounds.yMin) / this.cellSize.y) + 1;
+			var zEnd = Math.ceil((surface.boundingBox.zMax - bounds.zMin) / this.cellSize.z) + 1;
+
+			// Insert the surface references from [xStart, yStart, zStart] to [xEnd, yEnd, zEnd] into the map
+			for (i in xStart...xEnd) {
+				for (j in yStart...yEnd) {
+					for (k in zStart...zEnd) {
+						var hash = hashVector(i, j, k);
+						if (!this.map.exists(hash)) {
+							this.map.set(hash, []);
+						}
+						this.map.get(hash).push(idx);
+					}
+				}
+			}
+		} else {
+			throw new Exception("Surface is not contained in the grid's bounds");
+		}
+	}
+
+	// searchbox should be in LOCAL coordinates
+	public function boundingSearch(searchbox:Bounds) {
+		var xStart = Math.floor((searchbox.xMin - bounds.xMin) / this.cellSize.x);
+		var yStart = Math.floor((searchbox.yMin - bounds.yMin) / this.cellSize.y);
+		var zStart = Math.floor((searchbox.zMin - bounds.zMin) / this.cellSize.z);
+		var xEnd = Math.ceil((searchbox.xMax - bounds.xMin) / this.cellSize.x) + 1;
+		var yEnd = Math.ceil((searchbox.yMax - bounds.yMin) / this.cellSize.y) + 1;
+		var zEnd = Math.ceil((searchbox.zMax - bounds.zMin) / this.cellSize.z) + 1;
+
+		var foundSurfaces = [];
+
+		for (surf in this.surfaces) {
+			surf.key = false;
+		}
+
+		// Insert the surface references from [xStart, yStart, zStart] to [xEnd, yEnd, zEnd] into the map
+		for (i in xStart...xEnd) {
+			for (j in yStart...yEnd) {
+				for (k in zStart...zEnd) {
+					var hash = hashVector(i, j, k);
+					if (this.map.exists(hash)) {
+						var surfs = this.map.get(hash);
+						var actualsurfs = surfs.map(x -> this.surfaces[x]);
+						for (surf in actualsurfs) {
+							if (surf.key)
+								continue;
+							if (searchbox.containsBounds(surf.boundingBox) || searchbox.collide(surf.boundingBox)) {
+								foundSurfaces.push(surf);
+								surf.key = true;
+							}
+						}
+					}
+				}
+			}
+		}
+
+		return foundSurfaces;
+	}
+
+	function elegantPair(x:Int, y:Int) {
+		return (x >= y) ? (x * x + x + y) : (y * y + x);
+	}
+
+	function hashVector(x:Int, y:Int, z:Int) {
+		return elegantPair(elegantPair(x, y), z);
+	}
+
+	public function rayCast(origin:Vector, direction:Vector) {
+		var cell = origin.sub(this.bounds.getMin().toVector());
+		cell.x /= this.cellSize.x;
+		cell.y /= this.cellSize.y;
+		cell.z /= this.cellSize.z;
+
+		var stepX, outX, X = Math.floor(cell.x);
+		var stepY, outY, Y = Math.floor(cell.y);
+		var stepZ, outZ, Z = Math.floor(cell.z);
+
+		if ((X < 0) || (X >= CELL_DIV.x) || (Y < 0) || (Y >= CELL_DIV.y) || (Z < 0) || (Z >= CELL_DIV.z))
+			return [];
+
+		var cb = new Vector();
+
+		if (direction.x > 0) {
+			stepX = 1;
+			outX = CELL_DIV.x;
+			cb.x = this.bounds.getMin().x + (X + 1) * this.cellSize.x;
+		} else {
+			stepX = -1;
+			outX = -1;
+			cb.x = this.bounds.getMin().x + X * this.cellSize.x;
+		}
+		if (direction.y > 0.0) {
+			stepY = 1;
+			outY = CELL_DIV.y;
+			cb.y = this.bounds.getMin().y + (Y + 1) * this.cellSize.y;
+		} else {
+			stepY = -1;
+			outY = -1;
+			cb.y = this.bounds.getMin().y + Y * this.cellSize.y;
+		}
+		if (direction.z > 0.0) {
+			stepZ = 1;
+			outZ = CELL_DIV.z;
+			cb.z = this.bounds.getMin().z + (Z + 1) * this.cellSize.z;
+		} else {
+			stepZ = -1;
+			outZ = -1;
+			cb.z = this.bounds.getMin().z + Z * this.cellSize.z;
+		}
+
+		var tmax = new Vector();
+		var tdelta = new Vector();
+
+		var rxr, ryr, rzr;
+		if (direction.x != 0) {
+			rxr = 1.0 / direction.x;
+			tmax.x = (cb.x - origin.x) * rxr;
+			tdelta.x = this.cellSize.x * stepX * rxr;
+		} else
+			tmax.x = 1000000;
+		if (direction.y != 0) {
+			ryr = 1.0 / direction.y;
+			tmax.y = (cb.y - origin.y) * ryr;
+			tdelta.y = this.cellSize.y * stepY * ryr;
+		} else
+			tmax.y = 1000000;
+		if (direction.z != 0) {
+			rzr = 1.0 / direction.z;
+			tmax.z = (cb.z - origin.z) * rzr;
+			tdelta.z = this.cellSize.z * stepZ * rzr;
+		} else
+			tmax.z = 1000000;
+
+		for (surf in this.surfaces) {
+			surf.key = false;
+		}
+
+		var results = [];
+
+		while (true) {
+			var hash = hashVector(X, Y, Z);
+			if (this.map.exists(hash)) {
+				var currentSurfaces = this.map.get(hash).map(x -> this.surfaces[x]);
+
+				for (surf in currentSurfaces) {
+					if (surf.key)
+						continue;
+					results = results.concat(surf.rayCast(origin, direction));
+					surf.key = true;
+				}
+			}
+			if (tmax.x < tmax.y) {
+				if (tmax.x < tmax.z) {
+					X = X + stepX;
+					if (X == outX)
+						break;
+					tmax.x += tdelta.x;
+				} else {
+					Z = Z + stepZ;
+					if (Z == outZ)
+						break;
+					tmax.z += tdelta.z;
+				}
+			} else {
+				if (tmax.y < tmax.z) {
+					Y = Y + stepY;
+					if (Y == outY)
+						break;
+					tmax.y += tdelta.y;
+				} else {
+					Z = Z + stepZ;
+					if (Z == outZ)
+						break;
+					tmax.z += tdelta.z;
+				}
+			}
+		}
+
+		return results;
+	}
+}