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