Ejemplo n.º 1
0
        public static void Swap(ref DbvtAabbMm a, ref DbvtAabbMm b)
        {
            DbvtAabbMm temp = b;

            b = a;
            a = temp;
        }
Ejemplo n.º 2
0
 public static void CollideTV(DbvtNode root, ref DbvtAabbMm volume, ICollide collideable)
 {
     CollideTVCount++;
     Debug.Assert(CollideTVCount < 2);
     CollideTVStack.Clear();
     if (root != null)
     {
         CollideTVStack.Push(root);
         do
         {
             DbvtNode n = CollideTVStack.Pop();
             if (DbvtAabbMm.Intersect(ref n.volume, ref volume))
             {
                 if (n.IsInternal())
                 {
                     CollideTVStack.Push(n._children[0]);
                     CollideTVStack.Push(n._children[1]);
                 }
                 else
                 {
                     collideable.Process(n);
                 }
             }
         } while (CollideTVStack.Count > 0);
     }
     CollideTVCount--;
 }
Ejemplo n.º 3
0
        public static DbvtAabbMm Merge(ref DbvtAabbMm a, ref DbvtAabbMm b)
        {
            DbvtAabbMm res = new DbvtAabbMm();

            Merge(ref a, ref b, ref res);
            return(res);
        }
Ejemplo n.º 4
0
        // volume+edge lengths
        public static float Size(ref DbvtAabbMm a)
        {
            IndexedVector3 edges = a.Lengths();

            return(edges.X * edges.Y * edges.Z +
                   edges.X + edges.Y + edges.Z);
        }
Ejemplo n.º 5
0
        public virtual void GetBroadphaseAabb(out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
        {
            DbvtAabbMm bounds = new DbvtAabbMm();

            if (!m_sets[0].Empty())
            {
                if (!m_sets[1].Empty())
                {
                    DbvtAabbMm.Merge(ref m_sets[0].m_root.volume, ref m_sets[1].m_root.volume, ref bounds);
                }
                else
                {
                    bounds = m_sets[0].m_root.volume;
                }
            }
            else if (!m_sets[1].Empty())
            {
                bounds = m_sets[1].m_root.volume;
            }
            else
            {
                IndexedVector3 temp = IndexedVector3.Zero;
                bounds = DbvtAabbMm.FromCR(ref temp, 0);
            }
            aabbMin = bounds.Mins();
            aabbMax = bounds.Maxs();
        }
Ejemplo n.º 6
0
 //
 public static void BottomUp(Dbvt pdbvt, ObjectArray <DbvtNode> leaves)
 {
     while (leaves.Count > 1)
     {
         float minsize = float.MaxValue;
         int[] minidx  = { -1, -1 };
         for (int i = 0; i < leaves.Count; ++i)
         {
             for (int j = i + 1; j < leaves.Count; ++j)
             {
                 DbvtAabbMm mergeResults = DbvtAabbMm.Merge(ref leaves[i].volume, ref leaves[j].volume);
                 float      sz           = Size(ref mergeResults);
                 if (sz < minsize)
                 {
                     minsize   = sz;
                     minidx[0] = i;
                     minidx[1] = j;
                 }
             }
         }
         DbvtNode[] n = { leaves[minidx[0]], leaves[minidx[1]] };
         DbvtNode   p = CreateNode(pdbvt, null, ref n[0].volume, ref n[1].volume, null);
         p._children[0]    = n[0];
         p._children[1]    = n[1];
         n[0].parent       = p;
         n[1].parent       = p;
         leaves[minidx[0]] = p;
         leaves.Swap(minidx[1], leaves.Count - 1);
         leaves.PopBack();
     }
 }
Ejemplo n.º 7
0
        public void AddChildShape(ref IndexedMatrix localTransform, CollisionShape shape)
        {
            m_updateRevision++;
            //m_childTransforms.push_back(localTransform);
            //m_childShapes.push_back(shape);
            CompoundShapeChild child = new CompoundShapeChild();

            child.m_transform      = localTransform;
            child.m_childShape     = shape;
            child.m_childShapeType = shape.GetShapeType();
            child.m_childMargin    = shape.GetMargin();

            //extend the local aabbMin/aabbMax
            IndexedVector3 localAabbMin;
            IndexedVector3 localAabbMax;

            shape.GetAabb(ref localTransform, out localAabbMin, out localAabbMax);
            MathUtil.VectorMin(ref localAabbMin, ref m_localAabbMin);
            MathUtil.VectorMax(ref localAabbMax, ref m_localAabbMax);

            if (m_dynamicAabbTree != null)
            {
                DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
                int        index  = m_children.Count;
                child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (object)index);
            }

            m_children.Add(child);
        }
