public virtual void GetAabb(BroadphaseProxy absproxy, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
DbvtProxy proxy = absproxy as DbvtProxy;
aabbMin = proxy.GetMinAABB();
aabbMax = proxy.GetMaxAABB();
}
//
// Helpers
//
//
public static void ListAppend(DbvtProxy item, ref DbvtProxy list)
{
item.links[0] = null;
item.links[1] = list;
if (list != null)
{
list.links[0] = item;
}
list = item;
}
//
public static int ListCount(DbvtProxy root)
{
int n = 0;
while (root != null)
{
++n;
root = root.links[1];
}
return(n);
}
//
public static void ListRemove(DbvtProxy item, ref DbvtProxy list)
{
if (item.links[0] != null)
{
item.links[0].links[1] = item.links[1];
}
else
{
list = item.links[1];
}
if (item.links[1] != null)
{
item.links[1].links[0] = item.links[0];
}
}
public void Process(DbvtNode na, DbvtNode nb)
{
if (na != nb)
{
DbvtProxy pa = na.data as DbvtProxy;
DbvtProxy pb = nb.data as DbvtProxy;
#if DBVT_BP_SORTPAIRS
if (pa.m_uniqueId > pb.m_uniqueId)
{
btSwap(pa, pb);
}
#endif
pbp.m_paircache.AddOverlappingPair(pa, pb);
++pbp.m_newpairs;
}
}
public virtual void DestroyProxy(BroadphaseProxy absproxy, IDispatcher dispatcher)
{
DbvtProxy proxy = absproxy as DbvtProxy;
if (proxy.stage == STAGECOUNT)
{
m_sets[1].Remove(proxy.leaf);
}
else
{
m_sets[0].Remove(proxy.leaf);
}
ListRemove(proxy, ref m_stageRoots[proxy.stage]);
// FIXME - MAN Hacky cleanup protection
if (m_paircache != null)
{
m_paircache.RemoveOverlappingPairsContainingProxy(proxy, dispatcher);
}
proxy = null;
m_needcleanup = true;
}
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);
}
///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);
}
}
}
public void Process(DbvtNode leaf)
{
DbvtProxy proxy = leaf.data as DbvtProxy;
m_aabbCallback.Process(proxy);
}
public void Initialize(DbvtBroadphase p)
{
pbp = p;
proxy = null;
}
} // for pool;
public DbvtTreeCollider(DbvtBroadphase p)
{
pbp = p;
proxy = null;
}
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();
}
