sap test

src/collision/CollisionWorld.hx

@@ -8,12 +8,20 @@ import h3d.col.Sphere;
 import h3d.Vector;
 import octree.Octree;
 
+@:structInit
+@:publicFields
+class SphereIntersectionResult {
+	var foundEntities:Array<CollisionEntity>;
+	var contacts:Array<CollisionInfo>;
+}
+
 class CollisionWorld {
 	public var staticWorld:CollisionEntity;
 	public var octree:Octree;
 	public var entities:Array<CollisionEntity> = [];
 	public var dynamicEntities:Array<CollisionEntity> = [];
 	public var dynamicOctree:Octree;
+	public var sap:SAP;
 
 	public var marbleEntities:Array<SphereCollisionEntity> = [];
 
@@ -23,9 +31,10 @@ class CollisionWorld {
 		this.octree = new Octree();
 		this.dynamicOctree = new Octree();
 		this.staticWorld = new CollisionEntity(null);
+		this.sap = new SAP();
 	}
 
-	public function sphereIntersection(spherecollision:SphereCollisionEntity, timeState:TimeState) {
+	public function sphereIntersection(spherecollision:SphereCollisionEntity, timeState:TimeState):SphereIntersectionResult {
 		var position = spherecollision.transform.getPosition();
 		var radius = spherecollision.radius;
 		// var velocity = spherecollision.velocity;
@@ -37,42 +46,50 @@ class CollisionWorld {
 		box.transform(rotQuat.toMatrix());
 		box.offset(position.x, position.y, position.z);
 		// box.addSpherePos(position.x + velocity.x * timeState.dt, position.y + velocity.y * timeState.dt, position.z + velocity.z * timeState.dt, radius);
-		var intersections = this.octree.boundingSearch(box);
+		// var intersections = this.octree.boundingSearch(box);
 
 		// var intersections = this.rtree.search([box.xMin, box.yMax, box.zMin], [box.xSize, box.ySize, box.zSize]);
 
 		var contacts = [];
 		var foundEntities = [];
 
-		for (obj in intersections) {
+		// for (obj in intersections) {
-			var entity:CollisionEntity = cast obj;
+		// 	var entity:CollisionEntity = cast obj;
 
-			foundEntities.push(entity);
+		// 	foundEntities.push(entity);
-			if (entity.go.isCollideable) {
+		// 	if (entity.go.isCollideable) {
-				contacts = contacts.concat(entity.sphereIntersection(spherecollision, timeState));
+		// 		contacts = contacts.concat(entity.sphereIntersection(spherecollision, timeState));
+		// 	}
+		// }
+
+		sap.recompute();
+		var sapCollisions = sap.getIntersections(spherecollision);
+		for (obj in sapCollisions) {
+			if (obj.boundingBox.collide(box) && obj.go.isCollideable) {
+				contacts = contacts.concat(obj.sphereIntersection(spherecollision, timeState));
 			}
 		}
 
 		// contacts = contacts.concat(this.staticWorld.sphereIntersection(spherecollision, timeState));
 
-		var dynSearch = dynamicOctree.boundingSearch(box).map(x -> cast(x, CollisionEntity));
+		// var dynSearch = dynamicOctree.boundingSearch(box).map(x -> cast(x, CollisionEntity));
-		for (obj in dynSearch) {
+		// for (obj in dynSearch) {
-			if (obj != spherecollision) {
+		// 	if (obj != spherecollision) {
-				if (obj.boundingBox.collide(box) && obj.go.isCollideable)
+		// 		if (obj.boundingBox.collide(box) && obj.go.isCollideable)
-					contacts = contacts.concat(obj.sphereIntersection(spherecollision, timeState));
+		// 			contacts = contacts.concat(obj.sphereIntersection(spherecollision, timeState));
-			}
+		// 	}
-		}
+		// }
 
-		for (marb in marbleEntities) {
+		// for (marb in marbleEntities) {
-			if (marb != spherecollision) {
+		// 	if (marb != spherecollision) {
-				if (spherecollision.go.isCollideable) {
+		// 		if (spherecollision.go.isCollideable) {
-					var isecs = marb.sphereIntersection(spherecollision, timeState);
+		// 			var isecs = marb.sphereIntersection(spherecollision, timeState);
-					if (isecs.length > 0)
+		// 			if (isecs.length > 0)
-						foundEntities.push(marb);
+		// 				foundEntities.push(marb);
-					contacts = contacts.concat(isecs);
+		// 			contacts = contacts.concat(isecs);
-				}
+		// 		}
-			}
+		// 	}
-		}
+		// }
 		return {foundEntities: foundEntities, contacts: contacts};
 	}
 
@@ -128,6 +145,7 @@ class CollisionWorld {
 	public function addEntity(entity:CollisionEntity) {
 		this.octree.insert(entity);
 		this.entities.push(entity);
+		this.sap.addEntity(entity);
 
 		// this.rtree.insert([entity.boundingBox.xMin, entity.boundingBox.yMin, entity.boundingBox.zMin],
 		// 	[entity.boundingBox.xSize, entity.boundingBox.ySize, entity.boundingBox.zSize], entity);
@@ -145,6 +163,7 @@ class CollisionWorld {
 		this.dynamicEntities.push(entity);
 		this.dynamicOctree.insert(entity);
 		this.dynamicEntitySet.set(entity, true);
+		this.sap.addEntity(entity);
 	}
 
 	public function removeMovingEntity(entity:CollisionEntity) {
@@ -152,6 +171,7 @@ class CollisionWorld {
 	}
 
 	public function updateTransform(entity:CollisionEntity) {
+		this.sap.update(entity);
 		if (!dynamicEntitySet.exists(entity)) {
 			this.octree.update(entity);
 		} else {

src/collision/SAP.hx

@@ -0,0 +1,168 @@
+package collision;
+
+@:structInit
+@:publicFields
+class SAPProxy {
+	var object:CollisionEntity;
+	var flags:Int;
+	var intersections:Array<CollisionEntity>;
+	var positions:Array<Int>;
+}
+
+class SAP {
+	var dimXEdges:Array<Float> = [];
+	var dimXEdgeLefts:Array<Bool> = [];
+	var dimXEdgeOwner:Array<Int> = [];
+	var dimYEdges:Array<Float> = [];
+	var dimYEdgeLefts:Array<Bool> = [];
+	var dimYEdgeOwner:Array<Int> = [];
+	var dimZEdges:Array<Float> = [];
+	var dimZEdgeLefts:Array<Bool> = [];
+	var dimZEdgeOwner:Array<Int> = [];
+
+	var objects:Array<SAPProxy> = [];
+	var objToProxy:Map<CollisionEntity, SAPProxy> = [];
+	var needsSort = true;
+
+	public function new() {}
+
+	public function addEntity(obj:CollisionEntity) {
+		needsSort = true;
+		var edgeLen = dimXEdges.length;
+		var proxy:SAPProxy = {
+			object: obj,
+			intersections: [],
+			flags: 0,
+			positions: [edgeLen, edgeLen + 1, edgeLen, edgeLen + 1, edgeLen, edgeLen + 1]
+		};
+		var idx = objects.length;
+		objects.push(proxy);
+		dimXEdges.push(obj.boundingBox.xMin);
+		dimXEdges.push(obj.boundingBox.xMax);
+		dimYEdges.push(obj.boundingBox.yMin);
+		dimYEdges.push(obj.boundingBox.yMax);
+		dimZEdges.push(obj.boundingBox.zMin);
+		dimZEdges.push(obj.boundingBox.zMax);
+		dimXEdgeLefts.push(true);
+		dimXEdgeLefts.push(false);
+		dimYEdgeLefts.push(true);
+		dimYEdgeLefts.push(false);
+		dimZEdgeLefts.push(true);
+		dimZEdgeLefts.push(false);
+		dimXEdgeOwner.push(idx);
+		dimXEdgeOwner.push(idx);
+		dimYEdgeOwner.push(idx);
+		dimYEdgeOwner.push(idx);
+		dimZEdgeOwner.push(idx);
+		dimZEdgeOwner.push(idx);
+
+		objToProxy.set(obj, proxy);
+	}
+
+	public function update(obj:CollisionEntity) {
+		if (!objToProxy.exists(obj))
+			addEntity(obj);
+		needsSort = true;
+		var proxy = objToProxy.get(obj);
+		proxy.object = obj;
+		proxy.intersections = [];
+		proxy.flags = 0;
+		dimXEdges[proxy.positions[0]] = obj.boundingBox.xMin;
+		dimXEdges[proxy.positions[1]] = obj.boundingBox.xMax;
+		dimYEdges[proxy.positions[2]] = obj.boundingBox.yMin;
+		dimYEdges[proxy.positions[3]] = obj.boundingBox.yMax;
+		dimZEdges[proxy.positions[4]] = obj.boundingBox.zMin;
+		dimZEdges[proxy.positions[5]] = obj.boundingBox.zMax;
+	}
+
+	public function sort(dim:Int) {
+		var edges;
+		var edgeLefts;
+		var edgeOwner;
+		if (dim == 0) {
+			edges = dimXEdges;
+			edgeLefts = this.dimXEdgeLefts;
+			edgeOwner = this.dimXEdgeOwner;
+		} else if (dim == 1) {
+			edges = dimYEdges;
+			edgeLefts = this.dimYEdgeLefts;
+			edgeOwner = this.dimYEdgeOwner;
+		} else {
+			edges = dimZEdges;
+			edgeLefts = this.dimZEdgeLefts;
+			edgeOwner = this.dimZEdgeOwner;
+		}
+
+		for (i in 0...edges.length) {
+			var j = i - 1;
+			while (j >= 0) {
+				if (edges[j] < edges[j + 1])
+					break;
+
+				// Swap
+
+				var edge1Owner = objects[edgeOwner[j]];
+				var edge2Owner = objects[edgeOwner[j + 1]];
+				var edge1Left = edgeLefts[j];
+				var edge2Left = edgeLefts[j + 1];
+
+				edge1Owner.positions[2 * dim + (edge1Left ? 1 : 0)] = j + 1;
+				edge2Owner.positions[2 * dim + (edge2Left ? 1 : 0)] = j;
+
+				var tmp = edges[j];
+				edges[j] = edges[j + 1];
+				edges[j + 1] = tmp;
+
+				var tmp2 = edgeLefts[j];
+				edgeLefts[j] = edgeLefts[j + 1];
+				edgeLefts[j + 1] = tmp2;
+
+				var tmp3 = edgeOwner[j];
+				edgeOwner[j] = edgeOwner[j + 1];
+				edgeOwner[j + 1] = tmp3;
+
+				// Sweep
+				var edge1 = j;
+				var edge2 = j + 1;
+				if (edgeLefts[edge1] && !edgeLefts[edge2]) {
+					var obj1 = edgeOwner[edge1];
+					var obj2 = edgeOwner[edge2];
+					objects[obj1].flags |= (1 << dim);
+					objects[obj2].flags |= (1 << dim);
+
+					if (objects[obj1].flags == 7 && objects[obj2].flags == 7) {
+						objects[obj1].intersections.push(objects[obj2].object);
+						objects[obj2].intersections.push(objects[obj1].object);
+					}
+				} else if (!edgeLefts[edge1] && edgeLefts[edge2]) {
+					var obj1 = edgeOwner[edge2];
+					var obj2 = edgeOwner[edge1];
+					if (objects[obj1].flags == 7) {
+						objects[obj1].intersections.remove(objects[obj2].object);
+					}
+					if (objects[obj2].flags == 7) {
+						objects[obj2].intersections.remove(objects[obj1].object);
+					}
+
+					objects[obj1].flags &= ~(1 << dim);
+					objects[obj2].flags &= ~(1 << dim);
+				}
+
+				j--;
+			}
+		}
+	}
+
+	public function recompute() {
+		if (needsSort) {
+			this.sort(0);
+			this.sort(1);
+			this.sort(2);
+			needsSort = false;
+		}
+	}
+
+	public function getIntersections(obj:CollisionEntity):Array<CollisionEntity> {
+		return objToProxy[obj].intersections;
+	}
+}