public static void DeleteNode(Dbvt pdbvt, DbvtNode node)
{
//btAlignedFree(pdbvt.m_free);
//pdbvt.m_free = node;
node.Reset();
BulletGlobals.DbvtNodePool.Free(node);
}
//
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();
}
}
public DbvtBroadphase(IOverlappingPairCache paircache)
{
m_sets[0] = new Dbvt();
m_sets[1] = new Dbvt();
m_deferedcollide = false;
m_needcleanup = true;
m_releasepaircache = (paircache != null) ? false : true;
m_prediction = 0;
m_stageCurrent = 0;
m_fixedleft = 0;
m_fupdates = 1;
m_dupdates = 0;
m_cupdates = 10;
m_newpairs = 1;
m_updates_call = 0;
m_updates_done = 0;
m_updates_ratio = 0;
m_paircache = paircache != null ? paircache : new HashedOverlappingPairCache();
m_gid = 0;
m_pid = 0;
m_cid = 0;
#if DBVT_BP_PROFILE
m_profiling = new ProfileBlock();
m_profiling.clear();
#endif
}
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);
}
//
public static DbvtNode CreateNode(Dbvt pdbvt,
DbvtNode parent,
ref DbvtAabbMm volume,
Object data)
{
DbvtNode node = CreateNode(pdbvt, parent, data);
node.volume = volume;
return(node);
}
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;
}
}
public static DbvtNode CreateNode(Dbvt pdbvt, DbvtNode parent, int data)
{
DbvtNode node = BulletGlobals.DbvtNodePool.Get();
node.parent = parent;
node.data = null;
node.dataAsInt = data;
node._children[0] = null;
node._children[1] = null;
return(node);
}
//
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);
}
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;
}
}
}
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, ObjectArray <DbvtNode> leafs, int depth)
{
if (root.IsInternal() && depth != 0)
{
FetchLeafs(pdbvt, root._children[0], leafs, depth - 1);
FetchLeafs(pdbvt, root._children[1], leafs, depth - 1);
DeleteNode(pdbvt, root);
}
else
{
leafs.Add(root);
}
}
public static void RecurseDeleteNode(Dbvt pdbvt, DbvtNode node)
{
if (!node.IsLeaf())
{
RecurseDeleteNode(pdbvt, node._children[0]);
RecurseDeleteNode(pdbvt, node._children[1]);
}
if (node == pdbvt.m_root)
{
pdbvt.m_root = null;
}
DeleteNode(pdbvt, node);
}
public CompoundShape(bool enableDynamicAabbTree)
{
m_children = new ObjectArray <CompoundShapeChild>();
m_localAabbMax = new IndexedVector3(float.MinValue);
m_localAabbMin = new IndexedVector3(float.MaxValue);
m_collisionMargin = 0f;
m_localScaling = new IndexedVector3(1f);
m_dynamicAabbTree = null;
m_updateRevision = 1;
m_shapeType = BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE;
if (enableDynamicAabbTree)
{
m_dynamicAabbTree = new Dbvt();
}
}
public static DbvtNode CreateNode(Dbvt pdbvt, DbvtNode parent, Object data)
{
DbvtNode node = BulletGlobals.DbvtNodePool.Get();
node.parent = parent;
node.data = data;
if (node.data is int)
{
//Debug.Assert(false);
node.dataAsInt = (int)node.data;
}
node._children[0] = null;
node._children[1] = null;
return(node);
}
public CompoundShape(bool enableDynamicAabbTree)
{
m_children = new ObjectArray<CompoundShapeChild>();
m_localAabbMax = new IndexedVector3(float.MinValue);
m_localAabbMin = new IndexedVector3(float.MaxValue);
m_collisionMargin = 0f;
m_localScaling = new IndexedVector3(1f);
m_dynamicAabbTree = null;
m_updateRevision = 1;
m_shapeType = BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE;
if (enableDynamicAabbTree)
{
m_dynamicAabbTree = new Dbvt();
}
}
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);
}
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);
}
}
}
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);
}
}
}
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 static DbvtNode CreateNode(Dbvt pdbvt, DbvtNode parent, Object data)
{
DbvtNode node = BulletGlobals.DbvtNodePool.Get();
node.parent = parent;
node.data = data;
if (node.data is int)
{
//Debug.Assert(false);
node.dataAsInt = (int)node.data;
}
node._children[0] = null;
node._children[1] = null;
return (node);
}
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]);
}
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;
}
}
}
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);
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = null;
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if (compoundShape.GetUpdateRevision() != m_compoundShapeRevision)
{
///clear and update all
RemoveChildAlgorithms();
PreallocateChildAlgorithms(body0, body1);
}
Dbvt tree = compoundShape.GetDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
using (CompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get())
{
callback.Initialize(colObj, otherObj, m_dispatcher, dispatchInfo, resultOut, this, m_childCollisionAlgorithms, m_sharedManifold);
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
m_manifoldArray.Clear();
for (int i = 0; i < m_childCollisionAlgorithms.Count; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
m_childCollisionAlgorithms[i].GetAllContactManifolds(m_manifoldArray);
for (int m = 0; m < m_manifoldArray.Count; m++)
{
if (m_manifoldArray[m].GetNumContacts() > 0)
{
resultOut.SetPersistentManifold(m_manifoldArray[m]);
resultOut.RefreshContactPoints();
resultOut.SetPersistentManifold(null);//??necessary?
}
}
m_manifoldArray.Clear();
}
}
}
if (tree != null)
{
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
IndexedMatrix otherInCompoundSpace;
//otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform());
otherInCompoundSpace = colObj.GetWorldTransform().Inverse() * otherObj.GetWorldTransform();
otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//process all children, that overlap with the given AABB bounds
Dbvt.CollideTV(tree.m_root, ref bounds, callback, tree.CollideTVStack, ref tree.CollideTVCount);
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
for (int i = 0; i < numChildren; i++)
{
callback.ProcessChildShape(compoundShape.GetChildShape(i), i);
}
}
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
m_manifoldArray.Clear();
CollisionShape childShape = null;
IndexedMatrix orgTrans;
IndexedMatrix orgInterpolationTrans;
IndexedMatrix newChildWorldTrans;
for (int i = 0; i < numChildren; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
childShape = compoundShape.GetChildShape(i);
//if not longer overlapping, remove the algorithm
orgTrans = colObj.GetWorldTransform();
orgInterpolationTrans = colObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
}
//
// depth is defaulted to -1
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, ObjectArray<DbvtNode> leafs)
{
FetchLeafs(pdbvt, root, leafs, -1);
}
//
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();
}
}
public void Visualise()
{
DbvtDraw dd = new DbvtDraw();
Dbvt.EnumNodes(m_sets[0].m_root, dd);
}
public static void DeleteNode(Dbvt pdbvt, DbvtNode node)
{
//btAlignedFree(pdbvt.m_free);
//pdbvt.m_free = node;
node.Reset();
BulletGlobals.DbvtNodePool.Free(node);
}
public static void RecurseDeleteNode(Dbvt pdbvt, DbvtNode node)
{
if (!node.IsLeaf())
{
RecurseDeleteNode(pdbvt, node._children[0]);
RecurseDeleteNode(pdbvt, node._children[1]);
}
if (node == pdbvt.m_root)
{
pdbvt.m_root = null;
}
DeleteNode(pdbvt, node);
}
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();
}
public static DbvtNode CreateNode(Dbvt pdbvt, DbvtNode parent, int data)
{
DbvtNode node = BulletGlobals.DbvtNodePool.Get();
node.parent = parent;
node.data = null;
node.dataAsInt = data;
node._children[0] = null;
node._children[1] = null;
return (node);
}
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]);
}
public DbvtBroadphase(IOverlappingPairCache paircache)
{
m_sets[0] = new Dbvt();
m_sets[1] = new Dbvt();
m_deferedcollide = false;
m_needcleanup = true;
m_releasepaircache = (paircache != null) ? false : true;
m_prediction = 0;
m_stageCurrent = 0;
m_fixedleft = 0;
m_fupdates = 1;
m_dupdates = 0;
m_cupdates = 10;
m_newpairs = 1;
m_updates_call = 0;
m_updates_done = 0;
m_updates_ratio = 0;
m_paircache = paircache != null ? paircache : new HashedOverlappingPairCache();
m_gid = 0;
m_pid = 0;
m_cid = 0;
#if DBVT_BP_PROFILE
m_profiling = new ProfileBlock();
m_profiling.clear();
#endif
}
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, ObjectArray<DbvtNode> leafs, int depth)
{
if (root.IsInternal() && depth != 0)
{
FetchLeafs(pdbvt, root._children[0], leafs, depth - 1);
FetchLeafs(pdbvt, root._children[1], leafs, depth - 1);
DeleteNode(pdbvt, root);
}
else
{
leafs.Add(root);
}
}
//
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);
}
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;
}
}
//
// depth is defaulted to -1
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, ObjectArray <DbvtNode> leafs)
{
FetchLeafs(pdbvt, root, leafs, -1);
}
//
public static DbvtNode CreateNode(Dbvt pdbvt,
DbvtNode parent,
ref DbvtAabbMm volume,
Object data)
{
DbvtNode node = CreateNode(pdbvt, parent, data);
node.volume = volume;
return (node);
}
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;
}
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 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);
}
}
}
