public void OptimizeIncremental(int passes)
{
if (passes < 0)
{
passes = m_leaves;
}
if (Root != null && (passes > 0))
{
int sizeOfUnsigned = 4;
int computedValue = (sizeOfUnsigned * 8 - 1);
do
{
DbvtNode node = Root;
int bit = 0;
while (node.IsInternal())
{
node = Sort(node, m_root)._children[(m_opath >> bit) & 1];
bit = (bit + 1) & (sizeof(UInt32) * 8 - 1);
}
Update(node);
++m_opath;
} while (--passes > 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--;
}
public virtual void Process(DbvtNode n, float f)
{
IndexedMatrix im = IndexedMatrix.Identity;
IndexedVector3 color = new IndexedVector3(1, 1, 1);
BulletGlobals.gDebugDraw.DrawBox(ref n.volume._min, ref n.volume._max, ref im, ref color);
}
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 DbvtNode Insert(ref DbvtAabbMm box, Object data)
{
DbvtNode leaf = CreateNode(this, null, ref box, data);
InsertLeaf(this, Root, leaf);
++m_leaves;
return(leaf);
}
public static void EnumNodes(DbvtNode root, ICollide collideable)
{
collideable.Process(root);
if (root.IsInternal())
{
EnumNodes(root._children[0], collideable);
EnumNodes(root._children[1], collideable);
}
}
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);
}
public void OptimizeTopDown(int bu_threshold)
{
// threshhold defaults to 128
if (Root != null)
{
ObjectArray <DbvtNode> leafs = new ObjectArray <DbvtNode>(m_leaves);
FetchLeafs(this, Root, leafs);
Root = TopDown(this, leafs, bu_threshold);
}
}
//
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 void OptimizeBottomUp()
{
if (Root != null)
{
ObjectArray <DbvtNode> leafs = new ObjectArray <DbvtNode>(m_leaves);
FetchLeafs(this, Root, leafs);
BottomUp(this, leafs);
Root = leafs[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);
}
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 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 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 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 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 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 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 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 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 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 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 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 void Update(DbvtNode leaf, int lookahead)
{
DbvtNode root = RemoveLeaf(this, leaf);
if (root != null)
{
if (lookahead >= 0)
{
for (int i = 0; (i < lookahead) && (root.parent != null); ++i)
{
root = root.parent;
}
}
else
{
root = Root;
}
}
InsertLeaf(this, root, leaf);
}
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);
}
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 void Process(DbvtNode n)
{
Process(n, proxy.leaf);
}
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 bool Descent(DbvtNode n)
{
return true;
}
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 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 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 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 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);
}
public void Process(DbvtNode n, DbvtNode n2)
{ }
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 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 DbvtNode CreateNode(Dbvt pdbvt,
DbvtNode parent,
ref DbvtAabbMm volume,
Object data)
{
DbvtNode node = CreateNode(pdbvt, parent, data);
node.volume = 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 void Process(DbvtNode leaf)
{
DbvtProxy proxy = leaf.data as DbvtProxy;
m_aabbCallback.Process(proxy);
}
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);
}
}
//
// depth is defaulted to -1
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, ObjectArray<DbvtNode> leafs)
{
FetchLeafs(pdbvt, root, leafs, -1);
}
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 void Remove(DbvtNode leaf)
{
RemoveLeaf(this, leaf);
DeleteNode(this, leaf);
--m_leaves;
}
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 void Process(DbvtNode n, float f)
{
Process(n);
}
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 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 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, 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 bool AllLeaves(DbvtNode n)
{
return true;
}
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 override void Process(DbvtNode leaf)
{
Process(leaf.dataAsInt);
}
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;
}
}
