fix wrong TRS order

src/Marble.hx

@@ -779,13 +779,16 @@ class Marble extends GameObject {
 			var relVel = velocity.sub(obj.velocity);
 			var relLocalVel = relVel.transformed3x3(invMatrix);
 
+			var invScale = invMatrix.getScale();
+			var sphereRadius = new Vector(radius * invScale.x, radius * invScale.y, radius * invScale.z);
+
 			var boundThing = new Bounds();
 			boundThing.addSpherePos(localpos.x, localpos.y, localpos.z, radius * 2);
 			boundThing.addSpherePos(localpos.x
 				+ relLocalVel.x * deltaT * 5, localpos.y
 				+ relLocalVel.y * deltaT * 5, localpos.z
 				+ relLocalVel.z * deltaT * 5,
-				radius * 2);
+				Math.max(Math.max(sphereRadius.x, sphereRadius.y), sphereRadius.z) * 2);
 
 			var currentFinalPos = position.add(relVel.multiply(finalT)); // localpos.add(relLocalVel.multiply(finalT));
 
@@ -1367,8 +1370,8 @@ class Marble extends GameObject {
 				tdiff = diff;
 			}
 			var expectedPos = finalPosData.position;
-			// var newPos = expectedPos;
+			var newPos = expectedPos;
-			var newPos = nudgeToContacts(expectedPos, _radius, finalPosData.foundContacts);
+			// var newPos = nudgeToContacts(expectedPos, _radius, finalPosData.foundContacts);
 
 			if (this.velocity.lengthSq() > 1e-8) {
 				var posDiff = newPos.sub(expectedPos);
@@ -1378,10 +1381,12 @@ class Marble extends GameObject {
 					var updatedTimestep = expectedProjPos.sub(pos).length() / velocity.length();
 
 					var tDiff = updatedTimestep - timeStep;
-					this.velocity = this.velocity.sub(A.multiply(tDiff));
+					if (tDiff > 0) {
-					this.omega = this.omega.sub(a.multiply(tDiff));
+						this.velocity = this.velocity.sub(A.multiply(tDiff));
+						this.omega = this.omega.sub(a.multiply(tDiff));
 
-					timeStep = updatedTimestep;
+						timeStep = updatedTimestep;
+					}
 				}
 			}
 

src/MarbleWorld.hx

