public void Remove(DbvtNode leaf)
{
RemoveLeaf(this, leaf);
DeleteNode(this, leaf);
--m_leaves;
}
public bool Descent(DbvtNode n)
{
return true;
}
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 void Process(DbvtNode n)
{
Process(n, proxy.leaf);
}
public void Process(DbvtNode n, DbvtNode n2)
{ }
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 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 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 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 GetMaxDepth(DbvtNode node, int depth, ref int maxDepth)
{
if (node.IsInternal())
{
GetMaxDepth(node._children[0], depth + 1, ref maxDepth);
GetMaxDepth(node._children[1], depth + 1, ref maxDepth);
}
else
{
maxDepth = Math.Max(depth, maxDepth);
}
}
public static void EnumLeaves(DbvtNode root, ICollide collideable)
{
if (root.IsInternal())
{
EnumLeaves(root._children[0], collideable);
EnumLeaves(root._children[1], collideable);
}
else
{
collideable.Process(root);
}
}
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);
}
}
//
// depth is defaulted to -1
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, ObjectArray<DbvtNode> leafs)
{
FetchLeafs(pdbvt, root, leafs, -1);
}
public static DbvtNode Sort(DbvtNode n, DbvtNode r)
{
DbvtNode p = n.parent;
Debug.Assert(n.IsInternal());
if (p != null && (p.id > n.id))
{
int i = IndexOf(n);
int j = 1 - i;
DbvtNode s = p._children[j];
DbvtNode q = p.parent;
Debug.Assert(n == p._children[i]);
if (q != null)
{
q._children[IndexOf(p)] = n;
}
else
{
r = n;
}
s.parent = n;
p.parent = n;
n.parent = q;
p._children[0] = n._children[0];
p._children[1] = n._children[1];
n._children[0].parent = p;
n._children[1].parent = p;
n._children[i] = p;
n._children[j] = s;
// swap id's? as well - probably not.
Swap(ref p.volume, ref n.volume);
return (p);
}
return (n);
}
//
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 RayTest(DbvtNode root,
ref IndexedVector3 rayFrom,
ref IndexedVector3 rayTo,
ICollide policy)
{
using (DbvtStackDataBlock stackDataBlock = BulletGlobals.DbvtStackDataBlockPool.Get())
{
if (root != null)
{
IndexedVector3 rayDir = (rayTo - rayFrom);
rayDir.Normalize();
///what about division by zero? -. just set rayDirection[i] to INF/BT_LARGE_FLOAT
IndexedVector3 rayDirectionInverse = new IndexedVector3(
rayDir.X == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.X,
rayDir.Y == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.Y,
rayDir.Z == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.Z);
stackDataBlock.signs[0] = rayDirectionInverse.X < 0.0f;
stackDataBlock.signs[1] = rayDirectionInverse.Y < 0.0f;
stackDataBlock.signs[2] = rayDirectionInverse.Z < 0.0f;
float lambda_max = IndexedVector3.Dot(rayDir, (rayTo - rayFrom));
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 2;
stackDataBlock.stack.Resize(DOUBLE_STACKSIZE);
stackDataBlock.stack[0] = root;
do
{
DbvtNode node = stackDataBlock.stack[--depth];
stackDataBlock.bounds[0] = node.volume.Mins();
stackDataBlock.bounds[1] = node.volume.Maxs();
float tmin = 1.0f, lambda_min = 0.0f;
bool result1 = AabbUtil2.RayAabb2(ref rayFrom, ref rayDirectionInverse, stackDataBlock.signs, stackDataBlock.bounds, out tmin, lambda_min, lambda_max);
#if COMPARE_BTRAY_AABB2
float param=1.0f;
bool result2 = AabbUtil.RayAabb(ref rayFrom,ref rayTo,node.volume.Mins(),node.volume.Maxs(),param,resultNormal);
Debug.Assert(result1 == result2);
#endif //TEST_BTRAY_AABB2
if (result1)
{
if (node.IsInternal())
{
if (depth > treshold)
{
stackDataBlock.stack.Resize(stackDataBlock.stack.Count * 2);
treshold = stackDataBlock.stack.Count - 2;
}
stackDataBlock.stack[depth++] = node._children[0];
stackDataBlock.stack[depth++] = node._children[1];
}
else
{
policy.Process(node);
}
}
} while (depth != 0);
}
}
}
public static void DeleteNode(Dbvt pdbvt, DbvtNode node)
{
//btAlignedFree(pdbvt.m_free);
//pdbvt.m_free = node;
node.Reset();
BulletGlobals.DbvtNodePool.Free(node);
}
public void RayTestInternal(DbvtNode root,
ref IndexedVector3 rayFrom,
ref IndexedVector3 rayTo,
ref IndexedVector3 rayDirectionInverse,
bool[] signs,
float lambda_max,
ref IndexedVector3 aabbMin,
ref IndexedVector3 aabbMax,
ICollide policy)
{
using (DbvtStackDataBlock stackDataBlock = BulletGlobals.DbvtStackDataBlockPool.Get())
{
// (void) rayTo;
//DBVT_CHECKTYPE
if (root != null)
{
IndexedVector3 resultNormal = new IndexedVector3(0, 1, 0);
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 2;
stackDataBlock.stack[0] = root;
do
{
DbvtNode node = stackDataBlock.stack[--depth];
stackDataBlock.bounds[0] = node.volume.Mins() - aabbMax;
stackDataBlock.bounds[1] = node.volume.Maxs() - aabbMin;
float tmin = 1.0f, lambda_min = 0.0f;
bool result1 = AabbUtil2.RayAabb2(ref rayFrom, ref rayDirectionInverse, signs, stackDataBlock.bounds, out tmin, lambda_min, lambda_max);
if (result1)
{
if (node.IsInternal())
{
if (depth > treshold)
{
stackDataBlock.stack.Resize(stackDataBlock.stack.Count * 2);
treshold = stackDataBlock.stack.Count - 2;
}
stackDataBlock.stack[depth++] = node._children[0];
stackDataBlock.stack[depth++] = node._children[1];
}
else
{
policy.Process(node);
}
}
} while (depth != 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 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 override void Process(DbvtNode leaf)
{
int index = leaf.dataAsInt;
CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());
CollisionShape childShape = compoundShape.GetChildShape(index);
if (m_dispatchInfo.getDebugDraw() != null && (((m_dispatchInfo.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawAabb)) != 0))
{
IndexedVector3 worldAabbMin;
IndexedVector3 worldAabbMax;
IndexedMatrix orgTrans = m_compoundColObj.GetWorldTransform();
AabbUtil2.TransformAabb(leaf.volume.Mins(), leaf.volume.Maxs(), 0f, ref orgTrans, out worldAabbMin, out worldAabbMax);
m_dispatchInfo.getDebugDraw().DrawAabb(worldAabbMin, worldAabbMax, new IndexedVector3(1, 0, 0));
}
ProcessChildShape(childShape, index);
}
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 void Process(DbvtNode leaf)
{
DbvtProxy proxy = leaf.data as DbvtProxy;
m_aabbCallback.Process(proxy);
}
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 void Process(DbvtNode n, float f)
{
Process(n);
}
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 bool AllLeaves(DbvtNode n)
{
return true;
}
//
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 override void Process(DbvtNode leaf)
{
Process(leaf.dataAsInt);
}
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);
}
