public void collideTV(Node root, Dispatcher dispatcher, DbvtAabbMm volume, short collisionFilterGroup, short collisionFilterMask, ICollide policy)
{
if (root != null)
{
List <Node> stack = new List <Node>(SIMPLE_STACKSIZE);
stack.Add(root);
do
{
Node n = stack[stack.Count - 1];
stack.RemoveAt(stack.Count - 1);
if (DbvtAabbMm.Intersect(n.volume, volume))
{
if (n.isinternal())
{
stack.Add(n.childs[0]);
stack.Add(n.childs[1]);
}
else
{
if (!dispatcher.needsCollision(collisionFilterGroup, collisionFilterMask, n.data.collisionFilterGroup, n.data.collisionFilterMask))
{
continue;
}
policy.Process(n);
}
}
} while (stack.Count > 0);
}
}
public override void getBroadphaseAabb(out VInt3 aabbMin, out VInt3 aabbMax)
{
DbvtAabbMm bounds = new DbvtAabbMm();
if (!sets[DYNAMIC_SET].empty())
{
if (!sets[FIXED_SET].empty())
{
DbvtAabbMm.Merge(sets[DYNAMIC_SET].root.volume, sets[FIXED_SET].root.volume, bounds);
}
else
{
bounds = new DbvtAabbMm(sets[DYNAMIC_SET].root.volume);
}
}
else if (!sets[FIXED_SET].empty())
{
bounds = new DbvtAabbMm(sets[FIXED_SET].root.volume);
}
else
{
DbvtAabbMm.FromCR(VInt3.zero, VFixedPoint.Zero, bounds);
}
aabbMin = bounds.Mins();
aabbMax = bounds.Maxs();
}
public static VFixedPoint Proximity(DbvtAabbMm a, DbvtAabbMm b)
{
VInt3 d = a.mi + a.mx;
VInt3 tmp = b.mi - b.mx;
d -= tmp;
return(d.x.Abs() + d.y.Abs() + d.z.Abs());
}
public static DbvtAabbMm FromCE(VInt3 c, VInt3 e, DbvtAabbMm output)
{
DbvtAabbMm box = output;
box.mi = c - e;
box.mx = c + e;
return(box);
}
public Node insert(DbvtAabbMm box, DbvtProxy data)
{
Node leaf = createnode(this, null, box, data);
insertleaf(this, leaf);
leaves++;
return(leaf);
}
public static DbvtAabbMm FromMM(VInt3 mi, VInt3 mx, DbvtAabbMm output)
{
DbvtAabbMm box = output;
box.mi = mi;
box.mx = mx;
return(box);
}
public static DbvtAabbMm FromVec(VInt3 from, VInt3 to, DbvtAabbMm output)
{
DbvtAabbMm box = output;
box.mi.x = FMath.Min(from.x, to.x); box.mi.y = FMath.Min(from.y, to.y); box.mi.z = FMath.Min(from.z, to.z);
box.mx.x = FMath.Max(from.x, to.x); box.mx.y = FMath.Max(from.y, to.y); box.mx.z = FMath.Max(from.z, to.z);
return(box);
}
public static bool Intersect(DbvtAabbMm a, DbvtAabbMm b)
{
return((a.mi.x <= b.mx.x) &&
(a.mx.x >= b.mi.x) &&
(a.mi.y <= b.mx.y) &&
(a.mx.y >= b.mi.y) &&
(a.mi.z <= b.mx.z) &&
(a.mx.z >= b.mi.z));
}
public bool Contain(DbvtAabbMm a)
{
return((mi.x <= a.mi.x) &&
(mi.y <= a.mi.y) &&
(mi.z <= a.mi.z) &&
(mx.x >= a.mx.x) &&
(mx.y >= a.mx.y) &&
(mx.z >= a.mx.z));
}
public static void collideTT(Node root0, Node root1, Dispatcher dispatcher, ICollide policy)
{
if (root0 != null && root1 != null)
{
List <sStkNN> stack = new List <sStkNN>(DOUBLE_STACKSIZE);
stack.Add(new sStkNN(root0, root1));
do
{
sStkNN p = stack[stack.Count - 1];
stack.RemoveAt(stack.Count - 1);
if (p.a == p.b)
{
if (p.a.isinternal())
{
stack.Add(new sStkNN(p.a.childs[0], p.a.childs[0]));
stack.Add(new sStkNN(p.a.childs[1], p.a.childs[1]));
stack.Add(new sStkNN(p.a.childs[0], p.a.childs[1]));
}
}
else if (DbvtAabbMm.Intersect(p.a.volume, p.b.volume))
{
if (p.a.isinternal())
{
if (p.b.isinternal())
{
stack.Add(new sStkNN(p.a.childs[0], p.b.childs[0]));
stack.Add(new sStkNN(p.a.childs[1], p.b.childs[0]));
stack.Add(new sStkNN(p.a.childs[0], p.b.childs[1]));
stack.Add(new sStkNN(p.a.childs[1], p.b.childs[1]));
}
else
{
stack.Add(new sStkNN(p.a.childs[0], p.b));
stack.Add(new sStkNN(p.a.childs[1], p.b));
}
}
else
{
if (p.b.isinternal())
{
stack.Add(new sStkNN(p.a, p.b.childs[0]));
stack.Add(new sStkNN(p.a, p.b.childs[1]));
}
else
{
if (!dispatcher.needsCollision(p.a.data.collisionFilterGroup, p.a.data.collisionFilterMask, p.b.data.collisionFilterGroup, p.b.data.collisionFilterMask))
{
continue;
}
policy.Process(p.a, p.b);
}
}
}
}while (stack.Count > 0);
}
}
private static Node createnode(Dbvt pdbvt, Node parent, DbvtAabbMm volume, DbvtProxy data)
{
Node node = new Node();
node.parent = parent;
node.volume = volume;
node.data = data;
node.height = 0;
return(node);
}
public override BroadphaseProxy createProxy(VInt3 aabbMin, VInt3 aabbMax, BroadphaseNativeType shapeType, CollisionObject collisionObject, short collisionFilterGroup, short collisionFilterMask, Dispatcher dispatcher)
{
DbvtProxy proxy = new DbvtProxy(collisionObject, collisionFilterGroup, collisionFilterMask);
DbvtAabbMm volume = DbvtAabbMm.FromMM(aabbMin, aabbMax, new DbvtAabbMm());
proxy.leaf = sets[DYNAMIC_SET].insert(volume, proxy);
proxy.stage = DYNAMIC_SET;
proxy.uniqueId = UUID.GetNextUUID();
stageRoots[DYNAMIC_SET] = listappend(proxy, stageRoots[DYNAMIC_SET]);
return(proxy);
}
public bool update(Node leaf, DbvtAabbMm volume)
{
if (leaf.volume.Contain(volume))
{
return(false);
}
removeleaf(this, leaf);
leaf.volume = volume;
insertleaf(this, leaf);
return(true);
}
private static void insertleaf(Dbvt pdbvt, Node leaf)
{
if (pdbvt.root == null)
{
pdbvt.root = leaf;
leaf.parent = null;
}
else
{
Node node = pdbvt.root;
while (!node.isleaf())
{
if (DbvtAabbMm.Proximity(node.childs[0].volume, leaf.volume) <
DbvtAabbMm.Proximity(node.childs[1].volume, leaf.volume))
{
node = node.childs[0];
}
else
{
node = node.childs[1];
}
}
Node sibling = node;
Node oldParent = node.parent;
Node newParent = createnode(pdbvt, oldParent, merge(leaf.volume, sibling.volume, new DbvtAabbMm()), null);
if (oldParent != null)
{
oldParent.childs[indexof(sibling)] = newParent;
newParent.childs[0] = sibling;
newParent.childs[1] = leaf;
sibling.parent = newParent;
leaf.parent = newParent;
}
else
{
newParent.childs[0] = sibling;
newParent.childs[1] = leaf;
sibling.parent = newParent;
leaf.parent = newParent;
pdbvt.root = newParent;
}
node = leaf.parent;
while (node != null)
{
node = Balance(pdbvt, node);
Node child0 = node.childs[0];
Node child1 = node.childs[1];
node.height = Math.Max(child0.height, child1.height) + 1;
node.volume = merge(child0.volume, child1.volume, new DbvtAabbMm());
node = node.parent;
}
}
}
public override void aabbTest(VInt3 aabbMin, VInt3 aabbMax, BroadphaseAabbCallback aabbCallback, Dispatcher dispatcher, short collisionFilterGroup, short collisionFilterMask)
{
BroadphaseAabbTester callback = new BroadphaseAabbTester(aabbCallback);
DbvtAabbMm bounds = new DbvtAabbMm();
DbvtAabbMm.FromMM(aabbMin, aabbMax, bounds);
//process all children, that overlap with the given AABB bounds
sets[DYNAMIC_SET].collideTV(sets[DYNAMIC_SET].root, dispatcher, bounds, collisionFilterGroup, collisionFilterMask, callback);
sets[FIXED_SET].collideTV(sets[FIXED_SET].root, dispatcher, bounds, collisionFilterGroup, collisionFilterMask, callback);
}
public static void swap(DbvtAabbMm p1, DbvtAabbMm p2)
{
VInt3 tmp = p1.mi;
p1.mi = p2.mi;
p2.mi = tmp;
tmp = p1.mx;
p1.mx = p2.mx;
p2.mx = tmp;
}
public static void Merge(DbvtAabbMm a, DbvtAabbMm b, DbvtAabbMm r)
{
r.mi.x = FMath.Min(a.mi.x, b.mi.x);
r.mx.x = FMath.Max(a.mx.x, b.mx.x);
r.mi.y = FMath.Min(a.mi.y, b.mi.y);
r.mx.y = FMath.Max(a.mx.y, b.mx.y);
r.mi.z = FMath.Min(a.mi.z, b.mi.z);
r.mx.z = FMath.Max(a.mx.z, b.mx.z);
}
public void collide(Dispatcher dispatcher)
{
DbvtTreeCollider collider = new DbvtTreeCollider(this);
//collide dynamics:
{
Dbvt.collideTT(sets[DYNAMIC_SET].root, sets[FIXED_SET].root, dispatcher, collider);
Dbvt.collideTT(sets[DYNAMIC_SET].root, sets[DYNAMIC_SET].root, dispatcher, collider);
}
//dynamic -> fixed set:
DbvtProxy current = stageRoots[DYNAMIC_SET];
while (current != null)
{
stageRoots[DYNAMIC_SET] = listremove(current, stageRoots[DYNAMIC_SET]);
stageRoots[FIXED_SET] = listappend(current, stageRoots[FIXED_SET]);
DbvtAabbMm volume = current.leaf.volume;
sets[DYNAMIC_SET].remove(current.leaf);
current.leaf = sets[FIXED_SET].insert(volume, current);
current.stage = FIXED_SET;
current = stageRoots[DYNAMIC_SET];
}
// clean up:
{
List <BroadphasePair> pairs = paircache.getOverlappingPairArray();
if (pairs.Count > 0)
{
for (int i = 0, ni = pairs.Count; i < ni; i++)
{
BroadphasePair p = pairs[i];
DbvtProxy pa = (DbvtProxy)p.pProxy0;
DbvtProxy pb = (DbvtProxy)p.pProxy1;
if (!DbvtAabbMm.Intersect(pa.leaf.volume, pb.leaf.volume))
{
if (pa.getUid() > pb.getUid())
{
DbvtProxy tmp = pa;
pa = pb;
pb = tmp;
}
paircache.removeOverlappingPair(pa, pb);
ni--;
i--;
}
}
}
}
}
public void SweepTest(CollisionObject testObject, VInt3 end, List <CastResult> results, short collisionFilterGroup = CollisionFilterGroups.DEFAULT_FILTER, short collisionFilterMask = CollisionFilterGroups.ALL_FILTER)
{
VInt3 aabbMin, aabbMax;
testObject.getCollisionShape().getAabb(VIntTransform.Identity, out aabbMin, out aabbMax);
VInt3 start = testObject.getWorldTransform().position;
VInt3 dir = end - start;
if (dir.sqrMagnitude < Globals.EPS2)
{
return;
}
DbvtAabbMm aabb = new DbvtAabbMm();
aabb = DbvtAabbMm.FromVec(start, end, aabb);
aabb.Expand(aabbMax);
SingleSweepCallback sweepCB = new SingleSweepCallback(testObject, end, dispatcher1, results);
broadphase.aabbTest(aabb.Mins(), aabb.Maxs(), sweepCB, dispatcher1, collisionFilterGroup, collisionFilterMask);
}
public override void setAabb(BroadphaseProxy absproxy, VInt3 aabbMin, VInt3 aabbMax, Dispatcher dispatcher)
{
DbvtProxy proxy = (DbvtProxy)absproxy;
DbvtAabbMm aabb = DbvtAabbMm.FromMM(aabbMin, aabbMax, new DbvtAabbMm());
if (aabb != proxy.leaf.volume)
{
proxy.leaf.volume = aabb;
if (proxy.stage == FIXED_SET)
{
sets[FIXED_SET].remove(proxy.leaf);
stageRoots[FIXED_SET] = listremove(proxy, stageRoots[FIXED_SET]);
proxy.leaf = sets[DYNAMIC_SET].insert(aabb, proxy);
proxy.stage = DYNAMIC_SET;
stageRoots[DYNAMIC_SET] = listappend(proxy, stageRoots[DYNAMIC_SET]);
}
else
{
// teleporting:
sets[DYNAMIC_SET].update(proxy.leaf, aabb);
}
}
}
private static DbvtAabbMm merge(DbvtAabbMm a, DbvtAabbMm b, DbvtAabbMm output)
{
DbvtAabbMm.Merge(a, b, output);
return(output);
}
public static DbvtAabbMm FromCR(VInt3 c, VFixedPoint r, DbvtAabbMm output)
{
VInt3 tmp = new VInt3(r, r, r);
return(FromCE(c, tmp, output));
}
public DbvtAabbMm(DbvtAabbMm o)
{
mi = o.mi;
mx = o.mx;
}
