fix minor bugs and improve moving platform broadphase

src/Marble.hx

@@ -1307,7 +1307,7 @@ class Marble extends GameObject {
 				var tempTimeState = timeState.clone();
 				tempState.currentAttemptTime = piTime;
 				tempState.dt = timeStep;
-				this.level.callCollisionHandlers(cast this, tempTimeState);
+				this.level.callCollisionHandlers(cast this, tempTimeState, pos, newPos, rot, quat);
 			}
 
 			if (contacts.length != 0)

src/MarbleWorld.hx

@@ -1063,6 +1063,8 @@ class MarbleWorld extends Scheduler {
 					val = Util.getKeyForButton(Settings.controlsSettings.powerup);
 				if (funcdata[1] == "freelook")
 					val = Util.getKeyForButton(Settings.controlsSettings.freelook);
+				if (funcdata[1] == "useblast")
+					val = Util.getKeyForButton(Settings.controlsSettings.blast);
 			}
 			start = val.length + pos;
 			text = pre + val + post;
@@ -1106,13 +1108,18 @@ class MarbleWorld extends Scheduler {
 		this.playGui.formatGemCounter(this.gemCount, this.totalGems);
 	}
 
-	public function callCollisionHandlers(marble:Marble, timeState:TimeState) {
+	public function callCollisionHandlers(marble:Marble, timeState:TimeState, start:Vector, end:Vector, startQuat:Quat, endQuat:Quat) {
-		var gjkSphere = new collision.gjk.Sphere();
+		var expansion = marble._radius + 0.2;
-		gjkSphere.position = marble.getAbsPos().getPosition();
+		var minP = new Vector(Math.min(start.x, end.x) - expansion, Math.min(start.y, end.y) - expansion, Math.min(start.z, end.z) - expansion);
-		gjkSphere.radius = marble._radius;
+		var maxP = new Vector(Math.max(start.x, end.x) + expansion, Math.max(start.y, end.y) + expansion, Math.max(start.z, end.z) + expansion);
+		var box = Bounds.fromPoints(minP.toPoint(), maxP.toPoint());
+
+		var marbleHitbox = new Bounds();
+		marbleHitbox.addSpherePos(0, 0, 0, marble._radius);
+		marbleHitbox.transform(startQuat.toMatrix());
+		marbleHitbox.transform(endQuat.toMatrix());
+		marbleHitbox.offset(end.x, end.y, end.z);
 
-		var spherebounds = new Bounds();
-		spherebounds.addSpherePos(gjkSphere.position.x, gjkSphere.position.y, gjkSphere.position.z, gjkSphere.radius);
 		// spherebounds.addSpherePos(gjkCapsule.p2.x, gjkCapsule.p2.y, gjkCapsule.p2.z, gjkCapsule.radius);
 		// var contacts = this.collisionWorld.radiusSearch(marble.getAbsPos().getPosition(), marble._radius);
 		var contacts = marble.contactEntities;
@@ -1123,6 +1130,7 @@ class MarbleWorld extends Scheduler {
 				if (contact.go is DtsObject) {
 					var shape:DtsObject = cast contact.go;
 
+					if (contact.boundingBox.collide(marbleHitbox)) {
 						shape.onMarbleInside(timeState);
 						if (!this.shapeOrTriggerInside.contains(contact.go)) {
 							this.shapeOrTriggerInside.push(contact.go);
@@ -1130,11 +1138,12 @@ class MarbleWorld extends Scheduler {
 						}
 						inside.push(contact.go);
 					}
+				}
 				if (contact.go is Trigger) {
 					var trigger:Trigger = cast contact.go;
 					var triggeraabb = trigger.collider.boundingBox;
 
-					if (triggeraabb.collide(spherebounds)) {
+					if (triggeraabb.collide(marbleHitbox)) {
 						trigger.onMarbleInside(timeState);
 						if (!this.shapeOrTriggerInside.contains(contact.go)) {
 							this.shapeOrTriggerInside.push(contact.go);
@@ -1154,11 +1163,11 @@ class MarbleWorld extends Scheduler {
 		}
 
 		if (this.finishTime == null) {
-			if (spherebounds.collide(this.endPad.finishBounds)) {
+			if (marbleHitbox.collide(this.endPad.finishBounds)) {
 				var padUp = this.endPad.getAbsPos().up();
 				padUp = padUp.multiply(10);
 
-				var checkBounds = spherebounds.clone();
+				var checkBounds = box.clone();
 				checkBounds.zMin -= 10;
 				checkBounds.zMax += 10;
 				var checkBoundsCenter = checkBounds.getCenter();

src/collision/BVHTree.hx

@@ -1,55 +1,42 @@
 package collision;
 
 import h3d.col.Bounds;
-import h3d.Vector;
 
-// https://github.com/Sopiro/DynamicBVH/blob/master/src/aabbtree.ts
+interface IBVHObject {
+	var boundingBox:Bounds;
+	function rayCast(rayOrigin:Vector, rayDirection:Vector):Array<octree.IOctreeObject.RayIntersectionData>;
+}
 
 @:publicFields
-class BVHNode {
+class BVHNode<T:IBVHObject> {
+	var id:Int;
+	var parent:BVHNode<T>;
+	var child1:BVHNode<T>;
+	var child2:BVHNode<T>;
+	var isLeaf:Bool;
 	var bounds:Bounds;
-	var objects:Array<CollisionSurface>;
+	var object:T;
-	var objectBounds:Bounds; // total bounds for objects stored in THIS node
-	var left:BVHNode;
-	var right:BVHNode;
-	var surfaceArea:Float;
 
 	public function new(bounds:Bounds) {
 		this.bounds = bounds.clone();
 		surfaceArea = this.bounds.xSize * this.bounds.ySize + this.bounds.xSize * this.bounds.zSize + this.bounds.ySize * this.bounds.zSize;
 	}
 
-	function getSplitCost(objs:Array<{obj:CollisionSurface, centroid:h3d.col.Point}>, axis:Int) {
+	class BVHTree<T:IBVHObject> {
-		// Pick best axis to split
+		var nodeId:Int = 0;
-		switch (axis) {
+		var root:BVHNode<T>;
-			case 0:
-				objs.sort((x, y) -> x.centroid.x > y.centroid.x ? 1 : -1);
-			case 1:
-				objs.sort((x, y) -> x.centroid.y > y.centroid.y ? 1 : -1);
-			case 2:
-				objs.sort((x, y) -> x.centroid.z > y.centroid.z ? 1 : -1);
-		};
 
-		var leftObjects = objs.slice(0, Math.ceil(objs.length / 2));
+		public function new() {}
-		var rightObjects = objs.slice(Math.ceil(objs.length / 2));
+
-		var leftAABB = new Bounds();
+		function update() {
-		var rightAABB = new Bounds();
+			var invalidNodes = [];
-		for (o in leftObjects)
+			this.traverse(node -> {
-			leftAABB.add(o.obj.boundingBox);
+				if (node.isLeaf) {
-		for (o in rightObjects)
+					var entity = node.object;
-			rightAABB.add(o.obj.boundingBox);
+					var tightAABB = entity.boundingBox;
-		var leftSA = leftAABB.xSize * leftAABB.ySize + leftAABB.xSize * leftAABB.zSize + leftAABB.ySize * leftAABB.zSize;
+
-		var rightSA = rightAABB.xSize * rightAABB.ySize + rightAABB.xSize * rightAABB.zSize + rightAABB.ySize * rightAABB.zSize;
+					if (node.bounds.containsBounds(tightAABB)) {
-		var splitCost = leftSA + rightSA;
+						return;
-		var bestSplit = {
-			cost: splitCost,
-			left: leftObjects,
-			right: rightObjects,
-			leftBounds: leftAABB,
-			rightBounds: rightAABB,
-			axis: axis
-		};
-		return bestSplit;
 					}
 
 					public function split() {
@@ -59,36 +46,24 @@ class BVHNode {
 							x.generateBoundingBox();
 							return {obj: x, centroid: x.boundingBox.getCenter()};
 						});
-
+						for (node in invalidNodes) {
-		// Find the best split cost
+							this.remove(node);
-		var costs = [getSplitCost(objs, 0), getSplitCost(objs, 1), getSplitCost(objs, 2)];
+							this.add(node.object);
-		costs.sort((x, y) -> x.cost > y.cost ? 1 : -1);
-		var bestSplit = costs[0];
-
-		// Sort the objects according to where they should go
-		var leftObjs = [];
-		var rightObjs = [];
-		var intersectObjs = [];
-		for (o in bestSplit.left.concat(bestSplit.right)) {
-			var inleft = bestSplit.leftBounds.containsBounds(o.obj.boundingBox);
-			var inright = bestSplit.rightBounds.containsBounds(o.obj.boundingBox);
-			if (inleft && inright) {
-				intersectObjs.push(o.obj);
-			} else if (inleft) {
-				leftObjs.push(o.obj);
-			} else if (inright) {
-				rightObjs.push(o.obj);
-			}
 						}
 
-		// Only one side has objects, egh
+						public function add(entity:T) {
-		if (leftObjs.length == 0 || rightObjs.length == 0) {
+							// Enlarged AABB
-			var thisobjs = leftObjs.concat(rightObjs).concat(intersectObjs);
+							var aabb = entity.boundingBox;
-			this.objects = thisobjs;
+
-			this.objectBounds = new Bounds();
+							var newNode = new BVHNode();
-			for (o in thisobjs)
+							newNode.id = this.nodeId++;
-				this.objectBounds.add(o.boundingBox);
+							newNode.bounds = aabb;
-			return;
+							newNode.object = entity;
+							newNode.isLeaf = true;
+
+							if (this.root == null) {
+								this.root = newNode;
+								return newNode;
 							}
 
 							// Make the child nodes
@@ -130,18 +105,44 @@ class BVHNode {
 							}
 						}
 
-	public function rayCast(origin:Vector, direction:Vector) {
+						function reset() {
-		var ray = h3d.col.Ray.fromValues(origin.x, origin.y, origin.z, direction.x, direction.y, direction.z);
+							this.nodeId = 0;
-		if (ray.collide(this.bounds)) {
+							this.root = null;
-			var intersects = [];
-			if (this.left != null && this.right != null) {
-				intersects = intersects.concat(this.left.rayCast(origin, direction));
-				intersects = intersects.concat(this.right.rayCast(origin, direction));
 						}
-			if (ray.collide(this.objectBounds)) {
+
-				for (o in this.objects) {
+						// BFS tree traversal
-					if (ray.collide(o.boundingBox))
+						function traverse(callback:(node:BVHNode<T>) -> Void) {
-						intersects = intersects.concat(o.rayCast(origin, direction));
+							var q = [this.root];
+
+							while (q.length != 0) {
+								var current = q.shift();
+								if (current == null) {
+									break;
+								}
+
+								callback(current);
+
+								if (!current.isLeaf) {
+									if (current.child1 != null)
+										q.push(current.child1);
+									if (current.child2 != null)
+										q.push(current.child2);
+								}
+							}
+						}
+
+						public function remove(node:BVHNode<T>) {
+							var parent = node.parent;
+
+							if (parent != null) {
+								var sibling = parent.child1 == node ? parent.child2 : parent.child1;
+
+								if (parent.parent != null) {
+									sibling.parent = parent.parent;
+									if (parent.parent.child1 == parent) {
+										parent.parent.child1 = sibling;
+									} else {
+										parent.parent.child2 = sibling;
 									}
 								}
 								return intersects;
@@ -149,7 +150,16 @@ class BVHNode {
 								return [];
 							}
 						}
+
+						function rotate(node:BVHNode<T>) {
+							if (node.parent == null) {
+								return;
 							}
+							var parent = node.parent;
+							var sibling = parent.child1 == node ? parent.child2 : parent.child1;
+							var costDiffs = [];
+							var nodeArea = node.bounds.xSize * node.bounds.ySize + node.bounds.zSize * node.bounds.ySize
+								+ node.bounds.xSize * node.bounds.zSize;
 
 							class BVHTree {
 								public var bounds:Bounds;
@@ -174,10 +184,50 @@ class BVHTree {
 								}
 
 								public function boundingSearch(searchbox:Bounds) {
-		return this.root.boundingSearch(searchbox);
+									var res = [];
+									if (this.root == null)
+										return res;
+
+									var q = [this.root];
+
+									while (q.length != 0) {
+										var current = q.shift();
+
+										if (current.bounds.containsBounds(searchbox) || current.bounds.collide(searchbox)) {
+											if (current.isLeaf) {
+												res.push(current.object);
+											} else {
+												if (current.child1 != null)
+													q.push(current.child1);
+												if (current.child2 != null)
+													q.push(current.child2);
+											}
+										}
+									}
+
+									return res;
 								}
 
 								public function rayCast(origin:Vector, direction:Vector) {
-		return this.root.rayCast(origin, direction);
+									var res = [];
+									if (this.root == null)
+										return res;
+
+									var ray = h3d.col.Ray.fromValues(origin.x, origin.y, origin.z, direction.x, direction.y, direction.z);
+									var q = [this.root];
+									while (q.length != 0) {
+										var current = q.shift();
+										if (ray.collide(current.bounds)) {
+											if (current.isLeaf) {
+												res = res.concat(current.object.rayCast(origin, direction));
+											} else {
+												if (current.child1 != null)
+													q.push(current.child1);
+												if (current.child2 != null)
+													q.push(current.child2);
+											}
+										}
+									}
+									return res;
 								}
 							}

src/collision/BoxCollisionEntity.hx

@@ -1,5 +1,6 @@
 package collision;
 
+import collision.BVHTree.IBVHObject;
 import src.MarbleGame;
 import src.TimeState;
 import h3d.Matrix;
@@ -10,7 +11,7 @@ import h3d.Vector;
 import h3d.col.Sphere;
 import h3d.col.Bounds;
 
-class BoxCollisionEntity extends CollisionEntity {
+class BoxCollisionEntity extends CollisionEntity implements IBVHObject {
 	var bounds:Bounds;
 
 	var _dbgEntity:h3d.scene.Object;

src/collision/CollisionEntity.hx

@@ -1,5 +1,6 @@
 package collision;
 
+import collision.BVHTree.IBVHObject;
 import src.TimeState;
 import src.GameObject;
 import dif.math.Point3F;
@@ -14,12 +15,12 @@ import h3d.col.Bounds;
 import src.PathedInterior;
 import src.Util;
 
-class CollisionEntity implements IOctreeObject {
+class CollisionEntity implements IOctreeObject implements IBVHObject {
 	public var boundingBox:Bounds;
 
 	public var octree:Octree;
 
-	public var bvh:BVHTree;
+	public var bvh:BVHTree<CollisionSurface>;
 
 	public var surfaces:Array<CollisionSurface>;
 

src/collision/CollisionSurface.hx

@@ -4,28 +4,22 @@ import h3d.Matrix;
 import h3d.col.Bounds;
 import octree.IOctreeObject;
 import h3d.Vector;
+import collision.BVHTree.IBVHObject;
 
-class CollisionSurface implements IOctreeObject {
+class CollisionSurface implements IOctreeObject implements IBVHObject {
 	public var priority:Int;
 	public var position:Int;
-
 	public var boundingBox:Bounds;
-
 	public var points:Array<Vector>;
 	public var normals:Array<Vector>;
 	public var indices:Array<Int>;
-
 	public var friction:Float = 1;
 	public var restitution:Float = 1;
 	public var force:Float = 0;
-
 	public var edgeData:Array<Int>;
-
 	public var edgeConcavities:Array<Bool>;
 	public var originalIndices:Array<Int>;
-
 	public var originalSurfaceIndex:Int;
-
 	public var key:Bool = false;
 
 	public function new() {}

src/collision/CollisionWorld.hx

@@ -11,9 +11,11 @@ class CollisionWorld {
 	public var octree:Octree;
 	public var entities:Array<CollisionEntity> = [];
 	public var dynamicEntities:Array<CollisionEntity> = [];
+	public var dynamicBVH:BVHTree<CollisionEntity>;
 
 	public function new() {
 		this.octree = new Octree();
+		this.dynamicBVH = new BVHTree();
 	}
 
 	public function sphereIntersection(spherecollision:SphereCollisionEntity, timeState:TimeState) {
@@ -44,7 +46,8 @@ class CollisionWorld {
 			}
 		}
 
-		for (obj in dynamicEntities) {
+		var dynSearch = dynamicBVH.boundingSearch(box);
+		for (obj in dynSearch) {
 			if (obj != spherecollision) {
 				if (obj.boundingBox.collide(box) && obj.go.isCollideable)
 					contacts = contacts.concat(obj.sphereIntersection(spherecollision, timeState));
@@ -72,20 +75,14 @@ class CollisionWorld {
 			contacts.push(entity);
 		}
 
-		for (obj in dynamicEntities) {
+		contacts = contacts.concat(dynamicBVH.boundingSearch(box));
-			if (obj.boundingBox.collide(box))
-				contacts.push(obj);
-		}
 
 		return contacts;
 	}
 
 	public function boundingSearch(bounds:Bounds, useCache:Bool = true) {
 		var contacts = this.octree.boundingSearch(bounds, useCache).map(x -> cast(x, CollisionEntity));
-		for (obj in dynamicEntities) {
+		contacts = contacts.concat(dynamicBVH.boundingSearch(bounds));
-			if (obj.boundingBox.collide(bounds))
-				contacts.push(obj);
-		}
 		return contacts;
 	}
 
@@ -117,6 +114,7 @@ class CollisionWorld {
 
 	public function addMovingEntity(entity:CollisionEntity) {
 		this.dynamicEntities.push(entity);
+		this.dynamicBVH.add(entity);
 	}
 
 	public function updateTransform(entity:CollisionEntity) {

src/gui/PlayGui.hx

@@ -411,6 +411,9 @@ class PlayGui {
 		} else if (powerupIdentifier == "Helicopter") {
 			powerupImageObject = new DtsObject();
 			powerupImageObject.dtsPath = "data/shapes/images/helicopter.dts";
+		} else if (powerupIdentifier == "MegaMarble") {
+			powerupImageObject = new DtsObject();
+			powerupImageObject.dtsPath = "data/shapes/items/megamarble.dts";
 		} else {
 			powerupIdentifier = "";
 			this.powerupImageObject = null;

src/octree/Octree.hx

@@ -139,7 +139,7 @@ class Octree {
 		return intersections;
 	}
 
-	public function boundingSearch(bounds:Bounds, useCache:Bool = true) {
+	public function boundingSearch(bounds:Bounds, useCache:Bool = false) {
 		var intersections = [];
 		if (useCache) {
 			if (this.prevBoundSearch != null) {