From a1e67dd7e9e5adbf8a2411faf406b0dd99edd7a4 Mon Sep 17 00:00:00 2001 From: RandomityGuy <31925790+RandomityGuy@users.noreply.github.com> Date: Sun, 18 Dec 2022 23:09:28 +0530 Subject: [PATCH] use different BVH implementation --- src/collision/BVHTree.hx | 493 +++++++++++++++++++++---------- src/collision/CollisionEntity.hx | 6 +- 2 files changed, 338 insertions(+), 161 deletions(-) diff --git a/src/collision/BVHTree.hx b/src/collision/BVHTree.hx index 25652712..688cbff0 100644 --- a/src/collision/BVHTree.hx +++ b/src/collision/BVHTree.hx @@ -1,183 +1,360 @@ package collision; -import h3d.col.Bounds; import h3d.Vector; - -// https://github.com/Sopiro/DynamicBVH/blob/master/src/aabbtree.ts +import h3d.col.Bounds; @:publicFields class BVHNode { + var id:Int; + var parent:BVHNode; + var child1:BVHNode; + var child2:BVHNode; + var isLeaf:Bool; var bounds:Bounds; - var objects:Array; - var objectBounds:Bounds; // total bounds for objects stored in THIS node - var left:BVHNode; - var right:BVHNode; - var surfaceArea:Float; + var surface:CollisionSurface; - 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) { - // Pick best axis to split - switch (axis) { - 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)); - var rightObjects = objs.slice(Math.ceil(objs.length / 2)); - var leftAABB = new Bounds(); - var rightAABB = new Bounds(); - for (o in leftObjects) - leftAABB.add(o.obj.boundingBox); - for (o in rightObjects) - rightAABB.add(o.obj.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; - var splitCost = leftSA + rightSA; - var bestSplit = { - cost: splitCost, - left: leftObjects, - right: rightObjects, - leftBounds: leftAABB, - rightBounds: rightAABB, - axis: axis - }; - return bestSplit; - } - - public function split() { - // Splitting first time - // Calculate the centroids of all objects - var objs = objects.map(x -> { - x.generateBoundingBox(); - return {obj: x, centroid: x.boundingBox.getCenter()}; - }); - - // Find the best split cost - var costs = [getSplitCost(objs, 0), getSplitCost(objs, 1), getSplitCost(objs, 2)]; - 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 - if (leftObjs.length == 0 || rightObjs.length == 0) { - var thisobjs = leftObjs.concat(rightObjs).concat(intersectObjs); - this.objects = thisobjs; - this.objectBounds = new Bounds(); - for (o in thisobjs) - this.objectBounds.add(o.boundingBox); - return; - } - - // Make the child nodes - var leftBounds = new Bounds(); - var rightBounds = new Bounds(); - for (o in leftObjs) - leftBounds.add(o.boundingBox); - for (o in rightObjs) - rightBounds.add(o.boundingBox); - left = new BVHNode(leftBounds); - right = new BVHNode(rightBounds); - left.objects = leftObjs; - right.objects = rightObjs; - this.objects = intersectObjs; - this.objectBounds = new Bounds(); - for (o in intersectObjs) - this.objectBounds.add(o.boundingBox); - - left.split(); - right.split(); - } - - public function boundingSearch(searchbox:Bounds) { - if (this.bounds.containsBounds(searchbox) || this.bounds.collide(searchbox)) { - var intersects = []; - if (this.left != null && this.right != null) { - intersects = intersects.concat(this.left.boundingSearch(searchbox)); - intersects = intersects.concat(this.right.boundingSearch(searchbox)); - } - if (this.objectBounds.collide(searchbox) || this.objectBounds.containsBounds(searchbox)) { - for (o in this.objects) { - if (o.boundingBox.containsBounds(searchbox) || o.boundingBox.collide(searchbox)) - intersects.push(o); - } - } - return intersects; - } else { - return []; - } - } - - public function rayCast(origin:Vector, direction:Vector) { - var ray = h3d.col.Ray.fromValues(origin.x, origin.y, origin.z, direction.x, direction.y, direction.z); - if (ray.collide(this.bounds)) { - 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) { - if (ray.collide(o.boundingBox)) - intersects = intersects.concat(o.rayCast(origin, direction)); - } - } - return intersects; - } else { - return []; - } - } + public function new() {} } class BVHTree { - public var bounds:Bounds; - - var surfaces:Array = []; - + var nodeId:Int = 0; var root:BVHNode; - public function new(bounds:Bounds) { - this.bounds = bounds.clone(); + public function new() {} + + function update() { + var invalidNodes = []; + this.traverse(node -> { + if (node.isLeaf) { + var entity = node.surface; + var tightAABB = entity.boundingBox; + + if (node.bounds.containsBounds(tightAABB)) { + return; + } + + invalidNodes.push(node); + } + }); + for (node in invalidNodes) { + this.remove(node); + this.add(node.surface); + } } - public function insert(surf:CollisionSurface) { - surfaces.push(surf); + public function add(entity:CollisionSurface) { + // Enlarged AABB + var aabb = entity.boundingBox; + + var newNode = new BVHNode(); + newNode.id = this.nodeId++; + newNode.bounds = aabb; + newNode.surface = entity; + newNode.isLeaf = true; + + if (this.root == null) { + this.root = newNode; + return newNode; + } + + // Find the best sibling for the new leaf + var bestSibling = this.root; + var bestCostBox = this.root.bounds.clone(); + bestCostBox.add(aabb); + var bestCost = bestCostBox.xSize * bestCostBox.ySize + bestCostBox.xSize * bestCostBox.zSize + bestCostBox.ySize * bestCostBox.zSize; + var q = [{p1: this.root, p2: 0.0}]; + + while (q.length != 0) { + var front = q.shift(); + var current = front.p1; + var inheritedCost = front.p2; + + var combined = current.bounds.clone(); + combined.add(aabb); + var directCost = combined.xSize * combined.ySize + combined.xSize * combined.zSize + combined.ySize * combined.zSize; + + var costForCurrent = directCost + inheritedCost; + if (costForCurrent < bestCost) { + bestCost = costForCurrent; + bestSibling = current; + } + + inheritedCost += directCost + - (current.bounds.xSize * current.bounds.ySize + current.bounds.xSize * current.bounds.zSize + current.bounds.ySize * current.bounds.zSize); + + var aabbCost = aabb.xSize * aabb.ySize + aabb.xSize * aabb.zSize + aabb.ySize * aabb.zSize; + var lowerBoundCost = aabbCost + inheritedCost; + if (lowerBoundCost < bestCost) { + if (!current.isLeaf) { + if (current.child1 != null) + q.push({p1: current.child1, p2: inheritedCost}); + if (current.child2 != null) + q.push({p1: current.child2, p2: inheritedCost}); + } + } + } + + // Create a new parent + var oldParent = bestSibling.parent; + var newParent = new BVHNode(); + newParent.id = this.nodeId++; + newParent.parent = oldParent; + newParent.bounds = bestSibling.bounds.clone(); + newParent.bounds.add(aabb); + newParent.isLeaf = false; + + if (oldParent != null) { + if (oldParent.child1 == bestSibling) { + oldParent.child1 = newParent; + } else { + oldParent.child2 = newParent; + } + + newParent.child1 = bestSibling; + newParent.child2 = newNode; + bestSibling.parent = newParent; + newNode.parent = newParent; + } else { + newParent.child1 = bestSibling; + newParent.child2 = newNode; + bestSibling.parent = newParent; + newNode.parent = newParent; + this.root = newParent; + } + + // Walk back up the tree refitting ancestors' AABB and applying rotations + var ancestor = newNode.parent; + + while (ancestor != null) { + var child1 = ancestor.child1; + var child2 = ancestor.child2; + + ancestor.bounds = new Bounds(); + if (child1 != null) + ancestor.bounds.add(child1.bounds); + if (child2 != null) + ancestor.bounds.add(child2.bounds); + + this.rotate(ancestor); + + ancestor = ancestor.parent; + } + + return newNode; } - public function build() { - root = new BVHNode(bounds); - // Add all children - root.objects = this.surfaces; - root.split(); + function reset() { + this.nodeId = 0; + this.root = null; + } + + // BFS tree traversal + function traverse(callback:(node:BVHNode) -> Void) { + 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) { + 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; + } + } else { + this.root = sibling; + sibling.parent = null; + } + + var ancestor = sibling.parent; + while (ancestor != null) { + var child1 = ancestor.child1; + var child2 = ancestor.child2; + + ancestor.bounds = child1.bounds.clone(); + ancestor.bounds.add(child2.bounds); + ancestor = ancestor.parent; + } + } else { + if (this.root == node) { + this.root = null; + } + } + } + + function rotate(node:BVHNode) { + 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; + + var ch1 = sibling.bounds.clone(); + ch1.add(node.child1.bounds); + costDiffs.push(ch1.xSize * ch1.ySize + ch1.zSize * ch1.ySize + ch1.xSize * ch1.zSize - nodeArea); + var ch2 = sibling.bounds.clone(); + ch2.add(node.child2.bounds); + costDiffs.push(ch2.xSize * ch2.ySize + ch2.zSize * ch2.ySize + ch2.xSize * ch2.zSize - nodeArea); + + if (!sibling.isLeaf) { + var siblingArea = sibling.bounds.xSize * sibling.bounds.ySize + sibling.bounds.zSize * sibling.bounds.ySize + + sibling.bounds.xSize * sibling.bounds.zSize; + if (sibling.child1 != null) { + var ch3 = node.bounds.clone(); + ch3.add(sibling.child1.bounds); + costDiffs.push(ch3.xSize * ch3.ySize + ch3.zSize * ch3.ySize + ch3.xSize * ch3.zSize - siblingArea); + } + if (sibling.child2 != null) { + var ch4 = node.bounds.clone(); + ch4.add(sibling.child2.bounds); + costDiffs.push(ch4.xSize * ch4.ySize + ch4.zSize * ch4.ySize + ch4.xSize * ch4.zSize - siblingArea); + } + } + + var bestDiffIndex = 0; + for (i in 1...costDiffs.length) { + if (costDiffs[i] < costDiffs[bestDiffIndex]) { + bestDiffIndex = i; + } + } + + if (costDiffs[bestDiffIndex] < 0.0) { + switch (bestDiffIndex) { + case 0: + if (parent.child1 == sibling) { + parent.child1 = node.child2; + } else { + parent.child2 = node.child2; + } + + if (node.child2 != null) { + node.child2.parent = parent; + } + + node.child2 = sibling; + sibling.parent = node; + node.bounds = sibling.bounds.clone(); + if (node.child1 != null) { + node.bounds.add(node.child1.bounds); + } + case 1: + if (parent.child1 == sibling) { + parent.child1 = node.child1; + } else { + parent.child2 = node.child1; + } + if (node.child1 != null) { + node.child1.parent = parent; + } + node.child1 = sibling; + sibling.parent = node; + node.bounds = sibling.bounds.clone(); + if (node.child2 != null) { + node.bounds.add(node.child2.bounds); + } + case 2: + if (parent.child1 == node) { + parent.child1 = sibling.child2; + } else { + parent.child2 = sibling.child2; + } + if (sibling.child2 != null) { + sibling.child2.parent = parent; + } + sibling.child2 = node; + node.parent = sibling; + sibling.bounds = node.bounds.clone(); + if (sibling.child2 != null) { + sibling.bounds.add(sibling.child2.bounds); + } + + case 3: + if (parent.child1 == node) { + parent.child1 = sibling.child1; + } else { + parent.child2 = sibling.child1; + } + if (sibling.child1 != null) { + sibling.child1.parent = parent; + } + sibling.child1 = node; + node.parent = sibling; + sibling.bounds = node.bounds.clone(); + if (sibling.child1 != null) { + sibling.bounds.add(sibling.child1.bounds); + } + } + } } 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.surface); + } 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.surface.rayCast(origin, direction)); + } else { + if (current.child1 != null) + q.push(current.child1); + if (current.child2 != null) + q.push(current.child2); + } + } + } + return res; } } diff --git a/src/collision/CollisionEntity.hx b/src/collision/CollisionEntity.hx index fcb52ec4..ce453624 100644 --- a/src/collision/CollisionEntity.hx +++ b/src/collision/CollisionEntity.hx @@ -55,11 +55,11 @@ class CollisionEntity implements IOctreeObject { // Generates the bvh public function finalize() { this.generateBoundingBox(); - this.bvh = new BVHTree(this.boundingBox); + this.bvh = new BVHTree(); for (surface in this.surfaces) { - this.bvh.insert(surface); + this.bvh.add(surface); } - this.bvh.build(); + // this.bvh.build(); } public function setTransform(transform:Matrix) {