@@ -555,9 +555,10 @@ class MarbleWorld extends Scheduler {
 			if (interiorScale.z == 0)
 				interiorScale.z = 0.0001;
 
-			var mat = new Matrix();
+			var mat = Matrix.S(interiorScale.x, interiorScale.y, interiorScale.z);
-			interiorRotation.toMatrix(mat);
+			var tmp = new Matrix();
-			mat.scale(interiorScale.x, interiorScale.y, interiorScale.z);
+			interiorRotation.toMatrix(tmp);
+			mat.multiply3x4(mat, tmp);
 			mat.setPosition(interiorPosition);
 
 			interior.setTransform(mat);
@@ -642,8 +643,10 @@ class MarbleWorld extends Scheduler {
 		if (shapeScale.z == 0)
 			shapeScale.z = 0.0001;
 
-		var mat = shapeRotation.toMatrix();
+		var mat = Matrix.S(shapeScale.x, shapeScale.y, shapeScale.z);
-		mat.scale(shapeScale.x, shapeScale.y, shapeScale.z);
+		var tmp = new Matrix();
+		shapeRotation.toMatrix(tmp);
+		mat.multiply3x4(mat, tmp);
 		mat.setPosition(shapePosition);
 
 		this.addDtsObject(shape, () -> {
@@ -720,8 +723,10 @@ class MarbleWorld extends Scheduler {
 		if (shapeScale.z == 0)
 			shapeScale.z = 0.0001;
 
-		var mat = shapeRotation.toMatrix();
+		var mat = Matrix.S(shapeScale.x, shapeScale.y, shapeScale.z);
-		mat.scale(shapeScale.x, shapeScale.y, shapeScale.z);
+		var tmp = new Matrix();
+		shapeRotation.toMatrix(tmp);
+		mat.multiply3x4(mat, tmp);
 		mat.setPosition(shapePosition);
 
 		this.addDtsObject(shape, () -> {
@@ -776,8 +781,10 @@ class MarbleWorld extends Scheduler {
 		if (shapeScale.z == 0)
 			shapeScale.z = 0.0001;
 
-		var mat = shapeRotation.toMatrix();
+		var mat = Matrix.S(shapeScale.x, shapeScale.y, shapeScale.z);
-		mat.scale(shapeScale.x, shapeScale.y, shapeScale.z);
+		var tmp = new Matrix();
+		shapeRotation.toMatrix(tmp);
+		mat.multiply3x4(mat, tmp);
 		mat.setPosition(shapePosition);
 
 		this.addDtsObject(tsShape, () -> {

src/collision/BoxCollisionEntity.hx

@@ -14,8 +14,6 @@ import h3d.col.Bounds;
 class BoxCollisionEntity extends CollisionEntity implements IBVHObject {
 	var bounds:Bounds;
 
-	var _dbgEntity:h3d.scene.Object;
-
 	public function new(bounds:Bounds, go:GameObject) {
 		super(go);
 		this.bounds = bounds;
@@ -26,11 +24,11 @@ class BoxCollisionEntity extends CollisionEntity implements IBVHObject {
 		this.boundingBox = bounds.clone();
 		this.boundingBox.transform(this.transform);
 		// if (_dbgEntity == null) {
-		// 	_dbgEntity = this.boundingBox.makeDebugObj();
+		// 	_dbgEntity = cast this.boundingBox.makeDebugObj();
 		// 	_dbgEntity.getMaterials()[0].mainPass.wireframe = true;
 		// 	MarbleGame.instance.scene.addChild(_dbgEntity);
 		// } else {
-		// 	_dbgEntity = this.boundingBox.makeDebugObj();
+		// 	_dbgEntity = cast this.boundingBox.makeDebugObj();
 		// 	_dbgEntity.getMaterials()[0].mainPass.wireframe = true;
 		// 	MarbleGame.instance.scene.addChild(_dbgEntity);
 		// }
@@ -39,7 +37,7 @@ class BoxCollisionEntity extends CollisionEntity implements IBVHObject {
 	public override function setTransform(transform:Matrix) {
 		super.setTransform(transform);
 		// if (_dbgEntity != null) {
-		// 	_dbgEntity = this.boundingBox.makeDebugObj();
+		// 	_dbgEntity = cast this.boundingBox.makeDebugObj();
 		// 	_dbgEntity.getMaterials()[0].mainPass.wireframe = true;
 		// 	MarbleGame.instance.scene.addChild(_dbgEntity);
 		// }

src/collision/CollisionEntity.hx

@@ -14,6 +14,8 @@ import h3d.Matrix;
 import h3d.col.Bounds;
 import src.PathedInterior;
 import src.Util;
+import src.Debug;
+import src.MarbleGame;
 
 class CollisionEntity implements IOctreeObject implements IBVHObject {
 	public var boundingBox:Bounds;
@@ -40,6 +42,8 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
 
 	var _transformKey:Int = 0;
 
+	var _dbgEntity:h3d.scene.Mesh;
+
 	public function new(go:GameObject) {
 		this.go = go;
 		this.octree = new Octree();
@@ -100,6 +104,18 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
 			boundingBox.add(tform);
 		}
 		this.boundingBox = boundingBox;
+		if (Debug.drawBounds) {
+			if (_dbgEntity == null) {
+				_dbgEntity = cast this.boundingBox.makeDebugObj();
+				_dbgEntity.getMaterials()[0].mainPass.wireframe = true;
+				MarbleGame.instance.scene.addChild(_dbgEntity);
+			} else {
+				_dbgEntity.remove();
+				_dbgEntity = cast this.boundingBox.makeDebugObj();
+				_dbgEntity.getMaterials()[0].mainPass.wireframe = true;
+				MarbleGame.instance.scene.addChild(_dbgEntity);
+			}
+		}
 	}
 
 	public function rayCast(rayOrigin:Vector, rayDirection:Vector):Array<RayIntersectionData> {
@@ -149,7 +165,9 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
 		localPos.transform(invMatrix);
 		// 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 invScale = invMatrix.getScale();
+		var sphereRadius = new Vector(radius * invScale.x, radius * invScale.y, radius * invScale.z);
+		sphereBounds.addSpherePos(localPos.x, localPos.y, localPos.z, Math.max(Math.max(sphereRadius.x, sphereRadius.y), sphereRadius.z) * 1.1);
 		var surfaces = bvh == null ? octree.boundingSearch(sphereBounds).map(x -> cast x) : bvh.boundingSearch(sphereBounds);
 
 		var tform = transform.clone();

src/collision/SphereCollisionEntity.hx

@@ -13,7 +13,6 @@ class SphereCollisionEntity extends CollisionEntity {
 	public var radius:Float;
 	public var marble:Marble;
 
-	var _dbgEntity:h3d.scene.Mesh;
 	var _dbgEntity2:h3d.scene.Mesh;
 
 	public function new(marble:Marble) {

src/octree/Octree.hx

@@ -31,8 +31,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;
 			}
@@ -55,8 +53,15 @@ class Octree {
 
 	/** Updates an object in the tree whose bounding box has changed. */
 	public function update(object:IOctreeObject) {
-		this.remove(object);
+		if (!this.objectToNode.exists(object)) {
-		this.insert(object);
+			this.insert(object);
+			return;
+		}
+		var success = this.objectToNode.get(object).update(object);
+		if (!success) {
+			this.objectToNode.remove(object);
+			this.insert(object);
+		}
 	}
 
 	/** Expand the octree towards an object that doesn't fit in it. */
@@ -112,8 +117,25 @@ class Octree {
 			this.root.depth = 0;
 			return;
 		}
-		if (this.root.octants == null)
+		if (this.root.octants == null) {
-			return;
+			this.root.createOctants();
+
+			var fittingOctant:OctreeNode = null;
+			for (obj in this.root.objects) {
+				if (this.root.octants[0].largerThan(obj)) {
+					for (i in 0...8) {
+						var octant = this.root.octants[i];
+						if (octant.containsCenter(obj)) {
+							if (fittingOctant != null && fittingOctant != octant)
+								return;
+							fittingOctant = octant;
+						}
+					}
+				} else {
+					return;
+				}
+			}
+		}
 		// Find the only non-empty octant
 		var nonEmptyOctant:OctreeNode = null;
 		for (i in 0...8) {