Ejemplo n.º 8
0
        public DbvtNode Insert(ref DbvtAabbMm box, Object data)
        {
            DbvtNode leaf = CreateNode(this, null, ref box, data);

            InsertLeaf(this, Root, leaf);
            ++m_leaves;
            return(leaf);
        }
Ejemplo n.º 9
0
 public static void Merge(ref DbvtAabbMm a, ref DbvtAabbMm b, ref DbvtAabbMm r)
 {
     //r = a;
     //SetMin(ref r._min, ref b._min);
     //SetMax(ref r._max, ref b._max);
     MathUtil.VectorMin(ref a._min, ref b._min, out r._min);
     MathUtil.VectorMax(ref a._max, ref b._max, out r._max);
 }
Ejemplo n.º 10
0
        public virtual void AabbTest(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, IBroadphaseAabbCallback aabbCallback)
        {
            BroadphaseAabbTester callback = new BroadphaseAabbTester(aabbCallback);
            DbvtAabbMm           bounds   = DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax);

            Dbvt.CollideTV(m_sets[0].m_root, ref bounds, callback);
            Dbvt.CollideTV(m_sets[1].m_root, ref bounds, callback);
        }
Ejemplo n.º 11
0
 public static bool Intersect(DbvtAabbMm a, ref IndexedVector3 b)
 {
     return((b.X >= a._min.X) &&
            (b.Y >= a._min.Y) &&
            (b.Z >= a._min.Z) &&
            (b.X <= a._max.X) &&
            (b.Y <= a._max.Y) &&
            (b.Z <= a._max.Z));
 }
Ejemplo n.º 12
0
 public static bool Intersect(ref DbvtAabbMm a, ref DbvtAabbMm b)
 {
     return((a._min.X <= b._max.X) &&
            (a._max.X >= b._min.X) &&
            (a._min.Y <= b._max.Y) &&
            (a._max.Y >= b._min.Y) &&
            (a._min.Z <= b._max.Z) &&
            (a._max.Z >= b._min.Z));
 }
Ejemplo n.º 13
0
 public bool Contain(ref DbvtAabbMm a)
 {
     return((_min.X <= a._min.X) &&
            (_min.Y <= a._min.Y) &&
            (_min.Z <= a._min.Z) &&
            (_max.X >= a._max.X) &&
            (_max.Y >= a._max.Y) &&
            (_max.Z >= a._max.Z));
 }
Ejemplo n.º 14
0
 public static bool NotEqual(ref DbvtAabbMm a, ref DbvtAabbMm b)
 {
     return((a._min.X != b._min.X) ||
            (a._min.Y != b._min.Y) ||
            (a._min.Z != b._min.Z) ||
            (a._max.X != b._max.X) ||
            (a._max.Y != b._max.Y) ||
            (a._max.Z != b._max.Z));
 }
Ejemplo n.º 15
0
 public bool Update(DbvtNode leaf, ref DbvtAabbMm volume, ref IndexedVector3 velocity)
 {
     if (leaf.volume.Contain(ref volume))
     {
         return(false);
     }
     volume.SignedExpand(velocity);
     Update(leaf, ref volume);
     return(true);
 }
Ejemplo n.º 16
0
 public bool Update(DbvtNode leaf, ref DbvtAabbMm volume, float margin)
 {
     if (leaf.volume.Contain(ref volume))
     {
         return(false);
     }
     volume.Expand(new IndexedVector3(margin));
     Update(leaf, ref volume);
     return(true);
 }
Ejemplo n.º 17
0
        //
        public static DbvtNode CreateNode(Dbvt pdbvt,
                                          DbvtNode parent,
                                          ref DbvtAabbMm volume,
                                          Object data)
        {
            DbvtNode node = CreateNode(pdbvt, parent, data);

            node.volume = volume;
            return(node);
        }
Ejemplo n.º 18
0
        //
        public static DbvtAabbMm Bounds(ObjectArray <DbvtNode> leafs)
        {
            DbvtAabbMm volume = leafs[0].volume;

            for (int i = 1, ni = leafs.Count; i < ni; ++i)
            {
                DbvtAabbMm.Merge(ref volume, ref leafs[i].volume, ref volume);
            }
            return(volume);
        }
Ejemplo n.º 19
0
        //
        public static DbvtNode CreateNode(Dbvt pdbvt,
                                          DbvtNode parent,
                                          ref DbvtAabbMm volume0,
                                          ref DbvtAabbMm volume1,
                                          Object data)
        {
            DbvtNode node = CreateNode(pdbvt, parent, data);

            DbvtAabbMm.Merge(ref volume0, ref volume1, ref node.volume);
            return(node);
        }
