diff --git a/src/collision/BVHTree.hx b/src/collision/BVHTree.hx index 39f7574b..2e7b09e4 100644 --- a/src/collision/BVHTree.hx +++ b/src/collision/BVHTree.hx @@ -5,6 +5,7 @@ import h3d.col.Bounds; interface IBVHObject { var boundingBox:Bounds; + var key:Int; function rayCast(rayOrigin:Vector, rayDirection:Vector):Array; } diff --git a/src/collision/CollisionEntity.hx b/src/collision/CollisionEntity.hx index ac4ae707..61959b28 100644 --- a/src/collision/CollisionEntity.hx +++ b/src/collision/CollisionEntity.hx @@ -25,8 +25,9 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { public var bvh:BVHTree; - public var surfaces:Array; + var grid:Grid; + public var surfaces:Array; public var priority:Int; public var position:Int; public var velocity:Vector = new Vector(); @@ -44,6 +45,8 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { var _transformKey:Int = 0; + public var key:Int = 0; + var _dbgEntity:h3d.scene.Mesh; public function new(go:GameObject) { @@ -70,6 +73,13 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { this.bvh.add(surface); } #end + var bbox = new Bounds(); + for (surface in this.surfaces) + bbox.add(surface.boundingBox); + this.grid = new Grid(bbox); + for (surface in this.surfaces) + this.grid.insert(surface); + this.grid.build(); // this.bvh.build(); } @@ -138,26 +148,26 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { var rStart = rayOrigin.clone(); rStart.transform(invMatrix); var rDir = rayDirection.transformed3x3(invMatrix); - if (bvh == null) { - var intersections = octree.raycast(rStart, rDir); - var iData:Array = []; - for (i in intersections) { - i.point.transform(transform); - i.normal.transform3x3(invTPos); - i.normal.normalize(); - iData.push({point: i.point, normal: i.normal, object: i.object}); - } - return iData; - } else { - var intersections = this.bvh.rayCast(rStart, rDir); - for (i in intersections) { - i.point.transform(transform); - i.normal.transform3x3(invTPos); - i.normal.normalize(); - } - - return intersections; + // if (bvh == null) { + // var intersections = grid.rayCast(rStart, rDir); // octree.raycast(rStart, rDir); + // // var iData:Array = []; + // for (i in intersections) { + // i.point.transform(transform); + // i.normal.transform3x3(invTPos); + // i.normal.normalize(); + // // iData.push({point: i.point, normal: i.normal, object: i.object}); + // } + // return intersections; // iData; + // } else { + var intersections = grid.rayCast(rStart, rDir); // this.bvh.rayCast(rStart, rDir); + for (i in intersections) { + i.point.transform(transform); + i.normal.transform3x3(invTPos); + i.normal.normalize(); } + + return intersections; + // } } public function getElementType() { @@ -183,7 +193,7 @@ 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 = bvh == null ? octree.boundingSearch(sphereBounds).map(x -> cast x) : bvh.boundingSearch(sphereBounds); + var surfaces = grid.boundingSearch(sphereBounds); // bvh == null ? octree.boundingSearch(sphereBounds).map(x -> cast x) : bvh.boundingSearch(sphereBounds); var invtform = invMatrix.clone(); invtform.transpose(); @@ -243,11 +253,11 @@ class CollisionEntity implements IOctreeObject implements IBVHObject { // bestDot = testDot; var cinfo = CollisionPool.alloc(); - cinfo.normal = normal.clone(); - cinfo.point = closest.clone(); + cinfo.normal.load(normal); + cinfo.point.load(closest); cinfo.collider = null; // cinfo.collider = this; - cinfo.velocity = this.velocity.clone(); + cinfo.velocity.load(this.velocity); cinfo.contactDistance = Math.sqrt(contactDist); cinfo.otherObject = this.go; // cinfo.penetration = radius - (position.sub(closest).dot(normal)); diff --git a/src/collision/CollisionInfo.hx b/src/collision/CollisionInfo.hx index 20a0ba3a..0a82d60b 100644 --- a/src/collision/CollisionInfo.hx +++ b/src/collision/CollisionInfo.hx @@ -4,9 +4,9 @@ import src.GameObject; import h3d.Vector; class CollisionInfo { - public var point:Vector; - public var normal:Vector; - public var velocity:Vector; + public var point:Vector = new Vector(); + public var normal:Vector = new Vector(); + public var velocity:Vector = new Vector(); public var collider:CollisionEntity; public var otherObject:GameObject; public var friction:Float; diff --git a/src/collision/CollisionSurface.hx b/src/collision/CollisionSurface.hx index 52d963b8..5a0557cc 100644 --- a/src/collision/CollisionSurface.hx +++ b/src/collision/CollisionSurface.hx @@ -50,6 +50,7 @@ class CollisionSurface implements IOctreeObject implements IBVHObject { public var originalIndices:Array; public var originalSurfaceIndex:Int; public var transformKeys:Array; + public var key:Int = 0; var _transformedPoints:Array; var _transformedNormals:Array; @@ -139,7 +140,7 @@ class CollisionSurface implements IOctreeObject implements IBVHObject { } public function rayCast(rayOrigin:Vector, rayDirection:Vector):Array { - var intersections = []; + var intersections:Array = []; var i = 0; while (i < indices.length) { var p1 = getPoint(indices[i]); diff --git a/src/collision/Grid.hx b/src/collision/Grid.hx index cd713ec8..800e2b82 100644 --- a/src/collision/Grid.hx +++ b/src/collision/Grid.hx @@ -9,16 +9,24 @@ class Grid { public var cellSize:Vector; // The dimensions of one cell - public var CELL_DIV = new Vector(12, 12, 12); // split the bounds into cells of dimensions 1/16th of the corresponding dimensions of the bounds + static var CELL_SIZE = 16; - var map:Map> = new Map(); + 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 surfaces:Array = []; + var searchKey:Int = 0; public function new(bounds:Bounds) { this.bounds = bounds.clone(); - this.cellSize = new Vector(bounds.xSize / CELL_DIV.x, bounds.ySize / CELL_DIV.y, bounds.zSize / CELL_DIV.z); + 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(surface:CollisionSurface) { @@ -26,61 +34,80 @@ class Grid { if (this.bounds.containsBounds(surface.boundingBox)) { var idx = this.surfaces.length; this.surfaces.push(surface); - - var xStart = Math.floor((surface.boundingBox.xMin - bounds.xMin) / this.cellSize.x); - var yStart = Math.floor((surface.boundingBox.yMin - bounds.yMin) / this.cellSize.y); - var zStart = Math.floor((surface.boundingBox.zMin - bounds.zMin) / this.cellSize.z); - var xEnd = Math.ceil((surface.boundingBox.xMax - bounds.xMin) / this.cellSize.x) + 1; - var yEnd = Math.ceil((surface.boundingBox.yMax - bounds.yMin) / this.cellSize.y) + 1; - var zEnd = Math.ceil((surface.boundingBox.zMax - bounds.zMin) / this.cellSize.z) + 1; - - // 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 (k in zStart...zEnd) { - var hash = hashVector(i, j, k); - if (!this.map.exists(hash)) { - this.map.set(hash, []); - } - this.map.get(hash).push(idx); - } - } - } } else { throw new Exception("Surface is not contained in the grid's bounds"); } } + public function build() { + 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.surfaces.length) { + var surface = this.surfaces[idx]; + var hullRect = new h2d.col.Bounds(); + hullRect.xMin = surface.boundingBox.xMin; + hullRect.yMin = surface.boundingBox.yMin; + hullRect.xMax = surface.boundingBox.xMax; + hullRect.yMax = surface.boundingBox.yMax; + + if (hullRect.intersects(binRect)) { + this.cells[16 * i + j].push(idx); + } + } + } + } + } + // searchbox should be in LOCAL coordinates public function boundingSearch(searchbox:Bounds) { - var xStart = Math.floor((searchbox.xMin - bounds.xMin) / this.cellSize.x); - var yStart = Math.floor((searchbox.yMin - bounds.yMin) / this.cellSize.y); - var zStart = Math.floor((searchbox.zMin - bounds.zMin) / this.cellSize.z); - var xEnd = Math.ceil((searchbox.xMax - bounds.xMin) / this.cellSize.x) + 1; - var yEnd = Math.ceil((searchbox.yMax - bounds.yMin) / this.cellSize.y) + 1; - var zEnd = Math.ceil((searchbox.zMax - bounds.zMin) / this.cellSize.z) + 1; + 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; var foundSurfaces = []; - for (surf in this.surfaces) { - surf.key = false; - } + 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 (k in zStart...zEnd) { - var hash = hashVector(i, j, k); - if (this.map.exists(hash)) { - var surfs = this.map.get(hash); - for (surf in surfs) { - if (surfaces[surf].key) - continue; - if (searchbox.containsBounds(surfaces[surf].boundingBox) || searchbox.collide(surfaces[surf].boundingBox)) { - foundSurfaces.push(surfaces[surf]); - surfaces[surf].key = true; - } - } + for (surfIdx in cells[16 * i + j]) { + var surf = surfaces[surfIdx]; + if (surf.key == searchKey) + continue; + surf.key = searchKey; + if (searchbox.containsBounds(surf.boundingBox) || searchbox.collide(surf.boundingBox)) { + foundSurfaces.push(surf); + surf.key = 1; } } } @@ -101,48 +128,31 @@ class Grid { var cell = origin.sub(this.bounds.getMin().toVector()); cell.x /= this.cellSize.x; cell.y /= this.cellSize.y; - cell.z /= this.cellSize.z; - var stepX, outX, X = Math.floor(cell.x); var stepY, outY, Y = Math.floor(cell.y); - var stepZ, outZ, Z = Math.floor(cell.z); - - if ((X < 0) || (X >= CELL_DIV.x) || (Y < 0) || (Y >= CELL_DIV.y) || (Z < 0) || (Z >= CELL_DIV.z)) + 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; - cb.x = this.bounds.getMin().x + (X + 1) * this.cellSize.x; + cb.x = this.bounds.xMin + (X + 1) * this.cellSize.x; } else { stepX = -1; outX = -1; - cb.x = this.bounds.getMin().x + X * this.cellSize.x; + cb.x = this.bounds.xMin + X * this.cellSize.x; } if (direction.y > 0.0) { stepY = 1; outY = CELL_DIV.y; - cb.y = this.bounds.getMin().y + (Y + 1) * this.cellSize.y; + cb.y = this.bounds.yMin + (Y + 1) * this.cellSize.y; } else { stepY = -1; outY = -1; - cb.y = this.bounds.getMin().y + Y * this.cellSize.y; + cb.y = this.bounds.yMin + Y * this.cellSize.y; } - if (direction.z > 0.0) { - stepZ = 1; - outZ = CELL_DIV.z; - cb.z = this.bounds.getMin().z + (Z + 1) * this.cellSize.z; - } else { - stepZ = -1; - outZ = -1; - cb.z = this.bounds.getMin().z + Z * this.cellSize.z; - } - var tmax = new Vector(); var tdelta = new Vector(); - var rxr, ryr, rzr; if (direction.x != 0) { rxr = 1.0 / direction.x; @@ -156,58 +166,29 @@ class Grid { tdelta.y = this.cellSize.y * stepY * ryr; } else tmax.y = 1000000; - if (direction.z != 0) { - rzr = 1.0 / direction.z; - tmax.z = (cb.z - origin.z) * rzr; - tdelta.z = this.cellSize.z * stepZ * rzr; - } else - tmax.z = 1000000; - - for (surf in this.surfaces) { - surf.key = false; - } - + searchKey++; var results = []; - while (true) { - var hash = hashVector(X, Y, Z); - if (this.map.exists(hash)) { - var currentSurfaces = this.map.get(hash).map(x -> this.surfaces[x]); - - for (surf in currentSurfaces) { - if (surf.key) - continue; - results = results.concat(surf.rayCast(origin, direction)); - surf.key = true; - } + var cell = cells[16 * X + Y]; + for (idx in cell) { + var surf = surfaces[idx]; + if (surf.key == searchKey) + continue; + surf.key = searchKey; + results = results.concat(surf.rayCast(origin, direction)); } if (tmax.x < tmax.y) { - if (tmax.x < tmax.z) { - X = X + stepX; - if (X == outX) - break; - tmax.x += tdelta.x; - } else { - Z = Z + stepZ; - if (Z == outZ) - break; - tmax.z += tdelta.z; - } + X = X + stepX; + if (X == outX) + break; + tmax.x += tdelta.x; } else { - if (tmax.y < tmax.z) { - Y = Y + stepY; - if (Y == outY) - break; - tmax.y += tdelta.y; - } else { - Z = Z + stepZ; - if (Z == outZ) - break; - tmax.z += tdelta.z; - } + Y = Y + stepY; + if (Y == outY) + break; + tmax.y += tdelta.y; } } - return results; } } diff --git a/src/collision/KDTree.hx b/src/collision/KDTree.hx new file mode 100644 index 00000000..5d760e58 --- /dev/null +++ b/src/collision/KDTree.hx @@ -0,0 +1,223 @@ +package collision; + +import h3d.col.Point; +import h3d.col.Bounds; +import h3d.Vector; + +@:publicFields +class KDTreeNode { + var axis:Int; + var d:Float; + var data:Array = []; + var leftIndex:Int; + var rightIndex:Int; + + public function new() {} +} + +class KDTree { + public var boxesPerBin = 8; + public var maxDepth = 10; + + var elements:Array; + + var nodes:Array = []; + + static var searchArray:Array = []; + static var searchArraySize = 0; + static var searchKey = 0; + + public function new() { + elements = []; + } + + public inline function add(element:CollisionSurface) { + elements.push(element); + } + + public function build() { + nodes = []; + addNodes(elements, 0, elements.length, 0); + for (i in 0...elements.length) { + var element = elements[i]; + var insNodes = boundingSearchForLeaves(element.boundingBox); + for (node in insNodes) { + node.data.push(i); + } + } + } + + public function boundingSearch(searchbox:Bounds) { + var res = []; + if (nodes.length == 0) + return res; + searchArraySize = 1; + if (searchArray.length < searchArraySize) + searchArray.push(0); + searchArray[0] = 0; + searchKey += 1; + var arr = [0]; + while (arr.length != 0) { + var idx = arr.pop(); // searchArray[searchArraySize - 1]; + searchArraySize--; + var node = nodes[idx]; + if (node.leftIndex == node.rightIndex) { + for (x in node.data) { + if (elements[x].key != searchKey) { + elements[x].key = searchKey; + res.push(elements[x]); + } + } + } else { + var minVal = getValuePt(searchbox.getMin(), node.axis); + var maxVal = getValuePt(searchbox.getMax(), node.axis); + if (minVal <= node.d) + arr.push(node.leftIndex); + // pushToSearchArray(node.leftIndex); + if (maxVal >= node.d) + arr.push(node.rightIndex); + // pushToSearchArray(node.rightIndex); + } + } + return res; + } + + public inline function pushToSearchArray(i:Int) { + searchArraySize++; + while (searchArray.length < searchArraySize) + searchArray.push(0); + searchArray[searchArraySize - 1] = i; + } + + function boundingSearchForLeaves(searchbox:Bounds) { + var res = []; + if (nodes.length == 0) + return res; + var stack = [0]; + while (stack.length != 0) { + var idx = stack.pop(); + var node = nodes[idx]; + if (node.leftIndex == node.rightIndex) { + res.push(node); + } else { + var minVal = getValuePt(searchbox.getMin(), node.axis); + var maxVal = getValuePt(searchbox.getMax(), node.axis); + if (minVal <= node.d) + stack.push(node.leftIndex); + if (maxVal >= node.d) + stack.push(node.rightIndex); + } + } + return res; + } + + function addNodes(boxes:Array, start:Int, end:Int, depth:Int) { + var node = new KDTreeNode(); + if (end - start < this.boxesPerBin || depth == maxDepth) { + node.axis = -1; + node.leftIndex = -1; + node.rightIndex = -1; + nodes.push(node); + return nodes.length - 1; + } + var ret = nodes.length; + nodes.push(node); + sortOnMinAxis(boxes, start, end, depth % 3); + var minSplitIndex = start + end >> 1; + var minSplitVal = 0.5 * (getMinValue(boxes[minSplitIndex], depth % 3) + getMinValue(boxes[minSplitIndex + 1], depth % 3)); + sortOnMaxAxis(boxes, start, end, depth % 3); + var maxSplitIndex = start + end >> 1; + var maxSplitVal = 0.5 * (getMaxValue(boxes[maxSplitIndex], depth % 3) + getMaxValue(boxes[maxSplitIndex + 1], depth % 3)); + var splitVal = 0.5 * (minSplitVal + maxSplitVal); + var splitIndex = start; + while (splitIndex < end && getMaxValue(boxes[splitIndex], depth % 3) <= splitVal) { + splitIndex++; + } + node.rightIndex = addNodes(boxes, splitIndex, end, depth + 1); + sortOnMinAxis(boxes, start, end, depth % 3); + splitIndex = start; + while (splitIndex < end && getMinValue(boxes[splitIndex], depth % 3) <= splitVal) { + splitIndex++; + } + node.leftIndex = addNodes(boxes, start, splitIndex, depth + 1); + node.axis = depth % 3; + node.d = splitVal; + nodes[ret] = node; + return ret; + } + + public inline function getValue(pt:Vector, axis:Int) { + if (axis == 0) + return pt.x; + else if (axis == 1) + return pt.y; + else + return pt.z; + } + + public inline function getValuePt(pt:Point, axis:Int) { + if (axis == 0) + return pt.x; + else if (axis == 1) + return pt.y; + else + return pt.z; + } + + public inline function getMinValue(element:CollisionSurface, axis:Int) { + if (axis == 0) + return element.boundingBox.xMin; + else if (axis == 1) + return element.boundingBox.yMin; + else + return element.boundingBox.zMin; + } + + public inline function getMaxValue(element:CollisionSurface, axis:Int) { + if (axis == 0) + return element.boundingBox.xMax; + else if (axis == 1) + return element.boundingBox.yMax; + else + return element.boundingBox.zMax; + } + + public inline function sortOnAxis(points:Array, start:Int, end:Int, axis:Int) { + var slice = points.slice(start, end); + slice.sort(function(a:Vector, b:Vector) { + if (axis == 0) + return (a.x > b.x) ? 1 : (a.x < b.x) ? -1 : 0; + else if (axis == 1) + return (a.y > b.y) ? 1 : (a.y < b.y) ? -1 : 0; + else + return (a.z > b.z) ? 1 : (a.z < b.z) ? -1 : 0; + }); + return points.slice(start, end).concat(slice).concat(points.slice(end)); + } + + public inline function sortOnMinAxis(elements:Array, start:Int, end:Int, axis:Int) { + var slice = elements.slice(start, end); + slice.sort(function(a:CollisionSurface, b:CollisionSurface) { + if (axis == 0) + return (a.boundingBox.xMin > b.boundingBox.xMin) ? 1 : (a.boundingBox.xMin < b.boundingBox.xMin) ? -1 : 0; + else if (axis == 1) + return (a.boundingBox.yMin > b.boundingBox.yMin) ? 1 : (a.boundingBox.yMin < b.boundingBox.yMin) ? -1 : 0; + else + return (a.boundingBox.zMin > b.boundingBox.zMin) ? 1 : (a.boundingBox.zMin < b.boundingBox.zMin) ? -1 : 0; + }); + return elements.slice(start, end).concat(slice).concat(elements.slice(end)); + } + + public inline function sortOnMaxAxis(elements:Array, start:Int, end:Int, axis:Int) { + var slice = elements.slice(start, end); + slice.sort(function(a:CollisionSurface, b:CollisionSurface) { + if (axis == 0) + return (a.boundingBox.xMax > b.boundingBox.xMax) ? 1 : (a.boundingBox.xMax < b.boundingBox.xMax) ? -1 : 0; + else if (axis == 1) + return (a.boundingBox.yMax > b.boundingBox.yMax) ? 1 : (a.boundingBox.yMax < b.boundingBox.yMax) ? -1 : 0; + else + return (a.boundingBox.zMax > b.boundingBox.zMax) ? 1 : (a.boundingBox.zMax < b.boundingBox.zMax) ? -1 : 0; + }); + return elements.slice(start, end).concat(slice).concat(elements.slice(end)); + } +}