mirror of
https://github.com/RandomityGuy/MBHaxe.git
synced 2025-10-30 08:11:25 +00:00
collision optimization, reduce heap allocs
This commit is contained in:
RandomityGuy
parent
4484a67057
commit
d833ac8a2b
5 changed files with 218 additions and 187 deletions
|
|
@ -10,10 +10,10 @@ interface IBVHObject {
|
|||
|
||||
@:publicFields
|
||||
class BVHNode<T:IBVHObject> {
|
||||
var id:Int;
|
||||
var parent:BVHNode<T>;
|
||||
var child1:BVHNode<T>;
|
||||
var child2:BVHNode<T>;
|
||||
var index:Int;
|
||||
var parent:Int = -1;
|
||||
var child1:Int = -1;
|
||||
var child2:Int = -1;
|
||||
var isLeaf:Bool;
|
||||
var bounds:Bounds;
|
||||
var object:T;
|
||||
|
|
@ -22,11 +22,19 @@ class BVHNode<T:IBVHObject> {
|
|||
}
|
||||
|
||||
class BVHTree<T:IBVHObject> {
|
||||
var nodeId:Int = 0;
|
||||
var root:BVHNode<T>;
|
||||
var nodes:Array<BVHNode<T>> = [];
|
||||
|
||||
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() {
|
||||
var invalidNodes = [];
|
||||
this.traverse(node -> {
|
||||
|
|
@ -51,8 +59,7 @@ class BVHTree<T:IBVHObject> {
|
|||
// Enlarged AABB
|
||||
var aabb = entity.boundingBox;
|
||||
|
||||
var newNode = new BVHNode();
|
||||
newNode.id = this.nodeId++;
|
||||
var newNode = allocateNode();
|
||||
newNode.bounds = aabb;
|
||||
newNode.object = entity;
|
||||
newNode.isLeaf = true;
|
||||
|
|
@ -67,11 +74,11 @@ class BVHTree<T:IBVHObject> {
|
|||
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}];
|
||||
var q = [{p1: this.root.index, p2: 0.0}];
|
||||
|
||||
while (q.length != 0) {
|
||||
var front = q.shift();
|
||||
var current = front.p1;
|
||||
var current = nodes[front.p1];
|
||||
var inheritedCost = front.p2;
|
||||
|
||||
var combined = current.bounds.clone();
|
||||
|
|
@ -91,115 +98,116 @@ class BVHTree<T:IBVHObject> {
|
|||
var lowerBoundCost = aabbCost + inheritedCost;
|
||||
if (lowerBoundCost < bestCost) {
|
||||
if (!current.isLeaf) {
|
||||
if (current.child1 != null)
|
||||
if (current.child1 != -1)
|
||||
q.push({p1: current.child1, p2: inheritedCost});
|
||||
if (current.child2 != null)
|
||||
if (current.child2 != -1)
|
||||
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;
|
||||
var oldParent = bestSibling.parent != -1 ? nodes[bestSibling.parent] : null;
|
||||
var newParent = allocateNode();
|
||||
newParent.parent = oldParent != null ? oldParent.index : -1;
|
||||
newParent.bounds = bestSibling.bounds.clone();
|
||||
newParent.bounds.add(aabb);
|
||||
newParent.isLeaf = false;
|
||||
|
||||
if (oldParent != null) {
|
||||
if (oldParent.child1 == bestSibling) {
|
||||
oldParent.child1 = newParent;
|
||||
if (oldParent.child1 == bestSibling.index) {
|
||||
oldParent.child1 = newParent.index;
|
||||
} else {
|
||||
oldParent.child2 = newParent;
|
||||
oldParent.child2 = newParent.index;
|
||||
}
|
||||
|
||||
newParent.child1 = bestSibling;
|
||||
newParent.child2 = newNode;
|
||||
bestSibling.parent = newParent;
|
||||
newNode.parent = newParent;
|
||||
newParent.child1 = bestSibling.index;
|
||||
newParent.child2 = newNode.index;
|
||||
bestSibling.parent = newParent.index;
|
||||
newNode.parent = newParent.index;
|
||||
} else {
|
||||
newParent.child1 = bestSibling;
|
||||
newParent.child2 = newNode;
|
||||
bestSibling.parent = newParent;
|
||||
newNode.parent = newParent;
|
||||
newParent.child1 = bestSibling.index;
|
||||
newParent.child2 = newNode.index;
|
||||
bestSibling.parent = newParent.index;
|
||||
newNode.parent = newParent.index;
|
||||
this.root = newParent;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
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);
|
||||
if (child1 != -1)
|
||||
ancestor.bounds.add(nodes[child1].bounds);
|
||||
if (child2 != -1)
|
||||
ancestor.bounds.add(nodes[child2].bounds);
|
||||
|
||||
this.rotate(ancestor);
|
||||
|
||||
ancestor = ancestor.parent;
|
||||
ancestor = nodes[ancestor.parent];
|
||||
}
|
||||
|
||||
return newNode;
|
||||
}
|
||||
|
||||
function reset() {
|
||||
this.nodeId = 0;
|
||||
this.nodes = [];
|
||||
this.root = null;
|
||||
}
|
||||
|
||||
// BFS tree traversal
|
||||
function traverse(callback:(node:BVHNode<T>) -> Void) {
|
||||
var q = [this.root];
|
||||
var q = [this.root.index];
|
||||
|
||||
while (q.length != 0) {
|
||||
var current = q.shift();
|
||||
if (current == null) {
|
||||
break;
|
||||
}
|
||||
var currentnode = nodes[current];
|
||||
callback(currentnode);
|
||||
|
||||
callback(current);
|
||||
|
||||
if (!current.isLeaf) {
|
||||
if (current.child1 != null)
|
||||
q.push(current.child1);
|
||||
if (current.child2 != null)
|
||||
q.push(current.child2);
|
||||
if (!currentnode.isLeaf) {
|
||||
if (currentnode.child1 != -1)
|
||||
q.push(currentnode.child1);
|
||||
if (currentnode.child2 != -1)
|
||||
q.push(currentnode.child2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public function remove(node:BVHNode<T>) {
|
||||
var parent = node.parent;
|
||||
var parent = node.parent != -1 ? nodes[node.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) {
|
||||
sibling.parent = parent.parent;
|
||||
if (parent.parent.child1 == parent) {
|
||||
parent.parent.child1 = sibling;
|
||||
if (parent.parent != -1) {
|
||||
siblingnode.parent = parent.parent;
|
||||
if (nodes[parent.parent].child1 == parent.index) {
|
||||
nodes[parent.parent].child1 = sibling;
|
||||
} else {
|
||||
parent.parent.child2 = sibling;
|
||||
nodes[parent.parent].child2 = sibling;
|
||||
}
|
||||
} else {
|
||||
this.root = sibling;
|
||||
sibling.parent = null;
|
||||
this.root = siblingnode;
|
||||
siblingnode.parent = -1;
|
||||
}
|
||||
|
||||
var ancestor = sibling.parent;
|
||||
while (ancestor != null) {
|
||||
var child1 = ancestor.child1;
|
||||
var child2 = ancestor.child2;
|
||||
var ancestor = siblingnode.parent;
|
||||
while (ancestor != -1) {
|
||||
var ancestornode = nodes[ancestor];
|
||||
var child1 = nodes[ancestornode.child1];
|
||||
var child2 = nodes[ancestornode.child2];
|
||||
|
||||
ancestor.bounds = child1.bounds.clone();
|
||||
ancestor.bounds.add(child2.bounds);
|
||||
ancestor = ancestor.parent;
|
||||
ancestornode.bounds = child1.bounds.clone();
|
||||
ancestornode.bounds.add(child2.bounds);
|
||||
ancestor = ancestornode.parent;
|
||||
}
|
||||
} else {
|
||||
if (this.root == node) {
|
||||
|
|
@ -209,32 +217,32 @@ class BVHTree<T:IBVHObject> {
|
|||
}
|
||||
|
||||
function rotate(node:BVHNode<T>) {
|
||||
if (node.parent == null) {
|
||||
if (node.parent == -1) {
|
||||
return;
|
||||
}
|
||||
var parent = node.parent;
|
||||
var sibling = parent.child1 == node ? parent.child2 : parent.child1;
|
||||
var parent = nodes[node.parent];
|
||||
var sibling = nodes[parent.child1 == node.index ? 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);
|
||||
ch1.add(nodes[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);
|
||||
ch2.add(nodes[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) {
|
||||
if (sibling.child1 != -1) {
|
||||
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);
|
||||
}
|
||||
if (sibling.child2 != null) {
|
||||
if (sibling.child2 != -1) {
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
@ -249,67 +257,67 @@ class BVHTree<T:IBVHObject> {
|
|||
if (costDiffs[bestDiffIndex] < 0.0) {
|
||||
switch (bestDiffIndex) {
|
||||
case 0:
|
||||
if (parent.child1 == sibling) {
|
||||
if (parent.child1 == sibling.index) {
|
||||
parent.child1 = node.child2;
|
||||
} else {
|
||||
parent.child2 = node.child2;
|
||||
}
|
||||
|
||||
if (node.child2 != null) {
|
||||
node.child2.parent = parent;
|
||||
if (node.child2 != -1) {
|
||||
nodes[node.child2].parent = parent.index;
|
||||
}
|
||||
|
||||
node.child2 = sibling;
|
||||
sibling.parent = node;
|
||||
node.child2 = sibling.index;
|
||||
sibling.parent = node.index;
|
||||
node.bounds = sibling.bounds.clone();
|
||||
if (node.child1 != null) {
|
||||
node.bounds.add(node.child1.bounds);
|
||||
if (node.child1 != -1) {
|
||||
node.bounds.add(nodes[node.child1].bounds);
|
||||
}
|
||||
case 1:
|
||||
if (parent.child1 == sibling) {
|
||||
if (parent.child1 == sibling.index) {
|
||||
parent.child1 = node.child1;
|
||||
} else {
|
||||
parent.child2 = node.child1;
|
||||
}
|
||||
if (node.child1 != null) {
|
||||
node.child1.parent = parent;
|
||||
if (node.child1 != -1) {
|
||||
nodes[node.child1].parent = parent.index;
|
||||
}
|
||||
node.child1 = sibling;
|
||||
sibling.parent = node;
|
||||
node.child1 = sibling.index;
|
||||
sibling.parent = node.index;
|
||||
node.bounds = sibling.bounds.clone();
|
||||
if (node.child2 != null) {
|
||||
node.bounds.add(node.child2.bounds);
|
||||
if (node.child2 != -1) {
|
||||
node.bounds.add(nodes[node.child2].bounds);
|
||||
}
|
||||
case 2:
|
||||
if (parent.child1 == node) {
|
||||
if (parent.child1 == node.index) {
|
||||
parent.child1 = sibling.child2;
|
||||
} else {
|
||||
parent.child2 = sibling.child2;
|
||||
}
|
||||
if (sibling.child2 != null) {
|
||||
sibling.child2.parent = parent;
|
||||
if (sibling.child2 != -1) {
|
||||
nodes[sibling.child2].parent = parent.index;
|
||||
}
|
||||
sibling.child2 = node;
|
||||
node.parent = sibling;
|
||||
sibling.child2 = node.index;
|
||||
node.parent = sibling.index;
|
||||
sibling.bounds = node.bounds.clone();
|
||||
if (sibling.child2 != null) {
|
||||
sibling.bounds.add(sibling.child2.bounds);
|
||||
if (sibling.child2 != -1) {
|
||||
sibling.bounds.add(nodes[sibling.child2].bounds);
|
||||
}
|
||||
|
||||
case 3:
|
||||
if (parent.child1 == node) {
|
||||
if (parent.child1 == node.index) {
|
||||
parent.child1 = sibling.child1;
|
||||
} else {
|
||||
parent.child2 = sibling.child1;
|
||||
}
|
||||
if (sibling.child1 != null) {
|
||||
sibling.child1.parent = parent;
|
||||
if (sibling.child1 != -1) {
|
||||
nodes[sibling.child1].parent = parent.index;
|
||||
}
|
||||
sibling.child1 = node;
|
||||
node.parent = sibling;
|
||||
sibling.child1 = node.index;
|
||||
node.parent = sibling.index;
|
||||
sibling.bounds = node.bounds.clone();
|
||||
if (sibling.child1 != null) {
|
||||
sibling.bounds.add(sibling.child1.bounds);
|
||||
if (sibling.child1 != -1) {
|
||||
sibling.bounds.add(nodes[sibling.child1].bounds);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -320,19 +328,21 @@ class BVHTree<T:IBVHObject> {
|
|||
if (this.root == null)
|
||||
return res;
|
||||
|
||||
var q = [this.root];
|
||||
var q = [this.root.index];
|
||||
var qptr = 0;
|
||||
|
||||
while (q.length != 0) {
|
||||
var current = q.shift();
|
||||
while (qptr != q.length) {
|
||||
var current = q[qptr++];
|
||||
var currentnode = this.nodes[current];
|
||||
|
||||
if (current.bounds.containsBounds(searchbox) || current.bounds.collide(searchbox)) {
|
||||
if (current.isLeaf) {
|
||||
res.push(current.object);
|
||||
if (currentnode.bounds.containsBounds(searchbox) || currentnode.bounds.collide(searchbox)) {
|
||||
if (currentnode.isLeaf) {
|
||||
res.push(currentnode.object);
|
||||
} else {
|
||||
if (current.child1 != null)
|
||||
q.push(current.child1);
|
||||
if (current.child2 != null)
|
||||
q.push(current.child2);
|
||||
if (currentnode.child1 != -1)
|
||||
q.push(currentnode.child1);
|
||||
if (currentnode.child2 != -1)
|
||||
q.push(currentnode.child2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -346,17 +356,19 @@ class BVHTree<T:IBVHObject> {
|
|||
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.object.rayCast(origin, direction));
|
||||
var q = [this.root.index];
|
||||
var qptr = 0;
|
||||
while (qptr != q.length) {
|
||||
var current = q[qptr++];
|
||||
var currentnode = this.nodes[current];
|
||||
if (ray.collide(currentnode.bounds)) {
|
||||
if (currentnode.isLeaf) {
|
||||
res = res.concat(currentnode.object.rayCast(origin, direction));
|
||||
} else {
|
||||
if (current.child1 != null)
|
||||
q.push(current.child1);
|
||||
if (current.child2 != null)
|
||||
q.push(current.child2);
|
||||
if (currentnode.child1 != -1)
|
||||
q.push(currentnode.child1);
|
||||
if (currentnode.child2 != -1)
|
||||
q.push(currentnode.child2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,11 +20,17 @@ typedef CPSSResult = {
|
|||
var c2:Vector;
|
||||
}
|
||||
|
||||
typedef ITSResult = {
|
||||
var result:Bool;
|
||||
@:publicFields
|
||||
class ITSResult {
|
||||
var result:Bool = false;
|
||||
var normal: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 {
|
||||
|
|
@ -45,7 +51,7 @@ class Collision {
|
|||
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 v = center.sub(start);
|
||||
var t = v.dot(d) / d.lengthSq();
|
||||
|
|
@ -65,12 +71,7 @@ class Collision {
|
|||
edgeConcavities:Array<Bool>) {
|
||||
var radiusSq = radius * radius;
|
||||
|
||||
var res:ITSResult = {
|
||||
result: false,
|
||||
point: null,
|
||||
normal: null,
|
||||
resIdx: 0
|
||||
};
|
||||
var res = new ITSResult();
|
||||
|
||||
var pnorm = normal.clone();
|
||||
var d = -v0.dot(pnorm);
|
||||
|
|
@ -104,12 +105,12 @@ class Collision {
|
|||
|
||||
var chosenEdge = 0; // Bitfield
|
||||
|
||||
var chosenPt:Vector;
|
||||
var chosenPt = new Vector();
|
||||
if (r1.distanceSq(center) < r2.distanceSq(center)) {
|
||||
chosenPt = r1;
|
||||
chosenPt.load(r1);
|
||||
chosenEdge = 1;
|
||||
} else {
|
||||
chosenPt = r2;
|
||||
chosenPt.load(r2);
|
||||
chosenEdge = 2;
|
||||
}
|
||||
if (chosenPt.distanceSq(center) < r3.distanceSq(center))
|
||||
|
|
@ -144,20 +145,10 @@ class Collision {
|
|||
return res;
|
||||
}
|
||||
|
||||
public static function TriangleSphereIntersection(A:Vector, B:Vector, C:Vector, N:Vector, P:Vector, r:Float) {
|
||||
var res:ITSResult = {
|
||||
result: false,
|
||||
point: null,
|
||||
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);
|
||||
public static inline function TriangleSphereIntersection(v0:Vector, v1:Vector, v2:Vector, N:Vector, P:Vector, r:Float, point:Vector, normal:Vector) {
|
||||
var A = v0.sub(P);
|
||||
var B = v1.sub(P);
|
||||
var C = v2.sub(P);
|
||||
var ca = C.sub(A);
|
||||
var ba = B.sub(A);
|
||||
var radiusSq = r * r;
|
||||
|
|
@ -165,7 +156,7 @@ class Collision {
|
|||
var aDotCp = A.dot(cp);
|
||||
var cpLenSq = cp.lengthSq();
|
||||
if (aDotCp * aDotCp > radiusSq * cpLenSq) {
|
||||
return res;
|
||||
return false;
|
||||
}
|
||||
|
||||
var aSq = A.dot(A);
|
||||
|
|
@ -176,13 +167,13 @@ class Collision {
|
|||
var cSq = C.dot(C);
|
||||
|
||||
if (aSq > radiusSq && aDotB > aSq && aDotC > aSq) {
|
||||
return res;
|
||||
return false;
|
||||
}
|
||||
if (bSq > radiusSq && aDotB > bSq && bDotC > bSq) {
|
||||
return res;
|
||||
return false;
|
||||
}
|
||||
if (cSq > radiusSq && aDotC > cSq && bDotC > cSq) {
|
||||
return res;
|
||||
return false;
|
||||
}
|
||||
|
||||
var cSubB = C.sub(B);
|
||||
|
|
@ -198,13 +189,13 @@ class Collision {
|
|||
var rhs3 = B.multiply(aSubCSq).sub(cTest);
|
||||
|
||||
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) {
|
||||
return res;
|
||||
return false;
|
||||
}
|
||||
if (cTest.dot(cTest) > radiusSq * aSubCSq * aSubCSq && cTest.dot(rhs3) > 0) {
|
||||
return res;
|
||||
return false;
|
||||
}
|
||||
|
||||
var lhs = P.sub(v0);
|
||||
|
|
@ -217,10 +208,9 @@ class Collision {
|
|||
var d2 = (baSq * lhsCa - baca * lhsBa) / len;
|
||||
|
||||
if (1 - d1 - d2 >= 0 && d1 >= 0 && d2 >= 0) {
|
||||
res.result = true;
|
||||
res.normal = N.clone();
|
||||
res.point = P.sub(N.multiply(P.sub(v0).dot(N)));
|
||||
res.resIdx = 0;
|
||||
normal.load(N);
|
||||
point.load(P.sub(N.multiply(P.sub(v0).dot(N))));
|
||||
return true;
|
||||
} else {
|
||||
var closestPt = P.sub(N.multiply(P.sub(v0).dot(N)));
|
||||
var r1 = ClosestPointLine(v0, v1, closestPt);
|
||||
|
|
@ -229,24 +219,22 @@ class Collision {
|
|||
|
||||
var chosenEdge = 0; // Bitfield
|
||||
|
||||
var chosenPt:Vector;
|
||||
var chosenPt:Vector = new Vector();
|
||||
if (r1.distanceSq(P) < r2.distanceSq(P)) {
|
||||
chosenPt = r1;
|
||||
chosenPt.load(r1);
|
||||
chosenEdge = 1;
|
||||
} else {
|
||||
chosenPt = r2;
|
||||
chosenPt.load(r2);
|
||||
chosenEdge = 2;
|
||||
}
|
||||
if (chosenPt.distanceSq(P) < r3.distanceSq(P))
|
||||
res.point = chosenPt;
|
||||
point.load(chosenPt);
|
||||
else {
|
||||
chosenEdge = 4;
|
||||
res.point = r3;
|
||||
point.load(r3);
|
||||
}
|
||||
res.normal = P.sub(res.point).normalized();
|
||||
res.result = true;
|
||||
|
||||
res.resIdx = chosenEdge;
|
||||
normal.load(P.sub(point).normalized());
|
||||
return true;
|
||||
|
||||
// if (res.normal.dot(N) > 0.8) {
|
||||
// // Internal edge
|
||||
|
|
@ -263,7 +251,6 @@ class Collision {
|
|||
// }
|
||||
// }
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
public static function IntersectSegmentCapsule(segStart:Vector, segEnd:Vector, capStart:Vector, capEnd:Vector, radius:Float) {
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
package collision;
|
||||
|
||||
import collision.Collision.ITSResult;
|
||||
import collision.BVHTree.IBVHObject;
|
||||
import src.TimeState;
|
||||
import src.GameObject;
|
||||
|
|
@ -200,33 +201,31 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
|
|||
// var v0 = surface.points[surface.indices[i]].transformed(tform);
|
||||
// var v = surface.points[surface.indices[i + 1]].transformed(tform);
|
||||
// var v2 = surface.points[surface.indices[i + 2]].transformed(tform);
|
||||
var v0 = verts.v1;
|
||||
var v = verts.v2;
|
||||
var v2 = verts.v3;
|
||||
var v0 = new Vector(verts.v1x, verts.v1y, verts.v1z);
|
||||
var v = new Vector(verts.v2x, verts.v2y, verts.v2z);
|
||||
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) {
|
||||
var vn = v.sub(v0).cross(v2.sub(v0)).normalized().multiply(-1);
|
||||
var vdot = vn.dot(surfacenormal);
|
||||
if (vdot < 0.95) {
|
||||
v0 = verts.v1;
|
||||
v = verts.v3;
|
||||
v2 = verts.v2;
|
||||
v.set(verts.v3x, verts.v3y, verts.v3z);
|
||||
v2.set(verts.v2x, verts.v2y, verts.v2z);
|
||||
|
||||
surfacenormal = vn;
|
||||
surfacenormal.load(vn);
|
||||
}
|
||||
}
|
||||
|
||||
var res = Collision.TriangleSphereIntersection(v0, v, v2, surfacenormal, position, radius);
|
||||
var closest = res.point;
|
||||
var closest = new Vector();
|
||||
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);
|
||||
if (closest != null) {
|
||||
if (res) {
|
||||
var contactDist = closest.distanceSq(position);
|
||||
Debug.drawTriangle(v0, v, v2);
|
||||
// Debug.drawTriangle(v0, v, v2);
|
||||
if (contactDist <= radius * radius) {
|
||||
var normal = res.normal;
|
||||
|
||||
if (position.sub(closest).dot(surfacenormal) > 0) {
|
||||
normal.normalize();
|
||||
|
||||
|
|
@ -236,8 +235,8 @@ class CollisionEntity implements IOctreeObject implements IBVHObject {
|
|||
// bestDot = testDot;
|
||||
|
||||
var cinfo = new CollisionInfo();
|
||||
cinfo.normal = normal;
|
||||
cinfo.point = closest;
|
||||
cinfo.normal = normal.clone();
|
||||
cinfo.point = closest.clone();
|
||||
// cinfo.collider = this;
|
||||
cinfo.velocity = this.velocity.clone();
|
||||
cinfo.contactDistance = Math.sqrt(contactDist);
|
||||
|
|
|
|||
|
|
@ -6,6 +6,37 @@ import octree.IOctreeObject;
|
|||
import h3d.Vector;
|
||||
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 {
|
||||
public var priority:Int;
|
||||
public var position:Int;
|
||||
|
|
@ -147,7 +178,7 @@ class CollisionSurface implements IOctreeObject implements IBVHObject {
|
|||
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) {
|
||||
_transformedPoints = points.copy();
|
||||
}
|
||||
|
|
@ -182,12 +213,10 @@ class CollisionSurface implements IOctreeObject implements IBVHObject {
|
|||
_transformedPoints[p3 * 3 + 2] = pt.z;
|
||||
transformKeys[p3] = key;
|
||||
}
|
||||
return {
|
||||
v1: new Vector(_transformedPoints[p1 * 3], _transformedPoints[p1 * 3 + 1], _transformedPoints[p1 * 3 + 2]),
|
||||
v2: new Vector(_transformedPoints[p2 * 3], _transformedPoints[p2 * 3 + 1], _transformedPoints[p2 * 3 + 2]),
|
||||
v3: new Vector(_transformedPoints[p3 * 3], _transformedPoints[p3 * 3 + 1], _transformedPoints[p3 * 3 + 2]),
|
||||
n: new Vector(_transformedNormals[p1 * 3], _transformedNormals[p1 * 3 + 1], _transformedNormals[p1 * 3 + 2])
|
||||
};
|
||||
return new TransformedCollisionTriangle(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]),
|
||||
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]));
|
||||
}
|
||||
|
||||
public function dispose() {
|
||||
|
|
|
|||
|
|
@ -60,8 +60,12 @@ class CollisionWorld {
|
|||
|
||||
for (marb in marbleEntities) {
|
||||
if (marb != spherecollision) {
|
||||
if (spherecollision.go.isCollideable)
|
||||
contacts = contacts.concat(marb.sphereIntersection(spherecollision, timeState));
|
||||
if (spherecollision.go.isCollideable) {
|
||||
var isecs = marb.sphereIntersection(spherecollision, timeState);
|
||||
if (isecs.length > 0)
|
||||
foundEntities.push(marb);
|
||||
contacts = contacts.concat(isecs);
|
||||
}
|
||||
}
|
||||
}
|
||||
return {foundEntities: foundEntities, contacts: contacts};
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue