mirror of
https://github.com/RandomityGuy/MBHaxe.git
synced 2025-12-24 08:52:43 +00:00
use different BVH implementation
This commit is contained in:
RandomityGuy
parent
92ec04a5b9
commit
a1e67dd7e9
2 changed files with 338 additions and 161 deletions
|
|
@ -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<CollisionSurface>;
|
||||
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<CollisionSurface> = [];
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue