some collision optimization shit

src/DtsObject.hx

@@ -795,8 +795,10 @@ class DtsObject extends GameObject {
 		for (i in 0...this.colliders.length) {
 			if (this.dirtyTransforms[this.colliders[i].userData]) {
 				var absTform = this.graphNodes[this.colliders[i].userData].getAbsPos().clone();
-				if (this.colliders[i] != null)
+				if (this.colliders[i] != null) {
 					this.colliders[i].setTransform(absTform);
+					this.level.collisionWorld.updateTransform(this.colliders[i]);
+				}
 			}
 		}
 		for (i in 0...this.dirtyTransforms.length) {

src/Main.hx

@@ -64,14 +64,6 @@ class Main extends hxd.App {
 		world.init();
 		world.start();
 
-		// s3d.camera.
-		// marble.setPosition(5, 0, 5);
-
-		// var marble2 = new Marble();
-		// world.addMarble(marble2);
-		// marble2.setPosition(5, 0, 5);
-		// marble.setPosition(-10, -5, 5);
-
 		fpsCounter = new Text(DefaultFont.get(), s2d);
 		fpsCounter.y = 40;
 		fpsCounter.color = new Vector(1, 1, 1, 1);

src/Marble.hx

@@ -245,11 +245,9 @@ class Marble extends GameObject {
 		if (currentTime - this.helicopterEnableTime < 5) {
 			A = A.multiply(0.25);
 		}
-		for (obj in level.dtsObjects) {
-			if (obj is ForceObject) {
-				var force = cast(obj, ForceObject).getForce(this.getAbsPos().getPosition());
-				A = A.add(force.multiply(1 / _mass));
-			}
+		for (obj in level.forceObjects) {
+			var force = cast(obj, ForceObject).getForce(this.getAbsPos().getPosition());
+			A = A.add(force.multiply(1 / _mass));
 		}
 		if (contacts.length != 0 && this.mode != Start) {
 			var contactForce = 0.0;
@@ -604,13 +602,14 @@ class Marble extends GameObject {
 	}
 
 	function getIntersectionTime(dt:Float, velocity:Vector) {
-		var searchbox = new Bounds();
-		searchbox.addSpherePos(this.x, this.y, this.z, _radius);
-		searchbox.addSpherePos(this.x + velocity.x * dt, this.y + velocity.y * dt, this.z + velocity.z * dt, _radius);
+		// var searchbox = new Bounds();
+		// searchbox.addSpherePos(this.x, this.y, this.z, _radius);
+		// searchbox.addSpherePos(this.x + velocity.x * dt, this.y + velocity.y * dt, this.z + velocity.z * dt, _radius);
 
 		var position = this.getAbsPos().getPosition();
 
-		var foundObjs = this.level.collisionWorld.boundingSearch(searchbox);
+		// var foundObjs = this.level.collisionWorld.boundingSearch(searchbox);
+		var foundObjs = this.contactEntities;
 
 		function toDifPoint(vec:Vector) {
 			return new Point3F(vec.x, vec.y, vec.z);

src/MarbleWorld.hx

@@ -1,5 +1,6 @@
 package src;
 
+import src.ForceObject;
 import h3d.scene.pbr.DirLight;
 import h3d.col.Bounds;
 import triggers.HelpTrigger;
@@ -65,6 +66,7 @@ class MarbleWorld extends Scheduler {
 	public var pathedInteriors:Array<PathedInterior> = [];
 	public var marbles:Array<Marble> = [];
 	public var dtsObjects:Array<DtsObject> = [];
+	public var forceObjects:Array<ForceObject> = [];
 	public var triggers:Array<Trigger> = [];
 
 	var shapeImmunity:Array<DtsObject> = [];
@@ -560,6 +562,9 @@ class MarbleWorld extends Scheduler {
 
 	public function addDtsObject(obj:DtsObject) {
 		this.dtsObjects.push(obj);
+		if (obj is ForceObject) {
+			this.forceObjects.push(cast obj);
+		}
 		obj.init(cast this);
 		if (obj.useInstancing) {
 			this.instanceManager.addObject(obj);
@@ -754,9 +759,11 @@ class MarbleWorld extends Scheduler {
 			}
 		}
 
-		if (spherebounds.collide(this.endPad.finishBounds)) {
-			if (collision.gjk.GJK.gjk(gjkSphere, this.endPad.finishCollider) != null) {
-				touchFinish();
+		if (this.finishTime == null) {
+			if (spherebounds.collide(this.endPad.finishBounds)) {
+				if (collision.gjk.GJK.gjk(gjkSphere, this.endPad.finishCollider) != null) {
+					touchFinish();
+				}
 			}
 		}
 		this.shapeImmunity = newImmunity;

src/collision/CollisionEntity.hx

@@ -91,7 +91,9 @@ class CollisionEntity implements IOctreeObject {
 		invMatrix.invert();
 		var localpos = position.clone();
 		localpos.transform(invMatrix);
-		var surfaces = octree.radiusSearch(localpos, radius * 1.1);
+		var sphereBounds = new Bounds();
+		sphereBounds.addSpherePos(localpos.x, localpos.y, localpos.z, radius * 1.1);
+		var surfaces = octree.boundingSearch(sphereBounds);
 
 		var tform = transform.clone();
 		// tform.setPosition(tform.getPosition().add(this.velocity.multiply(timeState.dt)));

src/collision/CollisionWorld.hx

@@ -19,16 +19,11 @@ class CollisionWorld {
 		var position = spherecollision.transform.getPosition();
 		var radius = spherecollision.radius;
 		var velocity = spherecollision.velocity;
-		var searchdist = (velocity.length() * timeState.dt) + radius;
 		// var intersections = this.octree.radiusSearch(position, searchdist);
 
 		var box = new Bounds();
-		box.xMin = position.x - radius;
-		box.yMin = position.y - radius;
-		box.zMin = position.z - radius;
-		box.xMax = position.x + radius;
-		box.yMax = position.y + radius;
-		box.zMax = position.z + radius;
+		box.addSpherePos(position.x, position.y, position.z, radius);
+		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.rtree.search([box.xMin, box.yMax, box.zMin], [box.xSize, box.ySize, box.zSize]);

src/octree/Octree.hx

@@ -25,8 +25,6 @@ class Octree {
 			return; // Don't insert if already contained in the tree
 		while (!this.root.largerThan(object) || !this.root.containsCenter(object)) {
 			// The root node does not fit the object; we need to grow the tree.
-			var a = !this.root.largerThan(object);
-			var b = !this.root.containsCenter(object);
 			if (this.root.depth == -32) {
 				return;
 			}
@@ -64,11 +62,11 @@ class Octree {
 			vec.y = (i & 2) == 2 ? towards.boundingBox.yMin : towards.boundingBox.yMax;
 			vec.z = (i & 4) == 4 ? towards.boundingBox.zMin : towards.boundingBox.zMax;
 			if (!this.root.containsPoint(vec)) {
-				averagePoint.add(vec);
+				averagePoint = averagePoint.add(vec);
 				count++;
 			}
 		}
-		averagePoint.multiply(1 / count); // count should be greater than 0, because that's why we're growing in the first place.
+		averagePoint = averagePoint.multiply(1 / count); // count should be greater than 0, because that's why we're growing in the first place.
 		// Determine the direction from the root center to the determined point
 		var rootCenter = this.root.min.clone().add(new Vector(this.root.size / 2, this.root.size / 2, this.root.size / 2));
 		var direction = averagePoint.sub(rootCenter); // Determine the "direction of growth"

src/octree/OctreeNode.hx

@@ -16,7 +16,7 @@ class OctreeNode implements IOctreeElement {
 	public var min = new Vector();
 
 	/** The size of the bounding box on all three axes. This forces the bounding box to be a cube. */
-	public var size:Int;
+	public var size:Float;
 
 	public var octants:Array<OctreeNode> = null;
 
@@ -84,7 +84,7 @@ class OctreeNode implements IOctreeElement {
 		for (i in 0...8) {
 			var newNode = new OctreeNode(this.octree, this.depth + 1);
 			newNode.parent = this;
-			newNode.size = cast this.size / 2;
+			newNode.size = this.size / 2;
 			newNode.min.set(this.min.x
 				+ newNode.size * ((i & 1) >> 0), // The x coordinate changes every index
 				this.min.y