diff --git a/src/Marble.hx b/src/Marble.hx index 048e6643..60bbd94f 100644 --- a/src/Marble.hx +++ b/src/Marble.hx @@ -267,7 +267,9 @@ class Marble extends GameObject { public var contacts:Array = []; public var bestContact:CollisionInfo; - public var contactEntities:Array = []; + + public static var contactScratch:Array = []; + static var surfaceScratch:Array = []; var queuedContacts:Array = []; var appliedImpulses:Array<{impulse:Vector, contactImpulse:Bool}> = []; @@ -693,9 +695,7 @@ class Marble extends GameObject { function findContacts(collisionWorld:CollisionWorld, timeState:TimeState) { this.contacts = queuedContacts; CollisionPool.clear(); - var c = collisionWorld.sphereIntersection(this.collider, timeState); - this.contactEntities = c.foundEntities; - contacts = contacts.concat(c.contacts); + collisionWorld.sphereIntersection(this.collider, timeState, this.contacts); } public function queueCollision(collisionInfo:CollisionInfo) { @@ -1255,7 +1255,10 @@ class Marble extends GameObject { searchbox.addSpherePos(position.x, position.y, position.z, _radius); searchbox.addSpherePos(position.x + velocity.x * deltaT, position.y + velocity.y * deltaT, position.z + velocity.z * deltaT, _radius); - var foundObjs = this.collisionWorld.boundingSearch(searchbox); + contactScratch.resize(0); + this.collisionWorld.boundingSearch(searchbox, contactScratch); + + var foundObjs = contactScratch; // foundObjs.push(this.collisionWorld.staticWorld); var finalT = deltaT; @@ -1325,11 +1328,13 @@ class Marble extends GameObject { Math.max(Math.max(sphereRadius.x, sphereRadius.y), sphereRadius.z) * 2); var currentFinalPos = position.add(relVel.multiply(finalT)); // localpos.add(relLocalVel.multiply(finalT)); - var surfaces = @:privateAccess obj.grid != null ? @:privateAccess obj.grid.boundingSearch(boundThing) : (obj.bvh == null ? obj.octree.boundingSearch(boundThing) - .map(x -> cast x) : obj.bvh.boundingSearch(boundThing)); + surfaceScratch.resize(0); + if (@:privateAccess obj.grid != null) + @:privateAccess obj.grid.boundingSearch(boundThing, surfaceScratch); + var surfaces = surfaceScratch; for (surf in surfaces) { - var surface:CollisionSurface = cast surf; + var surface:CollisionSurface = surf; currentFinalPos = position.add(relVel.multiply(finalT)); @@ -1851,7 +1856,7 @@ class Marble extends GameObject { } } } - this.queuedContacts = []; + this.queuedContacts.resize(0); newPos = this.collider.transform.getPosition(); @@ -2604,7 +2609,6 @@ class Marble extends GameObject { this.megaMarbleUseTick = 0; this.netFlags = MarbleNetFlags.DoBlast | MarbleNetFlags.DoMega | MarbleNetFlags.DoHelicopter | MarbleNetFlags.PickupPowerup | MarbleNetFlags.GravityChange | MarbleNetFlags.UsePowerup; this.lastContactNormal = new Vector(0, 0, 1); - this.contactEntities = []; this._firstTick = true; this.finishAnimTime = 0; this.physicsAccumulator = 0; diff --git a/src/MarbleWorld.hx b/src/MarbleWorld.hx index 699fa0d9..f3a50491 100644 --- a/src/MarbleWorld.hx +++ b/src/MarbleWorld.hx @@ -517,6 +517,9 @@ class MarbleWorld extends Scheduler { public function start() { Console.log("LEVEL START"); restart(this.marble, true); + + this.collisionWorld.build(); + for (interior in this.interiors) interior.onLevelStart(); for (shape in this.dtsObjects) @@ -1996,7 +1999,9 @@ class MarbleWorld extends Scheduler { // 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; - var contacts = this.collisionWorld.boundingSearch(box); + Marble.contactScratch.resize(0); + this.collisionWorld.boundingSearch(box, Marble.contactScratch); + var contacts = Marble.contactScratch; var inside = []; for (contact in contacts) { @@ -2122,7 +2127,9 @@ class MarbleWorld extends Scheduler { var checkSphereRadius = checkBounds.getMax().sub(checkBoundsCenter).length(); var checkSphere = new Bounds(); checkSphere.addSpherePos(checkBoundsCenter.x, checkBoundsCenter.y, checkBoundsCenter.z, checkSphereRadius); - var endpadBB = this.collisionWorld.boundingSearch(checkSphere, false); + Marble.contactScratch.resize(0); + this.collisionWorld.boundingSearch(checkSphere, Marble.contactScratch, false); + var endpadBB = Marble.contactScratch; var found = false; for (collider in endpadBB) { if (collider.go == this.endPad) { diff --git a/src/MeshBatch.hx b/src/MeshBatch.hx index 51347d34..96b30622 100644 --- a/src/MeshBatch.hx +++ b/src/MeshBatch.hx @@ -40,7 +40,7 @@ class MeshBatch extends MultiMaterial { static var modelViewID = hxsl.Globals.allocID("global.modelView"); static var modelViewTransposeID = hxsl.Globals.allocID("global.modelViewTranspose"); static var modelViewInverseID = hxsl.Globals.allocID("global.modelViewInverse"); - static var MAX_BUFFER_ELEMENTS = 4096; + static var MAX_BUFFER_ELEMENTS = 1024; var instanced:h3d.prim.Instanced; var dataPasses:BatchData; diff --git a/src/Util.hx b/src/Util.hx index d73e9c4c..1df08c73 100644 --- a/src/Util.hx +++ b/src/Util.hx @@ -22,6 +22,14 @@ class Util { return r2; } + public static inline function imin(a:Int, b:Int) { + return a < b ? a : b; + } + + public static inline function imax(a:Int, b:Int) { + return a > b ? a : b; + } + public static inline function clamp(value:Float, min:Float, max:Float) { if (value < min) return min; diff --git a/src/collision/BVHTree.hx b/src/collision/BVHTree.hx index 2364519e..6d6c654a 100644 --- a/src/collision/BVHTree.hx +++ b/src/collision/BVHTree.hx @@ -6,7 +6,7 @@ import h3d.col.Bounds; interface IBVHObject { var boundingBox:Bounds; var key:Int; - function rayCast(rayOrigin:Vector, rayDirection:Vector, results:Array):Void; + function rayCast(rayOrigin:Vector, rayDirection:Vector, results:Array, bestT:Float):Float; } @:generic @@ -463,7 +463,7 @@ class BVHTree { return res; } - public function rayCast(origin:Vector, direction:Vector) { + public function rayCast(origin:Vector, direction:Vector, bestT:Float) { var res = []; if (this.root == null) return res; @@ -476,7 +476,7 @@ class BVHTree { var currentnode = this.nodes[current]; if (currentnode.collideRay(ray)) { if (currentnode.isLeaf) { - currentnode.object.rayCast(origin, direction, res); + bestT = currentnode.object.rayCast(origin, direction, res, bestT); } else { if (currentnode.child1 != -1) q.push(currentnode.child1); diff --git a/src/collision/BoxCollisionEntity.hx b/src/collision/BoxCollisionEntity.hx index 9332e26e..a4501e03 100644 --- a/src/collision/BoxCollisionEntity.hx +++ b/src/collision/BoxCollisionEntity.hx @@ -27,10 +27,12 @@ class BoxCollisionEntity extends CollisionEntity implements IBVHObject { if (Debug.drawBounds) { if (_dbgEntity == null) { _dbgEntity = cast this.boundingBox.makeDebugObj(); + _dbgEntity.getMaterials()[0].castShadows = false; _dbgEntity.getMaterials()[0].mainPass.wireframe = true; MarbleGame.instance.scene.addChild(_dbgEntity); } else { _dbgEntity = cast this.boundingBox.makeDebugObj(); + _dbgEntity.getMaterials()[0].castShadows = false; _dbgEntity.getMaterials()[0].mainPass.wireframe = true; MarbleGame.instance.scene.addChild(_dbgEntity); } @@ -42,17 +44,17 @@ class BoxCollisionEntity extends CollisionEntity implements IBVHObject { if (Debug.drawBounds) { if (_dbgEntity != null) { _dbgEntity = cast this.boundingBox.makeDebugObj(); + _dbgEntity.getMaterials()[0].castShadows = false; _dbgEntity.getMaterials()[0].mainPass.wireframe = true; MarbleGame.instance.scene.addChild(_dbgEntity); } } } - public override function rayCast(rayOrigin:Vector, rayDirection:Vector, results:Array) { + public override function rayCast(rayOrigin:Vector, rayDirection:Vector, results:Array, bestT:Float) { // TEMP cause bruh + return Math.POSITIVE_INFINITY; } - public override function sphereIntersection(collisionEntity:SphereCollisionEntity, timeState:TimeState) { - return []; - } + public override function sphereIntersection(collisionEntity:SphereCollisionEntity, timeState:TimeState, contacts:Array) {} } diff --git a/src/collision/CollisionEntity.hx b/src/collision/CollisionEntity.hx index 41349ea8..aad9f494 100644 --- a/src/collision/CollisionEntity.hx +++ b/src/collision/CollisionEntity.hx @@ -23,8 +23,7 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { public var octree:Octree; - public var bvh:BVHTree; - + // public var bvh:BVHTree; var grid:Grid; public var surfaces:Array; @@ -67,12 +66,12 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { // Generates the bvh public function finalize() { this.generateBoundingBox(); - #if hl - this.bvh = new BVHTree(); - for (surface in this.surfaces) { - this.bvh.add(surface); - } - #end + // #if hl + // this.bvh = new BVHTree(); + // for (surface in this.surfaces) { + // this.bvh.add(surface); + // } + // #end var bbox = new Bounds(); for (surface in this.surfaces) bbox.add(surface.boundingBox); @@ -84,11 +83,14 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { } public function dispose() { - for (s in this.surfaces) - s.dispose(); + if (this.surfaces != null) { + for (s in this.surfaces) + s.dispose(); + } go = null; surfaces = null; - bvh = null; + grid = null; + // bvh = null; octree = null; } @@ -100,7 +102,7 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { var oldPos = this.transform.getPosition(); var newPos = transform.getPosition(); this.transform.setPosition(newPos); - this.invTransform = this.transform.getInverse(); + this.invTransform.prependTranslation(oldPos.x - newPos.x, oldPos.y - newPos.y, oldPos.z - newPos.z); if (this.boundingBox == null) generateBoundingBox(); else { @@ -110,6 +112,21 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { this.boundingBox.yMax += newPos.y - oldPos.y; this.boundingBox.zMin += newPos.z - oldPos.z; this.boundingBox.zMax += newPos.z - oldPos.z; + + if (Debug.drawBounds) { + if (_dbgEntity == null) { + _dbgEntity = cast this.boundingBox.makeDebugObj(); + _dbgEntity.getMaterials()[0].castShadows = false; + _dbgEntity.getMaterials()[0].mainPass.wireframe = true; + MarbleGame.instance.scene.addChild(_dbgEntity); + } else { + _dbgEntity.remove(); + _dbgEntity = cast this.boundingBox.makeDebugObj(); + _dbgEntity.getMaterials()[0].castShadows = false; + _dbgEntity.getMaterials()[0].mainPass.wireframe = true; + MarbleGame.instance.scene.addChild(_dbgEntity); + } + } } } else { this.transform.load(transform); @@ -130,18 +147,20 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { if (Debug.drawBounds) { if (_dbgEntity == null) { _dbgEntity = cast this.boundingBox.makeDebugObj(); + _dbgEntity.getMaterials()[0].castShadows = false; _dbgEntity.getMaterials()[0].mainPass.wireframe = true; MarbleGame.instance.scene.addChild(_dbgEntity); } else { _dbgEntity.remove(); _dbgEntity = cast this.boundingBox.makeDebugObj(); + _dbgEntity.getMaterials()[0].castShadows = false; _dbgEntity.getMaterials()[0].mainPass.wireframe = true; MarbleGame.instance.scene.addChild(_dbgEntity); } } } - public function rayCast(rayOrigin:Vector, rayDirection:Vector, results:Array) { + public function rayCast(rayOrigin:Vector, rayDirection:Vector, results:Array, bestT:Float) { var invMatrix = invTransform; var invTPos = invMatrix.clone(); invTPos.transpose(); @@ -159,13 +178,17 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { // } // return intersections; // iData; // } else { - var intersections = grid.rayCast(rStart, rDir); // this.bvh.rayCast(rStart, rDir); + var intersections = grid.rayCast(rStart, rDir, bestT); // this.bvh.rayCast(rStart, rDir); for (i in intersections) { i.point.transform(transform); i.normal.transform3x3(invTPos); i.normal.normalize(); - results.push(i); + if (i.t < bestT) { + bestT = i.t; + results.push(i); + } } + return bestT; // } } @@ -177,7 +200,9 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { this.priority = priority; } - public function sphereIntersection(collisionEntity:SphereCollisionEntity, timeState:TimeState) { + static var surfaceSearchPool:Array = []; + + public function sphereIntersection(collisionEntity:SphereCollisionEntity, timeState:TimeState, contacts:Array) { var position = collisionEntity.transform.getPosition(); var radius = collisionEntity.radius + 0.001; @@ -192,7 +217,9 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { 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 = grid.boundingSearch(sphereBounds); // bvh == null ? octree.boundingSearch(sphereBounds).map(x -> cast x) : bvh.boundingSearch(sphereBounds); + surfaceSearchPool.resize(0); + grid.boundingSearch(sphereBounds, surfaceSearchPool); + var surfaces = surfaceSearchPool; var invtform = invMatrix.clone(); invtform.transpose(); @@ -204,8 +231,6 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { invtform.load(Matrix.I()); } - var contacts = []; - for (obj in surfaces) { var surface:CollisionSurface = cast obj; @@ -278,7 +303,5 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { // if (surfaceBestContact != null) // contacts.push(surfaceBestContact); } - - return contacts; } } diff --git a/src/collision/CollisionHull.hx b/src/collision/CollisionHull.hx index 3b8bdd17..604e0be8 100644 --- a/src/collision/CollisionHull.hx +++ b/src/collision/CollisionHull.hx @@ -18,7 +18,7 @@ class CollisionHull extends CollisionEntity { super(go); } - public override function sphereIntersection(collisionEntity:SphereCollisionEntity, timeState:TimeState):Array { + public override function sphereIntersection(collisionEntity:SphereCollisionEntity, timeState:TimeState, contacts:Array) { var bbox = this.boundingBox; var box = new Bounds(); var pos = collisionEntity.transform.getPosition(); @@ -51,10 +51,9 @@ class CollisionHull extends CollisionEntity { cinfo.friction = friction; cinfo.force = force; this.go.onMarbleContact(collisionEntity.marble, timeState, cinfo); - return [cinfo]; + contacts.push(cinfo); } } - return []; } public override function addSurface(surface:CollisionSurface) { diff --git a/src/collision/CollisionSurface.hx b/src/collision/CollisionSurface.hx index e7c23f65..4d3dc8b6 100644 --- a/src/collision/CollisionSurface.hx +++ b/src/collision/CollisionSurface.hx @@ -139,7 +139,7 @@ class CollisionSurface implements IOctreeObject implements IBVHObject { normals.push(z); } - public function rayCast(rayOrigin:Vector, rayDirection:Vector, intersections:Array) { + public function rayCast(rayOrigin:Vector, rayDirection:Vector, intersections:Array, bestT:Float) { var i = 0; while (i < indices.length) { var p1 = getPoint(indices[i]); @@ -152,10 +152,19 @@ class CollisionSurface implements IOctreeObject implements IBVHObject { var ip = rayOrigin.add(rayDirection.multiply(t)); ip.w = 1; if (t >= 0 && Collision.PointInTriangle(ip, p1, p2, p3)) { - intersections.push({point: ip, normal: n, object: cast this}); + if (t < bestT) { + bestT = t; + intersections.push({ + point: ip.clone(), + normal: n.clone(), + object: cast this, + t: t + }); + } } i += 3; } + return bestT; } public function support(direction:Vector, transform:Matrix) { diff --git a/src/collision/CollisionWorld.hx b/src/collision/CollisionWorld.hx index f07e89d8..da1a97f9 100644 --- a/src/collision/CollisionWorld.hx +++ b/src/collision/CollisionWorld.hx @@ -17,26 +17,33 @@ class SphereIntersectionResult { class CollisionWorld { public var staticWorld:CollisionEntity; - public var octree:Octree; + public var grid:GridBroadphase; public var entities:Array = []; public var dynamicEntities:Array = []; - public var dynamicOctree:Octree; + public var dynamicGrid:GridBroadphase; public var marbleEntities:Array = []; var dynamicEntitySet:Map = []; public function new() { - this.octree = new Octree(); - this.dynamicOctree = new Octree(); + this.grid = new GridBroadphase(); + this.dynamicGrid = new GridBroadphase(); this.staticWorld = new CollisionEntity(null); } - public function sphereIntersection(spherecollision:SphereCollisionEntity, timeState:TimeState):SphereIntersectionResult { + public function build() { + this.grid.build(); + this.dynamicGrid.setBounds(this.grid.bounds); + this.dynamicGrid.build(); + } + + var contactList:Array = []; + var intersectionList:Array = []; + + public function sphereIntersection(spherecollision:SphereCollisionEntity, timeState:TimeState, contacts:Array) { var position = spherecollision.transform.getPosition(); var radius = spherecollision.radius; - // var velocity = spherecollision.velocity; - // var intersections = this.octree.radiusSearch(position, searchdist); var box = new Bounds(); box.addSpherePos(0, 0, 0, radius); @@ -44,84 +51,24 @@ 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); + this.intersectionList.resize(0); + this.grid.boundingSearch(box, this.intersectionList); + dynamicGrid.boundingSearch(box, this.intersectionList); - // 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) { - var entity:CollisionEntity = cast obj; - - foundEntities.push(entity); - if (entity.go.isCollideable) { - contacts = contacts.concat(entity.sphereIntersection(spherecollision, timeState)); - } - } - - // if (marbleEntities.length > 1) { - // marbleSap.recompute(); - // var sapCollisions = marbleSap.getIntersections(spherecollision); - // for (obj in sapCollisions) { - // if (obj.go.isCollideable) { - // contacts = contacts.concat(obj.sphereIntersection(spherecollision, timeState)); - // } - // } - // } - - // contacts = contacts.concat(this.staticWorld.sphereIntersection(spherecollision, timeState)); - - var dynSearch = dynamicOctree.boundingSearch(box); - for (obj in dynSearch) { + for (obj in this.intersectionList) { if (obj != spherecollision) { - var col = cast(obj, CollisionEntity); - if (col.boundingBox.collide(box) && col.go.isCollideable) - contacts = contacts.concat(col.sphereIntersection(spherecollision, timeState)); + var entity = obj; + + if (obj.boundingBox.collide(box) && entity.go.isCollideable) { + entity.sphereIntersection(spherecollision, timeState, contacts); + } } } - - // for (marb in marbleEntities) { - // if (marb != spherecollision) { - // if (spherecollision.go.isCollideable) { - // var isecs = marb.sphereIntersection(spherecollision, timeState); - // if (isecs.length > 0) - // foundEntities.push(marb); - // contacts = contacts.concat(isecs); - // } - // } - // } - return {foundEntities: foundEntities, contacts: contacts}; } - public function radiusSearch(center:Vector, radius:Float) { - var intersections = this.octree.radiusSearch(center, radius); - - var box = new Bounds(); - box.xMin = center.x - radius; - box.yMin = center.y - radius; - box.zMin = center.z - radius; - box.xMax = center.x + radius; - box.yMax = center.y + radius; - box.zMax = center.z + radius; - - var contacts:Array = []; - - for (obj in intersections) { - var entity:CollisionEntity = cast obj; - - contacts.push(entity); - } - - contacts = contacts.concat(dynamicOctree.boundingSearch(box, false).map(x -> cast(x, CollisionEntity))); - - return contacts; - } - - public function boundingSearch(bounds:Bounds, useCache:Bool = true) { - var contacts = this.octree.boundingSearch(bounds, useCache).map(x -> cast(x, CollisionEntity)); - contacts = contacts.concat(dynamicOctree.boundingSearch(bounds, useCache).map(x -> cast(x, CollisionEntity))); - return contacts; + public function boundingSearch(bounds:Bounds, contacts:Array, useCache:Bool = true) { + this.grid.boundingSearch(bounds, contacts); + dynamicGrid.boundingSearch(bounds, contacts); } public function rayCast(rayStart:Vector, rayDirection:Vector, rayLength:Float) { @@ -134,30 +81,33 @@ class CollisionWorld { + rayDirection.x * rayLength, rayStart.y + rayDirection.y * rayLength, rayStart.z + rayDirection.z * rayLength); - var objs = this.octree.boundingSearch(bounds); - var dynObjs = dynamicOctree.boundingSearch(bounds); + this.intersectionList.resize(0); + + this.grid.boundingSearch(bounds, this.intersectionList); + dynamicGrid.boundingSearch(bounds, this.intersectionList); + var results = []; - for (obj in objs) { - var oo = cast(obj, CollisionEntity); - oo.rayCast(rayStart, rayDirection, results); + for (obj in this.intersectionList) { + var oo = obj; + oo.rayCast(rayStart, rayDirection, results, rayLength); } - for (obj in dynObjs) { - var oo = cast(obj, CollisionEntity); - oo.rayCast(rayStart, rayDirection, results); - } - // results = results.concat(this.staticWorld.rayCast(rayStart, rayDirection)); return results; } public function addEntity(entity:CollisionEntity) { - this.octree.insert(entity); + this.grid.insert(entity); this.entities.push(entity); // this.rtree.insert([entity.boundingBox.xMin, entity.boundingBox.yMin, entity.boundingBox.zMin], // [entity.boundingBox.xSize, entity.boundingBox.ySize, entity.boundingBox.zSize], entity); } + public function removeEntity(entity:CollisionEntity) { + this.entities.remove(entity); + this.grid.remove(entity); + } + public function addMarbleEntity(entity:SphereCollisionEntity) { this.marbleEntities.push(entity); } @@ -168,21 +118,21 @@ class CollisionWorld { public function addMovingEntity(entity:CollisionEntity) { this.dynamicEntities.push(entity); - this.dynamicOctree.insert(entity); + this.dynamicGrid.insert(entity); this.dynamicEntitySet.set(entity, true); } public function removeMovingEntity(entity:CollisionEntity) { this.dynamicEntities.remove(entity); - this.dynamicOctree.remove(entity); + this.dynamicGrid.remove(entity); this.dynamicEntitySet.remove(entity); } public function updateTransform(entity:CollisionEntity) { if (!dynamicEntitySet.exists(entity)) { - this.octree.update(entity); + this.grid.update(entity); } else { - this.dynamicOctree.update(entity); + this.dynamicGrid.update(entity); } } @@ -204,10 +154,10 @@ class CollisionWorld { for (e in dynamicEntities) { e.dispose(); } - octree = null; + grid = null; entities = null; dynamicEntities = null; - dynamicOctree = null; + dynamicGrid = null; dynamicEntitySet = null; staticWorld.dispose(); staticWorld = null; diff --git a/src/collision/Grid.hx b/src/collision/Grid.hx index 75459b1c..40bf455d 100644 --- a/src/collision/Grid.hx +++ b/src/collision/Grid.hx @@ -3,6 +3,7 @@ package collision; import haxe.Exception; import h3d.Vector; import h3d.col.Bounds; +import src.Util; class Grid { public var bounds:Bounds; // The bounds of the grid @@ -74,7 +75,7 @@ class Grid { } // searchbox should be in LOCAL coordinates - public function boundingSearch(searchbox:Bounds) { + public function boundingSearch(searchbox:Bounds, foundSurfaces:Array) { var queryMinX = Math.max(searchbox.xMin, bounds.xMin); var queryMinY = Math.max(searchbox.yMin, bounds.yMin); var queryMaxX = Math.min(searchbox.xMax, bounds.xMax); @@ -93,8 +94,6 @@ class Grid { if (yEnd > CELL_SIZE) yEnd = CELL_SIZE; - var foundSurfaces = []; - searchKey++; // Insert the surface references from [xStart, yStart, zStart] to [xEnd, yEnd, zEnd] into the map @@ -112,8 +111,6 @@ class Grid { } } } - - return foundSurfaces; } function elegantPair(x:Int, y:Int) { @@ -124,31 +121,40 @@ class Grid { return elegantPair(elegantPair(x, y), z); } - public function rayCast(origin:Vector, direction:Vector) { + public function rayCast(origin:Vector, direction:Vector, bestT:Float) { var cell = origin.sub(this.bounds.getMin().toVector()); cell.x /= this.cellSize.x; cell.y /= this.cellSize.y; + var destCell = origin.add(direction.multiply(bestT)).sub(this.bounds.getMin().toVector()); + destCell.x /= this.cellSize.x; + destCell.y /= this.cellSize.y; var stepX, outX, X = Math.floor(cell.x); var stepY, outY, Y = Math.floor(cell.y); + var destX = Util.clamp(Math.max(Math.floor(destCell.x), 0), 0, CELL_DIV.x); + var destY = Util.clamp(Math.max(Math.floor(destCell.y), 0), 0, CELL_DIV.y); if ((X < 0) || (X >= CELL_DIV.x) || (Y < 0) || (Y >= CELL_DIV.y)) return []; var cb = new Vector(); if (direction.x > 0) { stepX = 1; - outX = CELL_DIV.x; + outX = destX; + if (outX == X) + outX = Math.min(CELL_DIV.x, outX + 1); cb.x = this.bounds.xMin + (X + 1) * this.cellSize.x; } else { stepX = -1; - outX = -1; + outX = destX - 1; cb.x = this.bounds.xMin + X * this.cellSize.x; } if (direction.y > 0.0) { stepY = 1; - outY = CELL_DIV.y; + outY = destY; + if (outY == Y) + outY = Math.min(CELL_DIV.y, outY + 1); cb.y = this.bounds.yMin + (Y + 1) * this.cellSize.y; } else { stepY = -1; - outY = -1; + outY = destY - 1; cb.y = this.bounds.yMin + Y * this.cellSize.y; } var tmax = new Vector(); @@ -175,7 +181,7 @@ class Grid { if (surf.key == searchKey) continue; surf.key = searchKey; - surf.rayCast(origin, direction, results); + bestT = surf.rayCast(origin, direction, results, bestT); } if (tmax.x < tmax.y) { X = X + stepX; diff --git a/src/collision/GridBroadphase.hx b/src/collision/GridBroadphase.hx new file mode 100644 index 00000000..1c776ef5 --- /dev/null +++ b/src/collision/GridBroadphase.hx @@ -0,0 +1,349 @@ +package collision; + +import haxe.Exception; +import h3d.Vector; +import h3d.col.Bounds; +import src.Util; + +@:publicFields +@:structInit +class GridBroadphaseProxy { + var index:Int; + var object:CollisionEntity; + var xMin:Int; + var xMax:Int; + var yMin:Int; + var yMax:Int; +} + +class GridBroadphase { + public var bounds:Bounds; // The bounds of the grid + + public var cellSize:Vector; // The dimensions of one cell + + static var CELL_SIZE = 16; + + public var CELL_DIV = new Vector(CELL_SIZE, CELL_SIZE); // split the bounds into cells of dimensions 1/16th of the corresponding dimensions of the bounds + + var cells:Array> = []; + + var objects:Array = []; + var objectToProxy:Map = []; + var searchKey:Int = 0; + + var _built = false; + var hasBounds:Bool = false; + + public function new() { + // this.bounds = bounds.clone(); + + // this.cellSize = new Vector(bounds.xSize / CELL_DIV.x, bounds.ySize / CELL_DIV.y); + for (i in 0...CELL_SIZE) { + for (j in 0...CELL_SIZE) { + this.cells.push([]); + } + } + } + + public function insert(object:CollisionEntity) { + if (!_built) { + var idx = this.objects.length; + this.objects.push({ + object: object, + xMin: 1000, + yMin: 1000, + xMax: -1000, + yMax: -1000, + index: idx, + }); + objectToProxy.set(object, this.objects[this.objects.length - 1]); + } else { + var idx = this.objects.length; + var proxy:GridBroadphaseProxy = { + object: object, + xMin: 1000, + yMin: 1000, + xMax: -1000, + yMax: -1000, + index: idx, + }; + this.objects.push(proxy); + objectToProxy.set(object, proxy); + + var queryMinX = Math.max(object.boundingBox.xMin, bounds.xMin); + var queryMinY = Math.max(object.boundingBox.yMin, bounds.yMin); + var queryMaxX = Math.min(object.boundingBox.xMax, bounds.xMax); + var queryMaxY = Math.min(object.boundingBox.yMax, bounds.yMax); + var xStart = Math.floor((queryMinX - bounds.xMin) / this.cellSize.x); + var yStart = Math.floor((queryMinY - bounds.yMin) / this.cellSize.y); + var xEnd = Math.ceil((queryMaxX - bounds.xMin) / this.cellSize.x); + var yEnd = Math.ceil((queryMaxY - bounds.yMin) / this.cellSize.y); + + for (i in xStart...xEnd) { + for (j in yStart...yEnd) { + this.cells[16 * i + j].push(idx); + proxy.xMin = Std.int(Math.min(proxy.xMin, i)); + proxy.yMin = Std.int(Math.min(proxy.yMin, j)); + proxy.xMax = Std.int(Math.max(proxy.xMax, i)); + proxy.yMax = Std.int(Math.max(proxy.yMax, j)); + } + } + } + } + + public function remove(object:CollisionEntity) { + var proxy = objectToProxy.get(object); + if (proxy == null) + return; + for (i in proxy.xMin...(proxy.xMax + 1)) { + for (j in proxy.yMin...(proxy.yMax + 1)) { + this.cells[16 * i + j].remove(proxy.index); + } + } + this.objects[proxy.index] = null; // Preserve indices pls + objectToProxy.remove(object); + } + + public function update(object:CollisionEntity) { + if (!_built) + return; + var queryMinX = Math.max(object.boundingBox.xMin, bounds.xMin); + var queryMinY = Math.max(object.boundingBox.yMin, bounds.yMin); + var queryMaxX = Math.min(object.boundingBox.xMax, bounds.xMax); + var queryMaxY = Math.min(object.boundingBox.yMax, bounds.yMax); + var xStart = Util.imax(0, Math.floor((queryMinX - bounds.xMin) / this.cellSize.x)); + var yStart = Util.imax(0, Math.floor((queryMinY - bounds.yMin) / this.cellSize.y)); + var xEnd = Util.imin(CELL_SIZE - 1, Math.floor((queryMaxX - bounds.xMin) / this.cellSize.x)); + var yEnd = Util.imin(CELL_SIZE - 1, Math.floor((queryMaxY - bounds.yMin) / this.cellSize.y)); + var proxy = objectToProxy.get(object); + if (proxy == null) { + insert(object); + } else { + // Update the cells + if (xStart != proxy.xMin || yStart != proxy.yMin || xEnd != proxy.xMax || yEnd != proxy.yMax) { + // Rebin the object + for (i in proxy.xMin...(proxy.xMax + 1)) { + for (j in proxy.yMin...(proxy.yMax + 1)) { + this.cells[16 * i + j].remove(proxy.index); + } + } + for (i in xStart...(xEnd + 1)) { + for (j in yStart...(yEnd + 1)) { + this.cells[16 * i + j].push(proxy.index); + } + } + proxy.xMin = xStart; + proxy.yMin = yStart; + proxy.xMax = xEnd; + proxy.yMax = yEnd; + } + } + } + + public function setBounds(bounds:Bounds) { + this.bounds = bounds.clone(); + this.cellSize = new Vector(bounds.xSize / CELL_DIV.x, bounds.ySize / CELL_DIV.y); + this.hasBounds = true; + } + + public function build() { + if (_built) + return; + _built = true; + // Find the bounds + if (!hasBounds) { + var xMin = 1e8; + var xMax = -1e8; + var yMin = 1e8; + var yMax = -1e8; + var zMin = 1e8; + var zMax = -1e8; + for (i in 0...this.objects.length) { + if (this.objects[i] == null) + continue; + var surface = this.objects[i].object; + xMin = Math.min(xMin, surface.boundingBox.xMin); + xMax = Math.max(xMax, surface.boundingBox.xMax); + yMin = Math.min(yMin, surface.boundingBox.yMin); + yMax = Math.max(yMax, surface.boundingBox.yMax); + zMin = Math.min(zMin, surface.boundingBox.zMin); + zMax = Math.max(zMax, surface.boundingBox.zMax); + } + // Some padding + xMin -= 100; + xMax += 100; + yMin -= 100; + yMax += 100; + zMin -= 100; + zMax += 100; + this.bounds = Bounds.fromValues(xMin, yMin, zMin, xMax - xMin, yMax - yMin, zMax - zMin); + this.cellSize = new Vector(this.bounds.xSize / CELL_DIV.x, this.bounds.ySize / CELL_DIV.y); + } + + // Insert the objects + for (i in 0...CELL_SIZE) { + var minX = this.bounds.xMin; + var maxX = this.bounds.xMin; + minX += i * this.cellSize.x; + maxX += (i + 1) * this.cellSize.x; + for (j in 0...CELL_SIZE) { + var minY = this.bounds.yMin; + var maxY = this.bounds.yMin; + minY += j * this.cellSize.y; + maxY += (j + 1) * this.cellSize.y; + + var binRect = new h2d.col.Bounds(); + binRect.xMin = minX; + binRect.yMin = minY; + binRect.xMax = maxX; + binRect.yMax = maxY; + + for (idx in 0...this.objects.length) { + if (this.objects[idx] == null) + continue; + var surface = this.objects[idx]; + var hullRect = new h2d.col.Bounds(); + hullRect.xMin = surface.object.boundingBox.xMin; + hullRect.yMin = surface.object.boundingBox.yMin; + hullRect.xMax = surface.object.boundingBox.xMax; + hullRect.yMax = surface.object.boundingBox.yMax; + + if (hullRect.intersects(binRect)) { + this.cells[16 * i + j].push(idx); + surface.xMin = Std.int(Math.min(surface.xMin, i)); + surface.yMin = Std.int(Math.min(surface.yMin, j)); + surface.xMax = Std.int(Math.max(surface.xMax, i)); + surface.yMax = Std.int(Math.max(surface.yMax, j)); + } + } + } + } + } + + // searchbox should be in LOCAL coordinates + public function boundingSearch(searchbox:Bounds, foundSurfaces:Array) { + var queryMinX = Math.max(searchbox.xMin, bounds.xMin); + var queryMinY = Math.max(searchbox.yMin, bounds.yMin); + var queryMaxX = Math.min(searchbox.xMax, bounds.xMax); + var queryMaxY = Math.min(searchbox.yMax, bounds.yMax); + var xStart = Math.floor((queryMinX - bounds.xMin) / this.cellSize.x); + var yStart = Math.floor((queryMinY - bounds.yMin) / this.cellSize.y); + var xEnd = Math.ceil((queryMaxX - bounds.xMin) / this.cellSize.x); + var yEnd = Math.ceil((queryMaxY - bounds.yMin) / this.cellSize.y); + + if (xStart < 0) + xStart = 0; + if (yStart < 0) + yStart = 0; + if (xEnd > CELL_SIZE) + xEnd = CELL_SIZE; + if (yEnd > CELL_SIZE) + yEnd = CELL_SIZE; + + searchKey++; + + // Insert the surface references from [xStart, yStart, zStart] to [xEnd, yEnd, zEnd] into the map + for (i in xStart...xEnd) { + for (j in yStart...yEnd) { + for (surfIdx in cells[16 * i + j]) { + var surf = objects[surfIdx].object; + if (surf.key == searchKey) + continue; + surf.key = searchKey; + if (searchbox.containsBounds(surf.boundingBox) || searchbox.collide(surf.boundingBox)) { + foundSurfaces.push(surf); + surf.key = searchKey; + } + } + } + } + + return foundSurfaces; + } + + function elegantPair(x:Int, y:Int) { + return (x >= y) ? (x * x + x + y) : (y * y + x); + } + + function hashVector(x:Int, y:Int, z:Int) { + return elegantPair(elegantPair(x, y), z); + } + + public function rayCast(origin:Vector, direction:Vector, bestT:Float) { + var cell = origin.sub(this.bounds.getMin().toVector()); + cell.x /= this.cellSize.x; + cell.y /= this.cellSize.y; + var destCell = origin.add(direction.multiply(bestT)).sub(this.bounds.getMin().toVector()); + destCell.x /= this.cellSize.x; + destCell.y /= this.cellSize.y; + var stepX, outX, X = Math.floor(cell.x); + var stepY, outY, Y = Math.floor(cell.y); + var destX = Util.clamp(Math.max(Math.floor(destCell.x), 0), 0, CELL_DIV.x); + var destY = Util.clamp(Math.max(Math.floor(destCell.y), 0), 0, CELL_DIV.y); + if ((X < 0) || (X >= CELL_DIV.x) || (Y < 0) || (Y >= CELL_DIV.y)) { + return []; + } + var cb = new Vector(); + if (direction.x > 0) { + stepX = 1; + outX = destX; + if (outX == X) + outX = Math.min(CELL_DIV.x, outX + 1); + cb.x = this.bounds.xMin + (X + 1) * this.cellSize.x; + } else { + stepX = -1; + outX = destX - 1; + cb.x = this.bounds.xMin + X * this.cellSize.x; + } + if (direction.y > 0.0) { + stepY = 1; + outY = destY; + if (outY == Y) + outY = Math.min(CELL_DIV.y, outY + 1); + cb.y = this.bounds.yMin + (Y + 1) * this.cellSize.y; + } else { + stepY = -1; + outY = destY - 1; + cb.y = this.bounds.yMin + Y * this.cellSize.y; + } + var tmax = new Vector(); + var tdelta = new Vector(); + var rxr, ryr, rzr; + if (direction.x != 0) { + rxr = 1.0 / direction.x; + tmax.x = (cb.x - origin.x) * rxr; + tdelta.x = this.cellSize.x * stepX * rxr; + } else + tmax.x = 1000000; + if (direction.y != 0) { + ryr = 1.0 / direction.y; + tmax.y = (cb.y - origin.y) * ryr; + tdelta.y = this.cellSize.y * stepY * ryr; + } else + tmax.y = 1000000; + searchKey++; + var results = []; + while (true) { + var cell = cells[16 * X + Y]; + for (idx in cell) { + var surf = objects[idx].object; + if (surf.key == searchKey) + continue; + surf.key = searchKey; + bestT = surf.rayCast(origin, direction, results, bestT); + } + if (tmax.x < tmax.y) { + X = X + stepX; + if (X == outX) + break; + tmax.x += tdelta.x; + } else { + Y = Y + stepY; + if (Y == outY) + break; + tmax.y += tdelta.y; + } + } + return results; + } +} diff --git a/src/collision/SphereCollisionEntity.hx b/src/collision/SphereCollisionEntity.hx index 5173d65b..a050396c 100644 --- a/src/collision/SphereCollisionEntity.hx +++ b/src/collision/SphereCollisionEntity.hx @@ -13,6 +13,7 @@ import src.Debug; class SphereCollisionEntity extends CollisionEntity { public var radius:Float; public var marble:Marble; + public var ignore:Bool = false; var _dbgEntity2:h3d.scene.Mesh; @@ -69,12 +70,14 @@ class SphereCollisionEntity extends CollisionEntity { } } - public override function rayCast(rayOrigin:Vector, rayDirection:Vector, results:Array) { + public override function rayCast(rayOrigin:Vector, rayDirection:Vector, results:Array, bestT:Float) { // TEMP cause bruh + return Math.POSITIVE_INFINITY; } - public override function sphereIntersection(collisionEntity:SphereCollisionEntity, timeState:TimeState) { - var contacts = []; + public override function sphereIntersection(collisionEntity:SphereCollisionEntity, timeState:TimeState, contacts:Array) { + if (ignore) + return; var thispos = transform.getPosition(); var position = collisionEntity.transform.getPosition(); var velocity = collisionEntity.velocity; @@ -89,10 +92,10 @@ class SphereCollisionEntity extends CollisionEntity { contact.collider = this; contact.friction = 1; contact.restitution = 1; - contact.velocity = this.velocity.clone(); + contact.velocity.load(this.velocity); contact.otherObject = this.go; - contact.point = position.add(normDist); - contact.normal = normDist.multiply(-1); + contact.point.load(position.add(normDist)); + contact.normal.load(normDist.multiply(-1)); contact.force = 0; contact.contactDistance = contact.point.distance(position); contacts.push(contact); @@ -109,6 +112,5 @@ class SphereCollisionEntity extends CollisionEntity { // othercontact.penetration = this.radius - (thispos.sub(othercontact.point).dot(othercontact.normal)); // this.marble.queueCollision(othercontact); } - return contacts; } } diff --git a/src/modes/HuntMode.hx b/src/modes/HuntMode.hx index 080f1478..eed4077a 100644 --- a/src/modes/HuntMode.hx +++ b/src/modes/HuntMode.hx @@ -84,7 +84,7 @@ class GemOctreeElem implements IOctreeObject { this.priority = priority; } - public function rayCast(rayOrigin:Vector, rayDirection:Vector, resultSet:Array) { + public function rayCast(rayOrigin:Vector, rayDirection:Vector, resultSet:Array, bestT:Float):Float { throw new haxe.exceptions.NotImplementedException(); // Not applicable } } diff --git a/src/octree/IOctreeObject.hx b/src/octree/IOctreeObject.hx index 4343b5c3..2376bc72 100644 --- a/src/octree/IOctreeObject.hx +++ b/src/octree/IOctreeObject.hx @@ -9,9 +9,10 @@ class RayIntersectionData { var point:Vector; var normal:Vector; var object:IOctreeObject; + var t:Float; } interface IOctreeObject extends IOctreeElement { var boundingBox:Bounds; - function rayCast(rayOrigin:Vector, rayDirection:Vector, resultSet:Array):Void; + function rayCast(rayOrigin:Vector, rayDirection:Vector, resultSet:Array, bestT:Float):Float; } diff --git a/src/octree/Octree.hx b/src/octree/Octree.hx index 7ad6152a..7adef030 100644 --- a/src/octree/Octree.hx +++ b/src/octree/Octree.hx @@ -152,9 +152,9 @@ class Octree { } /** Returns a list of all objects that intersect with the given ray, sorted by distance. */ - public function raycast(rayOrigin:Vector, rayDirection:Vector) { + public function raycast(rayOrigin:Vector, rayDirection:Vector, bestT:Float) { var intersections:Array = []; - this.root.raycast(rayOrigin, rayDirection, intersections); + this.root.raycast(rayOrigin, rayDirection, intersections, bestT); intersections.sort((a, b) -> (a.distance == b.distance) ? 0 : (a.distance > b.distance ? 1 : -1)); return intersections; } diff --git a/src/octree/OctreeNode.hx b/src/octree/OctreeNode.hx index 38f0f7fc..7de6f848 100644 --- a/src/octree/OctreeNode.hx +++ b/src/octree/OctreeNode.hx @@ -192,7 +192,7 @@ class OctreeNode implements IOctreeElement { return maxmin; } - public function raycast(rayOrigin:Vector, rayDirection:Vector, intersections:Array) { + public function raycast(rayOrigin:Vector, rayDirection:Vector, intersections:Array, bestT:Float) { var ray = Ray.fromValues(rayOrigin.x, rayOrigin.y, rayOrigin.z, rayDirection.x, rayDirection.y, rayDirection.z); // Construct the loose bounding box of this node (2x in size, with the regular bounding box in the center) @@ -201,7 +201,7 @@ class OctreeNode implements IOctreeElement { for (obj in this.objects) { var iSecs = []; - obj.rayCast(rayOrigin, rayDirection, iSecs); + obj.rayCast(rayOrigin, rayDirection, iSecs, bestT); for (intersection in iSecs) { var intersectionData = new OctreeIntersection(); intersectionData.distance = rayOrigin.distance(intersection.point); @@ -215,7 +215,7 @@ class OctreeNode implements IOctreeElement { if (this.octants != null) { for (i in 0...8) { var octant = this.octants[i]; - octant.raycast(rayOrigin, rayDirection, intersections); + octant.raycast(rayOrigin, rayDirection, intersections, bestT); } } }