diff --git a/src/Marble.hx b/src/Marble.hx index ce689fc4..7a6f6c1b 100644 --- a/src/Marble.hx +++ b/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 = nudgeToContacts(expectedPos, _radius, finalPosData.foundContacts); + var newPos = expectedPos; + // 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)); - this.omega = this.omega.sub(a.multiply(tDiff)); + if (tDiff > 0) { + this.velocity = this.velocity.sub(A.multiply(tDiff)); + this.omega = this.omega.sub(a.multiply(tDiff)); - timeStep = updatedTimestep; + timeStep = updatedTimestep; + } } } diff --git a/src/MarbleWorld.hx b/src/MarbleWorld.hx index 171d14b9..9c0905a3 100644 --- a/src/MarbleWorld.hx +++ b/src/MarbleWorld.hx @@ -555,9 +555,10 @@ class MarbleWorld extends Scheduler { if (interiorScale.z == 0) interiorScale.z = 0.0001; - var mat = new Matrix(); - interiorRotation.toMatrix(mat); - mat.scale(interiorScale.x, interiorScale.y, interiorScale.z); + var mat = Matrix.S(interiorScale.x, interiorScale.y, interiorScale.z); + var tmp = new Matrix(); + 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(); - mat.scale(shapeScale.x, shapeScale.y, shapeScale.z); + var mat = Matrix.S(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(); - mat.scale(shapeScale.x, shapeScale.y, shapeScale.z); + var mat = Matrix.S(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(); - mat.scale(shapeScale.x, shapeScale.y, shapeScale.z); + var mat = Matrix.S(shapeScale.x, shapeScale.y, shapeScale.z); + var tmp = new Matrix(); + shapeRotation.toMatrix(tmp); + mat.multiply3x4(mat, tmp); mat.setPosition(shapePosition); this.addDtsObject(tsShape, () -> { diff --git a/src/collision/BoxCollisionEntity.hx b/src/collision/BoxCollisionEntity.hx index c8b37950..dd8a71f9 100644 --- a/src/collision/BoxCollisionEntity.hx +++ b/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); // } diff --git a/src/collision/CollisionEntity.hx b/src/collision/CollisionEntity.hx index a1433d75..0f8c4e89 100644 --- a/src/collision/CollisionEntity.hx +++ b/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 { @@ -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(); diff --git a/src/collision/SphereCollisionEntity.hx b/src/collision/SphereCollisionEntity.hx index a813ad63..ed3b23bc 100644 --- a/src/collision/SphereCollisionEntity.hx +++ b/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) { diff --git a/src/octree/Octree.hx b/src/octree/Octree.hx index 6dbd258a..23529497 100644 --- a/src/octree/Octree.hx +++ b/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); - this.insert(object); + if (!this.objectToNode.exists(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) - return; + if (this.root.octants == null) { + 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) {