Ejemplo n.º 20
0
 public DbvtNode(Dbvt tree, DbvtNode aparent, ref DbvtAabbMm avolume, Object adata)
     : this()
 {
     volume = avolume;
     parent = aparent;
     data   = adata;
     if (data is int)
     {
         dataAsInt = (int)data;
     }
 }
Ejemplo n.º 21
0
        public static void InsertLeaf(Dbvt pdbvt, DbvtNode root, DbvtNode leaf)
        {
            if (pdbvt.Root == null)
            {
                pdbvt.Root  = leaf;
                leaf.parent = null;
            }
            else
            {
                if (!root.IsLeaf())
                {
                    do
                    {
                        root = root._children[DbvtAabbMm.Select(ref leaf.volume,
                                                                ref root._children[0].volume,
                                                                ref root._children[1].volume)];
                    } while (!root.IsLeaf());
                }
                DbvtNode   prev         = root.parent;
                DbvtAabbMm mergeResults = DbvtAabbMm.Merge(ref leaf.volume, ref root.volume);

                DbvtNode node = CreateNode2(pdbvt, prev, ref mergeResults, null);
                if (prev != null)
                {
                    prev._children[IndexOf(root)] = node;
                    node._children[0]             = root;
                    root.parent       = node;
                    node._children[1] = leaf;
                    leaf.parent       = node;
                    do
                    {
                        if (!prev.volume.Contain(ref node.volume))
                        {
                            DbvtAabbMm.Merge(ref prev._children[0].volume, ref prev._children[1].volume, ref prev.volume);
                        }
                        else
                        {
                            break;
                        }
                        node = prev;
                    } while (null != (prev = node.parent));
                }
                else
                {
                    node._children[0] = root;
                    root.parent       = node;
                    node._children[1] = leaf;
                    leaf.parent       = node;
                    pdbvt.Root        = node;
                }
            }
        }
Ejemplo n.º 22
0
        public static DbvtNode CreateNode2(Dbvt tree, DbvtNode aparent, ref DbvtAabbMm avolume, Object adata)
        {
            DbvtNode node = BulletGlobals.DbvtNodePool.Get();

            node.volume       = avolume;
            node.parent       = aparent;
            node.data         = adata;
            node._children[0] = null;
            node._children[1] = null;

            if (node.data is int)
            {
                Debug.Assert(false);
                node.dataAsInt = (int)node.data;
            }

            return(node);
        }
Ejemplo n.º 23
0
        public void CreateAabbTreeFromChildren()
        {
            if (m_dynamicAabbTree == null)
            {
                m_dynamicAabbTree = new Dbvt();

                for (int index = 0; index < m_children.Count; index++)
                {
                    CompoundShapeChild child = m_children[index];

                    //extend the local aabbMin/aabbMax
                    IndexedVector3 localAabbMin, localAabbMax;
                    child.m_childShape.GetAabb(ref child.m_transform, out localAabbMin, out localAabbMax);

                    DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
                    child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (object)index);
                }
            }
        }
Ejemplo n.º 24
0
 public static DbvtNode RemoveLeaf(Dbvt pdbvt, DbvtNode leaf)
 {
     if (leaf == pdbvt.Root)
     {
         pdbvt.Root = null;
         return(null);
     }
     else
     {
         DbvtNode parent  = leaf.parent;
         DbvtNode prev    = parent.parent;
         DbvtNode sibling = parent._children[1 - IndexOf(leaf)];
         if (prev != null)
         {
             prev._children[IndexOf(parent)] = sibling;
             sibling.parent = prev;
             DeleteNode(pdbvt, parent);
             while (prev != null)
             {
                 DbvtAabbMm pb = prev.volume;
                 DbvtAabbMm.Merge(ref prev._children[0].volume, ref prev._children[1].volume, ref prev.volume);
                 if (DbvtAabbMm.NotEqual(ref pb, ref prev.volume))
                 {
                     sibling = prev;
                     prev    = prev.parent;
                 }
                 else
                 {
                     break;
                 }
             }
             return(prev != null ? prev : pdbvt.Root);
         }
         else
         {
             pdbvt.Root     = sibling;
             sibling.parent = null;
             DeleteNode(pdbvt, parent);
             return(pdbvt.Root);
         }
     }
 }
