thirdpartymirrors/MBHaxe
1
0
Fork 0
mirror of https://github.com/RandomityGuy/MBHaxe.git synced 2025-12-23 16:32:49 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

use grid for faster collision detection

Browse source
This commit is contained in:
RandomityGuy 2024-03-20 18:41:31 +05:30
parent a108e01873
commit aa411159d1
6 changed files with 355 additions and 139 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
src/collision/BVHTree.hx
View file

@ -5,6 +5,7 @@ import h3d.col.Bounds;
interface IBVHObject {
var boundingBox:Bounds;
var key:Int;
function rayCast(rayOrigin:Vector, rayDirection:Vector):Array<octree.IOctreeObject.RayIntersectionData>;
}

58
src/collision/CollisionEntity.hx
View file

@ -25,8 +25,9 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
public var bvh:BVHTree<CollisionSurface>;
public var surfaces:Array<CollisionSurface>;
var grid:Grid;
public var surfaces:Array<CollisionSurface>;
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<RayIntersectionData> = [];
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<RayIntersectionData> = [];
// 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));

6
src/collision/CollisionInfo.hx
View file

@ -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;

3
src/collision/CollisionSurface.hx
View file

@ -50,6 +50,7 @@ class CollisionSurface implements IOctreeObject implements IBVHObject {
public var originalIndices:Array<Int>;
public var originalSurfaceIndex:Int;
public var transformKeys:Array<Int>;
public var key:Int = 0;
var _transformedPoints:Array<Float>;
var _transformedNormals:Array<Float>;
@ -139,7 +140,7 @@ class CollisionSurface implements IOctreeObject implements IBVHObject {
}
public function rayCast(rayOrigin:Vector, rayDirection:Vector):Array<RayIntersectionData> {
var intersections = [];
var intersections:Array<RayIntersectionData> = [];
var i = 0;
while (i < indices.length) {
var p1 = getPoint(indices[i]);

203
src/collision/Grid.hx
View file

@ -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<Int, Array<Int>> = 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<Array<Int>> = [];
var surfaces:Array<CollisionSurface> = [];
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;
}
}

223
src/collision/KDTree.hx Normal file
View file

@ -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<Int> = [];
var leftIndex:Int;
var rightIndex:Int;
public function new() {}
}
class KDTree {
public var boxesPerBin = 8;
public var maxDepth = 10;
var elements:Array<CollisionSurface>;
var nodes:Array<KDTreeNode> = [];
static var searchArray:Array<Int> = [];
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<CollisionSurface>, 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<Vector>, 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<CollisionSurface>, 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<CollisionSurface>, 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));
}
}