thirdpartymirrors/MBHaxe
1
0
Fork 0
mirror of https://github.com/RandomityGuy/MBHaxe.git synced 2025-10-30 08:11:25 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

collision optimization, reduce heap allocs

Browse source
This commit is contained in:
RandomityGuy 2024-01-31 23:50:14 +05:30
parent 4484a67057
commit d833ac8a2b
5 changed files with 218 additions and 187 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

240
src/collision/BVHTree.hx
View file

@ -10,10 +10,10 @@ interface IBVHObject {
@:publicFields @:publicFields
class BVHNode<T:IBVHObject> { class BVHNode<T:IBVHObject> {
var id:Int; var index:Int;
var parent:BVHNode<T>; var parent:Int = -1;
var child1:BVHNode<T>; var child1:Int = -1;
var child2:BVHNode<T>; var child2:Int = -1;
var isLeaf:Bool; var isLeaf:Bool;
var bounds:Bounds; var bounds:Bounds;
var object:T; var object:T;
@ -22,11 +22,19 @@ class BVHNode<T:IBVHObject> {
} }
class BVHTree<T:IBVHObject> { class BVHTree<T:IBVHObject> {
var nodeId:Int = 0;
var root:BVHNode<T>; var root:BVHNode<T>;
var nodes:Array<BVHNode<T>> = [];
public function new() {} public function new() {}
public function allocateNode():BVHNode<T> {
var node = new BVHNode<T>();
var index = this.nodes.length;
node.index = index;
this.nodes.push(node);
return node;
}
public function update() { public function update() {
var invalidNodes = []; var invalidNodes = [];
this.traverse(node -> { this.traverse(node -> {
@ -51,8 +59,7 @@ class BVHTree<T:IBVHObject> {
// Enlarged AABB // Enlarged AABB
var aabb = entity.boundingBox; var aabb = entity.boundingBox;
var newNode = new BVHNode(); var newNode = allocateNode();
newNode.id = this.nodeId++;
newNode.bounds = aabb; newNode.bounds = aabb;
newNode.object = entity; newNode.object = entity;
newNode.isLeaf = true; newNode.isLeaf = true;
@ -67,11 +74,11 @@ class BVHTree<T:IBVHObject> {
var bestCostBox = this.root.bounds.clone(); var bestCostBox = this.root.bounds.clone();
bestCostBox.add(aabb); bestCostBox.add(aabb);
var bestCost = bestCostBox.xSize * bestCostBox.ySize + bestCostBox.xSize * bestCostBox.zSize + bestCostBox.ySize * bestCostBox.zSize; var bestCost = bestCostBox.xSize * bestCostBox.ySize + bestCostBox.xSize * bestCostBox.zSize + bestCostBox.ySize * bestCostBox.zSize;
var q = [{p1: this.root, p2: 0.0}]; var q = [{p1: this.root.index, p2: 0.0}];
while (q.length != 0) { while (q.length != 0) {
var front = q.shift(); var front = q.shift();
var current = front.p1; var current = nodes[front.p1];
var inheritedCost = front.p2; var inheritedCost = front.p2;
var combined = current.bounds.clone(); var combined = current.bounds.clone();
@ -91,115 +98,116 @@ class BVHTree<T:IBVHObject> {
var lowerBoundCost = aabbCost + inheritedCost; var lowerBoundCost = aabbCost + inheritedCost;
if (lowerBoundCost < bestCost) { if (lowerBoundCost < bestCost) {
if (!current.isLeaf) { if (!current.isLeaf) {
if (current.child1 != null) if (current.child1 != -1)
q.push({p1: current.child1, p2: inheritedCost}); q.push({p1: current.child1, p2: inheritedCost});
if (current.child2 != null) if (current.child2 != -1)
q.push({p1: current.child2, p2: inheritedCost}); q.push({p1: current.child2, p2: inheritedCost});
} }
} }
} }
// Create a new parent // Create a new parent
var oldParent = bestSibling.parent; var oldParent = bestSibling.parent != -1 ? nodes[bestSibling.parent] : null;
var newParent = new BVHNode(); var newParent = allocateNode();
newParent.id = this.nodeId++; newParent.parent = oldParent != null ? oldParent.index : -1;
newParent.parent = oldParent;
newParent.bounds = bestSibling.bounds.clone(); newParent.bounds = bestSibling.bounds.clone();
newParent.bounds.add(aabb); newParent.bounds.add(aabb);
newParent.isLeaf = false; newParent.isLeaf = false;
if (oldParent != null) { if (oldParent != null) {
if (oldParent.child1 == bestSibling) { if (oldParent.child1 == bestSibling.index) {
oldParent.child1 = newParent; oldParent.child1 = newParent.index;
} else { } else {
oldParent.child2 = newParent; oldParent.child2 = newParent.index;
} }
newParent.child1 = bestSibling; newParent.child1 = bestSibling.index;
newParent.child2 = newNode; newParent.child2 = newNode.index;
bestSibling.parent = newParent; bestSibling.parent = newParent.index;
newNode.parent = newParent; newNode.parent = newParent.index;
} else { } else {
newParent.child1 = bestSibling; newParent.child1 = bestSibling.index;
newParent.child2 = newNode; newParent.child2 = newNode.index;
bestSibling.parent = newParent; bestSibling.parent = newParent.index;
newNode.parent = newParent; newNode.parent = newParent.index;
this.root = newParent; this.root = newParent;
} }
// Walk back up the tree refitting ancestors' AABB and applying rotations // Walk back up the tree refitting ancestors' AABB and applying rotations
var ancestor = newNode.parent; var ancestor = newNode.parent != -1 ? nodes[newNode.parent] : null;
while (ancestor != null) { while (ancestor != null) {
var child1 = ancestor.child1; var child1 = ancestor.child1;
var child2 = ancestor.child2; var child2 = ancestor.child2;
ancestor.bounds = new Bounds(); ancestor.bounds = new Bounds();
if (child1 != null) if (child1 != -1)
ancestor.bounds.add(child1.bounds); ancestor.bounds.add(nodes[child1].bounds);
if (child2 != null) if (child2 != -1)
ancestor.bounds.add(child2.bounds); ancestor.bounds.add(nodes[child2].bounds);
this.rotate(ancestor); this.rotate(ancestor);
ancestor = ancestor.parent; ancestor = nodes[ancestor.parent];
} }
return newNode; return newNode;
} }
function reset() { function reset() {
this.nodeId = 0; this.nodes = [];
this.root = null; this.root = null;
} }
// BFS tree traversal // BFS tree traversal
function traverse(callback:(node:BVHNode<T>) -> Void) { function traverse(callback:(node:BVHNode<T>) -> Void) {
var q = [this.root]; var q = [this.root.index];
while (q.length != 0) { while (q.length != 0) {
var current = q.shift(); var current = q.shift();
if (current == null) { if (current == null) {
break; break;
} }
var currentnode = nodes[current];
callback(currentnode);
callback(current); if (!currentnode.isLeaf) {
if (currentnode.child1 != -1)
if (!current.isLeaf) { q.push(currentnode.child1);
if (current.child1 != null) if (currentnode.child2 != -1)
q.push(current.child1); q.push(currentnode.child2);
if (current.child2 != null)
q.push(current.child2);
} }
} }
} }
public function remove(node:BVHNode<T>) { public function remove(node:BVHNode<T>) {
var parent = node.parent; var parent = node.parent != -1 ? nodes[node.parent] : null;
if (parent != null) { if (parent != null) {
var sibling = parent.child1 == node ? parent.child2 : parent.child1; var sibling = parent.child1 == node.index ? parent.child2 : parent.child1;
var siblingnode = nodes[sibling];
if (parent.parent != null) { if (parent.parent != -1) {
sibling.parent = parent.parent; siblingnode.parent = parent.parent;
if (parent.parent.child1 == parent) { if (nodes[parent.parent].child1 == parent.index) {
parent.parent.child1 = sibling; nodes[parent.parent].child1 = sibling;
} else { } else {
parent.parent.child2 = sibling; nodes[parent.parent].child2 = sibling;
} }
} else { } else {
this.root = sibling; this.root = siblingnode;
sibling.parent = null; siblingnode.parent = -1;
} }
var ancestor = sibling.parent; var ancestor = siblingnode.parent;
while (ancestor != null) { while (ancestor != -1) {
var child1 = ancestor.child1; var ancestornode = nodes[ancestor];
var child2 = ancestor.child2; var child1 = nodes[ancestornode.child1];
var child2 = nodes[ancestornode.child2];
ancestor.bounds = child1.bounds.clone(); ancestornode.bounds = child1.bounds.clone();
ancestor.bounds.add(child2.bounds); ancestornode.bounds.add(child2.bounds);
ancestor = ancestor.parent; ancestor = ancestornode.parent;
} }
} else { } else {
if (this.root == node) { if (this.root == node) {
@ -209,32 +217,32 @@ class BVHTree<T:IBVHObject> {
} }
function rotate(node:BVHNode<T>) { function rotate(node:BVHNode<T>) {
if (node.parent == null) { if (node.parent == -1) {
return; return;
} }
var parent = node.parent; var parent = nodes[node.parent];
var sibling = parent.child1 == node ? parent.child2 : parent.child1; var sibling = nodes[parent.child1 == node.index ? parent.child2 : parent.child1];
var costDiffs = []; var costDiffs = [];
var nodeArea = node.bounds.xSize * node.bounds.ySize + node.bounds.zSize * node.bounds.ySize + node.bounds.xSize * node.bounds.zSize; 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(); var ch1 = sibling.bounds.clone();
ch1.add(node.child1.bounds); ch1.add(nodes[node.child1].bounds);
costDiffs.push(ch1.xSize * ch1.ySize + ch1.zSize * ch1.ySize + ch1.xSize * ch1.zSize - nodeArea); costDiffs.push(ch1.xSize * ch1.ySize + ch1.zSize * ch1.ySize + ch1.xSize * ch1.zSize - nodeArea);
var ch2 = sibling.bounds.clone(); var ch2 = sibling.bounds.clone();
ch2.add(node.child2.bounds); ch2.add(nodes[node.child2].bounds);
costDiffs.push(ch2.xSize * ch2.ySize + ch2.zSize * ch2.ySize + ch2.xSize * ch2.zSize - nodeArea); costDiffs.push(ch2.xSize * ch2.ySize + ch2.zSize * ch2.ySize + ch2.xSize * ch2.zSize - nodeArea);
if (!sibling.isLeaf) { if (!sibling.isLeaf) {
var siblingArea = sibling.bounds.xSize * sibling.bounds.ySize + sibling.bounds.zSize * sibling.bounds.ySize var siblingArea = sibling.bounds.xSize * sibling.bounds.ySize + sibling.bounds.zSize * sibling.bounds.ySize
+ sibling.bounds.xSize * sibling.bounds.zSize; + sibling.bounds.xSize * sibling.bounds.zSize;
if (sibling.child1 != null) { if (sibling.child1 != -1) {
var ch3 = node.bounds.clone(); var ch3 = node.bounds.clone();
ch3.add(sibling.child1.bounds); ch3.add(nodes[sibling.child1].bounds);
costDiffs.push(ch3.xSize * ch3.ySize + ch3.zSize * ch3.ySize + ch3.xSize * ch3.zSize - siblingArea); costDiffs.push(ch3.xSize * ch3.ySize + ch3.zSize * ch3.ySize + ch3.xSize * ch3.zSize - siblingArea);
} }
if (sibling.child2 != null) { if (sibling.child2 != -1) {
var ch4 = node.bounds.clone(); var ch4 = node.bounds.clone();
ch4.add(sibling.child2.bounds); ch4.add(nodes[sibling.child2].bounds);
costDiffs.push(ch4.xSize * ch4.ySize + ch4.zSize * ch4.ySize + ch4.xSize * ch4.zSize - siblingArea); costDiffs.push(ch4.xSize * ch4.ySize + ch4.zSize * ch4.ySize + ch4.xSize * ch4.zSize - siblingArea);
} }
} }
@ -249,67 +257,67 @@ class BVHTree<T:IBVHObject> {
if (costDiffs[bestDiffIndex] < 0.0) { if (costDiffs[bestDiffIndex] < 0.0) {
switch (bestDiffIndex) { switch (bestDiffIndex) {
case 0: case 0:
if (parent.child1 == sibling) { if (parent.child1 == sibling.index) {
parent.child1 = node.child2; parent.child1 = node.child2;
} else { } else {
parent.child2 = node.child2; parent.child2 = node.child2;
} }
if (node.child2 != null) { if (node.child2 != -1) {
node.child2.parent = parent; nodes[node.child2].parent = parent.index;
} }
node.child2 = sibling; node.child2 = sibling.index;
sibling.parent = node; sibling.parent = node.index;
node.bounds = sibling.bounds.clone(); node.bounds = sibling.bounds.clone();
if (node.child1 != null) { if (node.child1 != -1) {
node.bounds.add(node.child1.bounds); node.bounds.add(nodes[node.child1].bounds);
} }
case 1: case 1:
if (parent.child1 == sibling) { if (parent.child1 == sibling.index) {
parent.child1 = node.child1; parent.child1 = node.child1;
} else { } else {
parent.child2 = node.child1; parent.child2 = node.child1;
} }
if (node.child1 != null) { if (node.child1 != -1) {
node.child1.parent = parent; nodes[node.child1].parent = parent.index;
} }
node.child1 = sibling; node.child1 = sibling.index;
sibling.parent = node; sibling.parent = node.index;
node.bounds = sibling.bounds.clone(); node.bounds = sibling.bounds.clone();
if (node.child2 != null) { if (node.child2 != -1) {
node.bounds.add(node.child2.bounds); node.bounds.add(nodes[node.child2].bounds);
} }
case 2: case 2:
if (parent.child1 == node) { if (parent.child1 == node.index) {
parent.child1 = sibling.child2; parent.child1 = sibling.child2;
} else { } else {
parent.child2 = sibling.child2; parent.child2 = sibling.child2;
} }
if (sibling.child2 != null) { if (sibling.child2 != -1) {
sibling.child2.parent = parent; nodes[sibling.child2].parent = parent.index;
} }
sibling.child2 = node; sibling.child2 = node.index;
node.parent = sibling; node.parent = sibling.index;
sibling.bounds = node.bounds.clone(); sibling.bounds = node.bounds.clone();
if (sibling.child2 != null) { if (sibling.child2 != -1) {
sibling.bounds.add(sibling.child2.bounds); sibling.bounds.add(nodes[sibling.child2].bounds);
} }
case 3: case 3:
if (parent.child1 == node) { if (parent.child1 == node.index) {
parent.child1 = sibling.child1; parent.child1 = sibling.child1;
} else { } else {
parent.child2 = sibling.child1; parent.child2 = sibling.child1;
} }
if (sibling.child1 != null) { if (sibling.child1 != -1) {
sibling.child1.parent = parent; nodes[sibling.child1].parent = parent.index;
} }
sibling.child1 = node; sibling.child1 = node.index;
node.parent = sibling; node.parent = sibling.index;
sibling.bounds = node.bounds.clone(); sibling.bounds = node.bounds.clone();
if (sibling.child1 != null) { if (sibling.child1 != -1) {
sibling.bounds.add(sibling.child1.bounds); sibling.bounds.add(nodes[sibling.child1].bounds);
} }
} }
} }
@ -320,19 +328,21 @@ class BVHTree<T:IBVHObject> {
if (this.root == null) if (this.root == null)
return res; return res;
var q = [this.root]; var q = [this.root.index];
var qptr = 0;
while (q.length != 0) { while (qptr != q.length) {
var current = q.shift(); var current = q[qptr++];
var currentnode = this.nodes[current];
if (current.bounds.containsBounds(searchbox) || current.bounds.collide(searchbox)) { if (currentnode.bounds.containsBounds(searchbox) || currentnode.bounds.collide(searchbox)) {
if (current.isLeaf) { if (currentnode.isLeaf) {
res.push(current.object); res.push(currentnode.object);
} else { } else {
if (current.child1 != null) if (currentnode.child1 != -1)
q.push(current.child1); q.push(currentnode.child1);
if (current.child2 != null) if (currentnode.child2 != -1)
q.push(current.child2); q.push(currentnode.child2);
} }
} }
} }
@ -346,17 +356,19 @@ class BVHTree<T:IBVHObject> {
return res; return res;
var ray = h3d.col.Ray.fromValues(origin.x, origin.y, origin.z, direction.x, direction.y, direction.z); var ray = h3d.col.Ray.fromValues(origin.x, origin.y, origin.z, direction.x, direction.y, direction.z);
var q = [this.root]; var q = [this.root.index];
while (q.length != 0) { var qptr = 0;
var current = q.shift(); while (qptr != q.length) {
if (ray.collide(current.bounds)) { var current = q[qptr++];
if (current.isLeaf) { var currentnode = this.nodes[current];
res = res.concat(current.object.rayCast(origin, direction)); if (ray.collide(currentnode.bounds)) {
if (currentnode.isLeaf) {
res = res.concat(currentnode.object.rayCast(origin, direction));
} else { } else {
if (current.child1 != null) if (currentnode.child1 != -1)
q.push(current.child1); q.push(currentnode.child1);
if (current.child2 != null) if (currentnode.child2 != -1)
q.push(current.child2); q.push(currentnode.child2);
} }
} }
} }

83
src/collision/Collision.hx
View file

@ -20,11 +20,17 @@ typedef CPSSResult = {
var c2:Vector; var c2:Vector;
} }
typedef ITSResult = { @:publicFields
var result:Bool; class ITSResult {
var result:Bool = false;
var normal:Vector; var normal:Vector;
var point:Vector; var point:Vector;
var resIdx:Int; var resIdx:Int = -1;
public inline function new() {
this.point = new Vector();
this.normal = new Vector();
}
} }
class Collision { class Collision {
@ -45,7 +51,7 @@ class Collision {
return p; return p;
} }
public static function ClosestPointLine(start:Vector, end:Vector, center:Vector) { public static inline function ClosestPointLine(start:Vector, end:Vector, center:Vector) {
var d = end.sub(start); var d = end.sub(start);
var v = center.sub(start); var v = center.sub(start);
var t = v.dot(d) / d.lengthSq(); var t = v.dot(d) / d.lengthSq();
@ -65,12 +71,7 @@ class Collision {
edgeConcavities:Array<Bool>) { edgeConcavities:Array<Bool>) {
var radiusSq = radius * radius; var radiusSq = radius * radius;
var res:ITSResult = { var res = new ITSResult();
result: false,
point: null,
normal: null,
resIdx: 0
};
var pnorm = normal.clone(); var pnorm = normal.clone();
var d = -v0.dot(pnorm); var d = -v0.dot(pnorm);
@ -104,12 +105,12 @@ class Collision {
var chosenEdge = 0; // Bitfield var chosenEdge = 0; // Bitfield
var chosenPt:Vector; var chosenPt = new Vector();
if (r1.distanceSq(center) < r2.distanceSq(center)) { if (r1.distanceSq(center) < r2.distanceSq(center)) {
chosenPt = r1; chosenPt.load(r1);
chosenEdge = 1; chosenEdge = 1;
} else { } else {
chosenPt = r2; chosenPt.load(r2);
chosenEdge = 2; chosenEdge = 2;
} }
if (chosenPt.distanceSq(center) < r3.distanceSq(center)) if (chosenPt.distanceSq(center) < r3.distanceSq(center))
@ -144,20 +145,10 @@ class Collision {
return res; return res;
} }
public static function TriangleSphereIntersection(A:Vector, B:Vector, C:Vector, N:Vector, P:Vector, r:Float) { public static inline function TriangleSphereIntersection(v0:Vector, v1:Vector, v2:Vector, N:Vector, P:Vector, r:Float, point:Vector, normal:Vector) {
var res:ITSResult = { var A = v0.sub(P);
result: false, var B = v1.sub(P);
point: null, var C = v2.sub(P);
normal: null,
resIdx: -1
};
var v0 = A;
var v1 = B;
var v2 = C;
A = A.sub(P);
B = B.sub(P);
C = C.sub(P);
var ca = C.sub(A); var ca = C.sub(A);
var ba = B.sub(A); var ba = B.sub(A);
var radiusSq = r * r; var radiusSq = r * r;
@ -165,7 +156,7 @@ class Collision {
var aDotCp = A.dot(cp); var aDotCp = A.dot(cp);
var cpLenSq = cp.lengthSq(); var cpLenSq = cp.lengthSq();
if (aDotCp * aDotCp > radiusSq * cpLenSq) { if (aDotCp * aDotCp > radiusSq * cpLenSq) {
return res; return false;
} }
var aSq = A.dot(A); var aSq = A.dot(A);
@ -176,13 +167,13 @@ class Collision {
var cSq = C.dot(C); var cSq = C.dot(C);
if (aSq > radiusSq && aDotB > aSq && aDotC > aSq) { if (aSq > radiusSq && aDotB > aSq && aDotC > aSq) {
return res; return false;
} }
if (bSq > radiusSq && aDotB > bSq && bDotC > bSq) { if (bSq > radiusSq && aDotB > bSq && bDotC > bSq) {
return res; return false;
} }
if (cSq > radiusSq && aDotC > cSq && bDotC > cSq) { if (cSq > radiusSq && aDotC > cSq && bDotC > cSq) {
return res; return false;
} }
var cSubB = C.sub(B); var cSubB = C.sub(B);
@ -198,13 +189,13 @@ class Collision {
var rhs3 = B.multiply(aSubCSq).sub(cTest); var rhs3 = B.multiply(aSubCSq).sub(cTest);
if (aTest.dot(aTest) > radiusSq * baSq * baSq && aTest.dot(rhs) > 0) { if (aTest.dot(aTest) > radiusSq * baSq * baSq && aTest.dot(rhs) > 0) {
return res; return false;
} }
if (bTest.dot(bTest) > radiusSq * cSubBSq * cSubBSq && bTest.dot(rhs2) > 0) { if (bTest.dot(bTest) > radiusSq * cSubBSq * cSubBSq && bTest.dot(rhs2) > 0) {
return res; return false;
} }
if (cTest.dot(cTest) > radiusSq * aSubCSq * aSubCSq && cTest.dot(rhs3) > 0) { if (cTest.dot(cTest) > radiusSq * aSubCSq * aSubCSq && cTest.dot(rhs3) > 0) {
return res; return false;
} }
var lhs = P.sub(v0); var lhs = P.sub(v0);
@ -217,10 +208,9 @@ class Collision {
var d2 = (baSq * lhsCa - baca * lhsBa) / len; var d2 = (baSq * lhsCa - baca * lhsBa) / len;
if (1 - d1 - d2 >= 0 && d1 >= 0 && d2 >= 0) { if (1 - d1 - d2 >= 0 && d1 >= 0 && d2 >= 0) {
res.result = true; normal.load(N);
res.normal = N.clone(); point.load(P.sub(N.multiply(P.sub(v0).dot(N))));
res.point = P.sub(N.multiply(P.sub(v0).dot(N))); return true;
res.resIdx = 0;
} else { } else {
var closestPt = P.sub(N.multiply(P.sub(v0).dot(N))); var closestPt = P.sub(N.multiply(P.sub(v0).dot(N)));
var r1 = ClosestPointLine(v0, v1, closestPt); var r1 = ClosestPointLine(v0, v1, closestPt);
@ -229,24 +219,22 @@ class Collision {
var chosenEdge = 0; // Bitfield var chosenEdge = 0; // Bitfield
var chosenPt:Vector; var chosenPt:Vector = new Vector();
if (r1.distanceSq(P) < r2.distanceSq(P)) { if (r1.distanceSq(P) < r2.distanceSq(P)) {
chosenPt = r1; chosenPt.load(r1);
chosenEdge = 1; chosenEdge = 1;
} else { } else {
chosenPt = r2; chosenPt.load(r2);
chosenEdge = 2; chosenEdge = 2;
} }
if (chosenPt.distanceSq(P) < r3.distanceSq(P)) if (chosenPt.distanceSq(P) < r3.distanceSq(P))
res.point = chosenPt; point.load(chosenPt);
else { else {
chosenEdge = 4; chosenEdge = 4;
res.point = r3; point.load(r3);
} }
res.normal = P.sub(res.point).normalized(); normal.load(P.sub(point).normalized());
res.result = true; return true;
res.resIdx = chosenEdge;
// if (res.normal.dot(N) > 0.8) { // if (res.normal.dot(N) > 0.8) {
// // Internal edge // // Internal edge
@ -263,7 +251,6 @@ class Collision {
// } // }
// } // }
} }
return res;
} }
public static function IntersectSegmentCapsule(segStart:Vector, segEnd:Vector, capStart:Vector, capEnd:Vector, radius:Float) { public static function IntersectSegmentCapsule(segStart:Vector, segEnd:Vector, capStart:Vector, capEnd:Vector, radius:Float) {

31
src/collision/CollisionEntity.hx
View file

@ -1,5 +1,6 @@
package collision; package collision;
import collision.Collision.ITSResult;
import collision.BVHTree.IBVHObject; import collision.BVHTree.IBVHObject;
import src.TimeState; import src.TimeState;
import src.GameObject; import src.GameObject;
@ -200,33 +201,31 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
// var v0 = surface.points[surface.indices[i]].transformed(tform); // var v0 = surface.points[surface.indices[i]].transformed(tform);
// var v = surface.points[surface.indices[i + 1]].transformed(tform); // var v = surface.points[surface.indices[i + 1]].transformed(tform);
// var v2 = surface.points[surface.indices[i + 2]].transformed(tform); // var v2 = surface.points[surface.indices[i + 2]].transformed(tform);
var v0 = verts.v1; var v0 = new Vector(verts.v1x, verts.v1y, verts.v1z);
var v = verts.v2; var v = new Vector(verts.v2x, verts.v2y, verts.v2z);
var v2 = verts.v3; var v2 = new Vector(verts.v3x, verts.v3y, verts.v3z);
var surfacenormal = verts.n; // surface.normals[surface.indices[i]].transformed3x3(transform).normalized(); var surfacenormal = new Vector(verts.nx, verts.ny, verts.nz); // surface.normals[surface.indices[i]].transformed3x3(transform).normalized();
if (correctNormals) { if (correctNormals) {
var vn = v.sub(v0).cross(v2.sub(v0)).normalized().multiply(-1); var vn = v.sub(v0).cross(v2.sub(v0)).normalized().multiply(-1);
var vdot = vn.dot(surfacenormal); var vdot = vn.dot(surfacenormal);
if (vdot < 0.95) { if (vdot < 0.95) {
v0 = verts.v1; v.set(verts.v3x, verts.v3y, verts.v3z);
v = verts.v3; v2.set(verts.v2x, verts.v2y, verts.v2z);
v2 = verts.v2;
surfacenormal = vn; surfacenormal.load(vn);
} }
} }
var res = Collision.TriangleSphereIntersection(v0, v, v2, surfacenormal, position, radius); var closest = new Vector();
var closest = res.point; var normal = new Vector();
var res = Collision.TriangleSphereIntersection(v0, v, v2, surfacenormal, position, radius, closest, normal);
// var closest = Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surfacenormal); // var closest = Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surfacenormal);
if (closest != null) { if (res) {
var contactDist = closest.distanceSq(position); var contactDist = closest.distanceSq(position);
Debug.drawTriangle(v0, v, v2); // Debug.drawTriangle(v0, v, v2);
if (contactDist <= radius * radius) { if (contactDist <= radius * radius) {
var normal = res.normal;
if (position.sub(closest).dot(surfacenormal) > 0) { if (position.sub(closest).dot(surfacenormal) > 0) {
normal.normalize(); normal.normalize();
@ -236,8 +235,8 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
// bestDot = testDot; // bestDot = testDot;
var cinfo = new CollisionInfo(); var cinfo = new CollisionInfo();
cinfo.normal = normal; cinfo.normal = normal.clone();
cinfo.point = closest; cinfo.point = closest.clone();
// cinfo.collider = this; // cinfo.collider = this;
cinfo.velocity = this.velocity.clone(); cinfo.velocity = this.velocity.clone();
cinfo.contactDistance = Math.sqrt(contactDist); cinfo.contactDistance = Math.sqrt(contactDist);

43
src/collision/CollisionSurface.hx
View file

@ -6,6 +6,37 @@ import octree.IOctreeObject;
import h3d.Vector; import h3d.Vector;
import collision.BVHTree.IBVHObject; import collision.BVHTree.IBVHObject;
@:publicFields
class TransformedCollisionTriangle {
var v1x:Float;
var v1y:Float;
var v1z:Float;
var v2x:Float;
var v2y:Float;
var v2z:Float;
var v3x:Float;
var v3y:Float;
var v3z:Float;
var nx:Float;
var ny:Float;
var nz:Float;
inline public function new(v1:Vector, v2:Vector, v3:Vector, n:Vector) {
v1x = v1.x;
v1y = v1.y;
v1z = v1.z;
v2x = v2.x;
v2y = v2.y;
v2z = v2.z;
v3x = v3.x;
v3y = v3.y;
v3z = v3.z;
nx = n.x;
ny = n.y;
nz = n.z;
}
}
class CollisionSurface implements IOctreeObject implements IBVHObject { class CollisionSurface implements IOctreeObject implements IBVHObject {
public var priority:Int; public var priority:Int;
public var position:Int; public var position:Int;
@ -147,7 +178,7 @@ class CollisionSurface implements IOctreeObject implements IBVHObject {
return furthestVertex; return furthestVertex;
} }
public function transformTriangle(idx:Int, tform:Matrix, invtform:Matrix, key:Int) { public inline function transformTriangle(idx:Int, tform:Matrix, invtform:Matrix, key:Int) {
if (_transformedPoints == null) { if (_transformedPoints == null) {
_transformedPoints = points.copy(); _transformedPoints = points.copy();
} }
@ -182,12 +213,10 @@ class CollisionSurface implements IOctreeObject implements IBVHObject {
_transformedPoints[p3 * 3 + 2] = pt.z; _transformedPoints[p3 * 3 + 2] = pt.z;
transformKeys[p3] = key; transformKeys[p3] = key;
} }
return { return new TransformedCollisionTriangle(new Vector(_transformedPoints[p1 * 3], _transformedPoints[p1 * 3 + 1], _transformedPoints[p1 * 3 + 2]),
v1: new Vector(_transformedPoints[p1 * 3], _transformedPoints[p1 * 3 + 1], _transformedPoints[p1 * 3 + 2]), new Vector(_transformedPoints[p2 * 3], _transformedPoints[p2 * 3 + 1], _transformedPoints[p2 * 3 + 2]),
v2: new Vector(_transformedPoints[p2 * 3], _transformedPoints[p2 * 3 + 1], _transformedPoints[p2 * 3 + 2]), new Vector(_transformedPoints[p3 * 3], _transformedPoints[p3 * 3 + 1], _transformedPoints[p3 * 3 + 2]),
v3: new Vector(_transformedPoints[p3 * 3], _transformedPoints[p3 * 3 + 1], _transformedPoints[p3 * 3 + 2]), new Vector(_transformedNormals[p1 * 3], _transformedNormals[p1 * 3 + 1], _transformedNormals[p1 * 3 + 2]));
n: new Vector(_transformedNormals[p1 * 3], _transformedNormals[p1 * 3 + 1], _transformedNormals[p1 * 3 + 2])
};
} }
public function dispose() { public function dispose() {

8
src/collision/CollisionWorld.hx
View file

@ -60,8 +60,12 @@ class CollisionWorld {
for (marb in marbleEntities) { for (marb in marbleEntities) {
if (marb != spherecollision) { if (marb != spherecollision) {
if (spherecollision.go.isCollideable) if (spherecollision.go.isCollideable) {
contacts = contacts.concat(marb.sphereIntersection(spherecollision, timeState)); var isecs = marb.sphereIntersection(spherecollision, timeState);
if (isecs.length > 0)
foundEntities.push(marb);
contacts = contacts.concat(isecs);
}
} }
} }
return {foundEntities: foundEntities, contacts: contacts}; return {foundEntities: foundEntities, contacts: contacts};