Ejemplo n.º 25
0
        public void UpdateChildTransform(int childIndex, ref IndexedMatrix newChildTransform, bool shouldRecalculateLocalAabb)
        {
            m_children[childIndex].m_transform = newChildTransform;

            if (m_dynamicAabbTree != null)
            {
                ///update the dynamic aabb tree
                IndexedVector3 localAabbMin;
                IndexedVector3 localAabbMax;
                m_children[childIndex].m_childShape.GetAabb(ref newChildTransform, out localAabbMin, out localAabbMax);
                DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
                //int index = m_children.Count - 1;
                m_dynamicAabbTree.Update(m_children[childIndex].m_treeNode, ref bounds);
            }

            if (shouldRecalculateLocalAabb)
            {
                RecalculateLocalAabb();
            }
        }
Ejemplo n.º 26
0
        public void Update(DbvtNode leaf, ref DbvtAabbMm volume)
        {
            DbvtNode root = RemoveLeaf(this, leaf);

            if (root != null)
            {
                if (m_lkhd >= 0)
                {
                    for (int i = 0; (i < m_lkhd) && (root.parent != null); ++i)
                    {
                        root = root.parent;
                    }
                }
                else
                {
                    root = Root;
                }
            }
            leaf.volume = volume;
            InsertLeaf(this, root, leaf);
        }
Ejemplo n.º 27
0
        public virtual BroadphaseProxy CreateProxy(ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, BroadphaseNativeTypes shapeType, Object userPtr, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask, IDispatcher dispatcher, Object multiSapProxy)
        {
            DbvtProxy proxy = new DbvtProxy(ref aabbMin, ref aabbMax, userPtr, collisionFilterGroup, collisionFilterMask);

            DbvtAabbMm aabb = DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax);

            //bproxy.aabb			=	btDbvtAabbMm::FromMM(aabbMin,aabbMax);
            proxy.stage      = m_stageCurrent;
            proxy.m_uniqueId = ++m_gid;
            proxy.leaf       = m_sets[0].Insert(ref aabb, proxy);
            ListAppend(proxy, ref m_stageRoots[m_stageCurrent]);
            if (!m_deferedcollide)
            {
                DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get();
                collider.Initialize(this);
                collider.proxy = proxy;
                Dbvt.CollideTV(m_sets[0].m_root, ref aabb, collider);
                Dbvt.CollideTV(m_sets[1].m_root, ref aabb, collider);
                BulletGlobals.DbvtTreeColliderPool.Free(collider);
            }
            return(proxy);
        }
Ejemplo n.º 28
0
        ///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
        ///it is not part of the btBroadphaseInterface but specific to btDbvtBroadphase.
        ///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
        ///http://code.google.com/p/bullet/issues/detail?id=223
        public void SetAabbForceUpdate(BroadphaseProxy absproxy, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, IDispatcher dispatcher)
        {
            DbvtProxy  proxy     = absproxy as DbvtProxy;
            DbvtAabbMm bounds    = DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax);
            bool       docollide = false;

            if (proxy.stage == STAGECOUNT)
            {/* fixed . dynamic set	*/
                m_sets[1].Remove(proxy.leaf);
                proxy.leaf = m_sets[0].Insert(ref bounds, proxy);
                docollide  = true;
            }
            else
            {/* dynamic set				*/
                ++m_updates_call;
                /* Teleporting			*/
                m_sets[0].Update(proxy.leaf, ref bounds);
                ++m_updates_done;
                docollide = true;
            }
            ListRemove(proxy, ref m_stageRoots[proxy.stage]);
            proxy.m_aabbMin = aabbMin;
            proxy.m_aabbMax = aabbMax;
            proxy.stage     = m_stageCurrent;
            ListAppend(proxy, ref m_stageRoots[m_stageCurrent]);
            if (docollide)
            {
                m_needcleanup = true;
                if (!m_deferedcollide)
                {
                    DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get();
                    collider.Initialize(this);
                    Dbvt.CollideTTpersistentStack(m_sets[1].m_root, proxy.leaf, collider);
                    Dbvt.CollideTTpersistentStack(m_sets[0].m_root, proxy.leaf, collider);
                    BulletGlobals.DbvtTreeColliderPool.Free(collider);
                }
            }
        }
