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--; }
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); } }
public static void CollideTT(DbvtNode root0, DbvtNode root1, ICollide collideable) { CollideTTCount++; Debug.Assert(CollideTTCount < 2); CollideTTStack.Clear(); if (root0 != null && root1 != null) { int depth = 1; int treshold = DOUBLE_STACKSIZE - 4; CollideTTStack[0] = new sStkNN(root0, root1); do { sStkNN p = CollideTTStack[--depth]; if (depth > treshold) { CollideTTStack.Resize(CollideTTStack.Count * 2); treshold = CollideTTStack.Count - 4; } if (p.a == p.b) { if (p.a.IsInternal()) { CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.a._children[0]); CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.a._children[1]); CollideTTStack[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()) { CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.b._children[0]); CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.b._children[0]); CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.b._children[1]); CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.b._children[1]); } else { CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.b); CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.b); } } else { if (p.b.IsInternal()) { CollideTTStack[depth++] = new sStkNN(p.a, p.b._children[0]); CollideTTStack[depth++] = new sStkNN(p.a, p.b._children[1]); } else { collideable.Process(p.a, p.b); } } } } while (depth > 0); } CollideTTCount--; }
public void PerformDeferredRemoval(IDispatcher dispatcher) { if (m_paircache.HasDeferredRemoval()) { ObjectArray <BroadphasePair> overlappingPairArray = m_paircache.GetOverlappingPairArray(); //perform a sort, to find duplicates and to sort 'invalid' pairs to the end overlappingPairArray.QuickSort(new BroadphasePairQuickSort()); int invalidPair = 0; int i; BroadphasePair previousPair = new BroadphasePair(); for (i = 0; i < overlappingPairArray.Count; i++) { BroadphasePair pair = overlappingPairArray[i]; bool isDuplicate = (pair == previousPair); previousPair = pair; bool needsRemoval = false; if (!isDuplicate) { //important to perform AABB check that is consistent with the broadphase DbvtProxy pa = pair.m_pProxy0 as DbvtProxy; DbvtProxy pb = pair.m_pProxy1 as DbvtProxy; bool hasOverlap = DbvtAabbMm.Intersect(ref pa.leaf.volume, ref pb.leaf.volume); if (hasOverlap) { needsRemoval = false; } else { needsRemoval = true; } } else { //remove duplicate needsRemoval = true; //should have no algorithm Debug.Assert(pair.m_algorithm != null); } if (needsRemoval) { m_paircache.CleanOverlappingPair(pair, dispatcher); pair.m_pProxy0 = null; pair.m_pProxy1 = null; invalidPair++; } } if (invalidPair > 0) { if (invalidPair < overlappingPairArray.Count) { int ibreak = 0; } //perform a sort, to sort 'invalid' pairs to the end overlappingPairArray.QuickSort(new BroadphasePairQuickSort()); //overlappingPairArray.resize(overlappingPairArray.size() - invalidPair); overlappingPairArray.Truncate(invalidPair); } } }
public virtual void SetAabb(BroadphaseProxy absproxy, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax, IDispatcher dispatcher) { DbvtProxy proxy = absproxy as DbvtProxy; DbvtAabbMm aabb = DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax); #if DBVT_BP_PREVENTFALSEUPDATE if (NotEqual(ref aabb, proxy, leaf.volume)) #endif { bool docollide = false; if (proxy.stage == STAGECOUNT) {/* fixed . dynamic set */ m_sets[1].Remove(proxy.leaf); proxy.leaf = m_sets[0].Insert(ref aabb, proxy); docollide = true; } else {/* dynamic set */ ++m_updates_call; if (DbvtAabbMm.Intersect(ref proxy.leaf.volume, ref aabb)) {/* Moving */ IndexedVector3 delta = aabbMin - proxy.m_aabbMin; IndexedVector3 velocity = (((proxy.m_aabbMax - proxy.m_aabbMin) / 2f) * m_prediction); if (delta.X < 0) { velocity.X = -velocity.X; } if (delta.Y < 0) { velocity.Y = -velocity.Y; } if (delta.Z < 0) { velocity.Z = -velocity.Z; } if ( #if DBVT_BP_MARGIN m_sets[0].Update(proxy.leaf, ref aabb, ref velocity, DBVT_BP_MARGIN) #else m_sets[0].update(proxy.leaf, aabb, ref velocity) #endif ) { ++m_updates_done; docollide = true; } } else {/* Teleporting */ m_sets[0].Update(proxy.leaf, ref aabb); ++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); } } } }
public void Collide(IDispatcher dispatcher) { BulletGlobals.StartProfile("BroadphaseCollide"); //SPC(m_profiling.m_total); /* optimize */ m_sets[0].OptimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100); if (m_fixedleft > 0) { int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100; m_sets[1].OptimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100); m_fixedleft = Math.Max(0, m_fixedleft - count); } /* dynamic . fixed set */ m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT; DbvtProxy current = m_stageRoots[m_stageCurrent]; if (current != null) { DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get(); collider.Initialize(this); do { DbvtProxy next = current.links[1]; ListRemove(current, ref m_stageRoots[current.stage]); ListAppend(current, ref m_stageRoots[STAGECOUNT]); #if DBVT_BP_ACCURATESLEEPING m_paircache.removeOverlappingPairsContainingProxy(current, dispatcher); collider.proxy = current; btDbvt::collideTV(m_sets[0].m_root, current.aabb, collider); btDbvt::collideTV(m_sets[1].m_root, current.aabb, collider); #endif m_sets[0].Remove(current.leaf); DbvtAabbMm curAabb = DbvtAabbMm.FromMM(ref current.m_aabbMin, ref current.m_aabbMax); current.leaf = m_sets[1].Insert(ref curAabb, current); current.stage = STAGECOUNT; current = next; } while (current != null); m_fixedleft = m_sets[1].m_leaves; BulletGlobals.DbvtTreeColliderPool.Free(collider); m_needcleanup = true; } /* collide dynamics */ { DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get(); collider.Initialize(this); if (m_deferedcollide) { //Stopwatch fdCollideStopwatch = new Stopwatch(); //fdCollideStopwatch.Start(); //SPC(m_profiling.m_fdcollide); Dbvt.CollideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider); //fdCollideStopwatch.Stop(); //m_profiling.m_fdcollide += (ulong)fdCollideStopwatch.ElapsedMilliseconds; } if (m_deferedcollide) { //Stopwatch ddCollideStopwatch = new Stopwatch(); //ddCollideStopwatch.Start(); //SPC(m_profiling.m_ddcollide); Dbvt.CollideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider); //ddCollideStopwatch.Stop(); //m_profiling.m_ddcollide += (ulong)ddCollideStopwatch.ElapsedMilliseconds; } BulletGlobals.DbvtTreeColliderPool.Free(collider); } /* clean up */ if (m_needcleanup) { Stopwatch cleanupStopwatch = new Stopwatch(); cleanupStopwatch.Start(); //SPC(m_profiling.m_cleanup); IList <BroadphasePair> pairs = m_paircache.GetOverlappingPairArray(); if (pairs.Count > 0) { int ni = Math.Min(pairs.Count, Math.Max(m_newpairs, (pairs.Count * m_cupdates) / 100)); for (int i = 0; i < ni; ++i) { BroadphasePair p = pairs[(m_cid + i) % pairs.Count]; DbvtProxy pa = p.m_pProxy0 as DbvtProxy; DbvtProxy pb = p.m_pProxy1 as DbvtProxy; if (!DbvtAabbMm.Intersect(ref pa.leaf.volume, ref pb.leaf.volume)) { #if DBVT_BP_SORTPAIRS if (pa.m_uniqueId > pb.m_uniqueId) { btSwap(pa, pb); } #endif m_paircache.RemoveOverlappingPair(pa, pb, dispatcher); --ni; --i; } } if (pairs.Count > 0) { m_cid = (m_cid + ni) % pairs.Count; } else { m_cid = 0; } } cleanupStopwatch.Stop(); //m_profiling.m_cleanup += (ulong)cleanupStopwatch.ElapsedMilliseconds; } ++m_pid; m_newpairs = 1; m_needcleanup = false; if (m_updates_call > 0) { m_updates_ratio = m_updates_done / (float)m_updates_call; } else { m_updates_ratio = 0; } m_updates_done /= 2; m_updates_call /= 2; BulletGlobals.StopProfile(); }