예제 #1
0
 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);
     }
 }
예제 #2
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();
        }
예제 #3
0
        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());
        }
예제 #4
0
        public static DbvtAabbMm FromCE(VInt3 c, VInt3 e, DbvtAabbMm output)
        {
            DbvtAabbMm box = output;

            box.mi = c - e;
            box.mx = c + e;
            return(box);
        }
예제 #5
0
        public Node insert(DbvtAabbMm box, DbvtProxy data)
        {
            Node leaf = createnode(this, null, box, data);

            insertleaf(this, leaf);
            leaves++;
            return(leaf);
        }
예제 #6
0
        public static DbvtAabbMm FromMM(VInt3 mi, VInt3 mx, DbvtAabbMm output)
        {
            DbvtAabbMm box = output;

            box.mi = mi;
            box.mx = mx;
            return(box);
        }
예제 #7
0
        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);
        }
예제 #8
0
 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));
 }
예제 #9
0
 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));
 }
예제 #10
0
 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);
     }
 }
예제 #11
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);
        }
예제 #12
0
        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);
        }
예제 #13
0
 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);
 }
예제 #14
0
        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;
                }
            }
        }
예제 #15
0
        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);
        }
예제 #16
0
        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;
        }
예제 #17
0
        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);
        }
예제 #18
0
        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--;
                        }
                    }
                }
            }
        }
예제 #19
0
        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);
        }
예제 #20
0
        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);
                }
            }
        }
예제 #21
0
 private static DbvtAabbMm merge(DbvtAabbMm a, DbvtAabbMm b, DbvtAabbMm output)
 {
     DbvtAabbMm.Merge(a, b, output);
     return(output);
 }
예제 #22
0
        public static DbvtAabbMm FromCR(VInt3 c, VFixedPoint r, DbvtAabbMm output)
        {
            VInt3 tmp = new VInt3(r, r, r);

            return(FromCE(c, tmp, output));
        }
예제 #23
0
 public DbvtAabbMm(DbvtAabbMm o)
 {
     mi = o.mi;
     mx = o.mx;
 }