Ejemplo n.º 29
0
        public static DbvtNode TopDown(Dbvt pdbvt, ObjectArray <DbvtNode> leaves, int bu_treshold)
        {
            if (leaves.Count > 1)
            {
                if (leaves.Count > bu_treshold)
                {
                    DbvtAabbMm               vol  = Bounds(leaves);
                    IndexedVector3           org  = vol.Center();
                    ObjectArray <DbvtNode>[] sets = { new ObjectArray <DbvtNode>(), new ObjectArray <DbvtNode>() };
                    int   bestaxis = -1;
                    int   bestmidp = leaves.Count;
                    int[] a1       = new int[] { 0, 0 };
                    int[] a2       = new int[] { 0, 0 };
                    int[] a3       = new int[] { 0, 0 };

                    int[][] splitcount = new int[][] { a1, a2, a3 };
                    int     i;
                    for (i = 0; i < leaves.Count; ++i)
                    {
                        IndexedVector3 x = leaves[i].volume.Center() - org;
                        for (int j = 0; j < 3; ++j)
                        {
                            ++splitcount[j][IndexedVector3.Dot(x, axis[j]) > 0 ? 1 : 0];
                        }
                    }
                    for (i = 0; i < 3; ++i)
                    {
                        if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
                        {
                            int midp = (int)Math.Abs((splitcount[i][0] - splitcount[i][1]));
                            if (midp < bestmidp)
                            {
                                bestaxis = i;
                                bestmidp = midp;
                            }
                        }
                    }
                    if (bestaxis >= 0)
                    {
                        sets[0].EnsureCapacity(splitcount[bestaxis][0]);
                        sets[1].EnsureCapacity(splitcount[bestaxis][1]);
                        Split(leaves, sets[0], sets[1], ref org, ref axis[bestaxis]);
                    }
                    else
                    {
                        sets[0].EnsureCapacity(leaves.Count / 2 + 1);
                        sets[1].EnsureCapacity(leaves.Count / 2);
                        for (int i2 = 0, ni = leaves.Count; i2 < ni; ++i2)
                        {
                            sets[i2 & 1].Add(leaves[i2]);
                        }
                    }
                    DbvtNode node = CreateNode(pdbvt, null, ref vol, null);
                    node._children[0]        = TopDown(pdbvt, sets[0], bu_treshold);
                    node._children[1]        = TopDown(pdbvt, sets[1], bu_treshold);
                    node._children[0].parent = node;
                    node._children[1].parent = node;
                    return(node);
                }
                else
                {
                    BottomUp(pdbvt, leaves);
                    return(leaves[0]);
                }
            }
            return(leaves[0]);
        }
Ejemplo n.º 30
0
        public static void CollideTTpersistentStack(DbvtNode root0,
                                                    DbvtNode root1,
                                                    ICollide collideable)
        {
            //CollideTT(root0, root1, collideable);
            //return;
            if (root0 != null && root1 != null)
            {
                int depth    = 1;
                int treshold = DOUBLE_STACKSIZE - 4;

                m_stkStack.Resize(DOUBLE_STACKSIZE);
                m_stkStack[0] = new sStkNN(root0, root1);
                do
                {
                    sStkNN p = m_stkStack[--depth];
                    if (depth > treshold)
                    {
                        m_stkStack.Resize(m_stkStack.Count * 2);
                        treshold = m_stkStack.Count - 4;
                    }
                    if (p.a == p.b)
                    {
                        if (p.a.IsInternal())
                        {
                            m_stkStack[depth++] = new sStkNN(p.a._children[0], p.a._children[0]);
                            m_stkStack[depth++] = new sStkNN(p.a._children[1], p.a._children[1]);
                            m_stkStack[depth++] = new sStkNN(p.a._children[0], p.a._children[1]);
                        }
                    }
                    else if (DbvtAabbMm.Intersect(ref p.a.volume, ref p.b.volume))
                    {
                        if (p.a.IsInternal())
                        {
                            if (p.b.IsInternal())
                            {
                                m_stkStack[depth++] = new sStkNN(p.a._children[0], p.b._children[0]);
                                m_stkStack[depth++] = new sStkNN(p.a._children[1], p.b._children[0]);
                                m_stkStack[depth++] = new sStkNN(p.a._children[0], p.b._children[1]);
                                m_stkStack[depth++] = new sStkNN(p.a._children[1], p.b._children[1]);
                            }
                            else
                            {
                                m_stkStack[depth++] = new sStkNN(p.a._children[0], p.b);
                                m_stkStack[depth++] = new sStkNN(p.a._children[1], p.b);
                            }
                        }
                        else
                        {
                            if (p.b.IsInternal())
                            {
                                m_stkStack[depth++] = new sStkNN(p.a, p.b._children[0]);
                                m_stkStack[depth++] = new sStkNN(p.a, p.b._children[1]);
                            }
                            else
                            {
                                collideable.Process(p.a, p.b);
                            }
                        }
                    }
                } while (depth > 0);
            }
        }