public DbvtNode(Dbvt tree, DbvtNode aparent, ref DbvtAabbMm avolume, Object adata)
{
volume = avolume;
parent = aparent;
data = adata;
if (data is int)
{
dataAsInt = (int)data;
}
}
public static void CollideTV(DbvtNode root, ref DbvtAabbMm volume, Collide collideable)
{
if (root != null)
{
Stack<DbvtNode> stack = new Stack<DbvtNode>(SIMPLE_STACKSIZE);
stack.Push(root);
do
{
DbvtNode n = stack.Pop();
if (DbvtAabbMm.Intersect(ref n.volume, ref volume))
{
if (n.IsInternal())
{
stack.Push(n._children[0]);
stack.Push(n._children[1]);
}
else
{
collideable.Process(n);
}
}
} while (stack.Count > 0);
}
}
public void OptimizeTopDown(int bu_threshold)
{
// threshhold defaults to 128
if (Root != null)
{
List<DbvtNode> leafs = new List<DbvtNode>(m_leaves);
FetchLeafs(this, Root, leafs);
Root = TopDown(this, leafs, bu_threshold);
}
}
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
{
if (DbvtAabbMm.Proximity(ref root._children[0].volume, ref leaf.volume) <
DbvtAabbMm.Proximity(ref root._children[1].volume, ref leaf.volume))
{
root = root._children[0];
}
else
{
root = root._children[1];
}
} while (!root.IsLeaf());
}
DbvtNode prev = root.parent;
DbvtAabbMm mergeResults = DbvtAabbMm.Merge(ref leaf.volume, ref root.volume);
DbvtNode node = new DbvtNode(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 DeleteNode(Dbvt dbvt, ref DbvtNode node)
{
node = null;
}
public static void CollideOCL(DbvtNode root, Vector3[] normals, float[] offsets, ref Vector3 sortaxis, int count, Collide collideable)
{
if (root != null)
{
uint srtsgns = (uint)((sortaxis.X >= 0 ? 1 : 0) + (sortaxis.Y >= 0 ? 2 : 0) + (sortaxis.Z >= 0 ? 4 : 0));
int inside = (1 << count) - 1;
//Stack<sStkNPS> stack = new Stack<sStkNPS>(SIMPLE_STACKSIZE);
List<sStkNPS> stack = new List<sStkNPS>(SIMPLE_STACKSIZE);
int[] signs = new int[count];
for (int i = 0; i < count; ++i)
{
signs[i] = ((normals[i].X >= 0) ? 1 : 0) +
((normals[i].Y >= 0) ? 2 : 0) +
((normals[i].Z >= 0) ? 4 : 0);
}
stack.Insert(0, new sStkNPS(root, 0, root.volume.ProjectMinimum(ref sortaxis, srtsgns)));
do
{
sStkNPS se = stack[0];
stack.RemoveAt(0);
if (se.mask != inside)
{
bool outp = false;
for (int i = 0, j = 1; (!outp) && (i < count); ++i, j <<= 1)
{
if (0 == (se.mask & j))
{
int side = se.node.volume.Classify(ref normals[i], offsets[i], signs[i]);
switch (side)
{
case -1: outp = true; break;
case +1: se.mask |= (uint)j; break;
}
}
}
if (outp)
{
continue;
}
}
if (collideable.Descent(se.node))
{
if (se.node.IsInternal())
{
for (int i = 0; i < 2; ++i)
{
DbvtNode n = se.node._children[i];
int j = stack.Count;
sStkNPS ne = new sStkNPS(n, se.mask, n.volume.ProjectMinimum(ref sortaxis, srtsgns));
stack.Insert(0, ne);
while ((j > 0) && (ne.value > stack[j - 1].value))
{
sStkNPS left = stack[j];
sStkNPS right = stack[j - 1];
stack[j] = right;
stack[j - 1] = left;
--j;
//btSwap(stack[j],stack[j-1]);--j;
}
}
}
else
{
collideable.Process(se.node);
}
}
} while (stack.Count > 0);
}
}
//
public static void BottomUp(Dbvt pdbvt, List<DbvtNode> leafs)
{
while (leafs.Count > 1)
{
float minsize = float.MaxValue;
int[] minidx = { -1, -1 };
for (int i = 0; i < leafs.Count; ++i)
{
for (int j = i + 1; j < leafs.Count; ++j)
{
DbvtAabbMm mergeResults = DbvtAabbMm.Merge(ref leafs[i].volume, ref leafs[j].volume);
float sz = Size(ref mergeResults);
if (sz < minsize)
{
minsize = sz;
minidx[0] = i;
minidx[1] = j;
}
}
}
DbvtNode[] n = { leafs[minidx[0]], leafs[minidx[1]] };
DbvtAabbMm mergeResults2 = DbvtAabbMm.Merge(ref n[0].volume, ref n[1].volume);
DbvtNode p = new DbvtNode(pdbvt, null, ref mergeResults2, null);
p._children[0] = n[0];
p._children[1] = n[1];
n[0].parent = p;
n[1].parent = p;
leafs[minidx[0]] = p;
DbvtNode left = leafs[minidx[1]];
DbvtNode right = leafs[leafs.Count - 1];
leafs[minidx[1]] = right;
leafs[leafs.Count - 1] = left;
leafs.RemoveAt(leafs.Count - 1);
}
}
public bool Update(DbvtNode leaf, ref DbvtAabbMm volume, float margin)
{
if (leaf.volume.Contain(ref volume))
{
return (false);
}
volume.Expand(new Vector3(margin, margin, margin));
Update(leaf, ref volume);
return (true);
}
public void Remove(DbvtNode leaf)
{
RemoveLeaf(this, leaf);
DeleteNode(this, ref leaf);
--m_leaves;
}
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 bool Update(DbvtNode leaf, ref DbvtAabbMm volume, ref Vector3 velocity)
{
if (leaf.volume.Contain(ref volume))
{
return (false);
}
volume.SignedExpand(velocity);
Update(leaf, ref volume);
return (true);
}
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)
{
Update(leaf, -1);
}
public DbvtNode Insert(ref DbvtAabbMm box, Object data)
{
DbvtNode leaf = new DbvtNode(this, null, ref box, data);
InsertLeaf(this, Root, leaf);
++m_leaves;
return leaf;
}
public static void CollideRAY(DbvtNode root, ref Vector3 origin, ref Vector3 direction, Collide collideable)
{
if (root != null)
{
Vector3 normal = direction;
normal.Normalize();
Vector3 invdir = new Vector3(1 / normal.X, 1 / normal.Y, 1 / normal.Z);
int[] signs = new int[] { direction.X < 0f ? 1 : 0, direction.Y < 0f ? 1 : 0, direction.Z < 0f ? 1 : 0 };
Stack<DbvtNode> stack = new Stack<DbvtNode>(SIMPLE_STACKSIZE);
stack.Push(root);
do
{
DbvtNode node = stack.Pop();
if (Intersect(ref node.volume, ref origin, ref invdir, signs))
{
if (node.IsInternal())
{
stack.Push(node._children[0]);
stack.Push(node._children[1]);
}
else
{
collideable.Process(node);
}
}
} while (stack.Count > 0);
}
}
//
// depth is defaulted to -1
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, List<DbvtNode> leafs)
{
FetchLeafs(pdbvt, root, leafs, -1);
}
public static void CollideKDOP(DbvtNode root, Vector3[] normals, float[] offsets, int count, Collide collideable)
{
if (root != null)
{
int inside = (1 << count) - 1;
Stack<sStkNP> stack = new Stack<sStkNP>(SIMPLE_STACKSIZE);
int[] signs = new int[count];
for (int i = 0; i < count; ++i)
{
signs[i] = ((normals[i].X >= 0) ? 1 : 0) +
((normals[i].Y >= 0) ? 2 : 0) +
((normals[i].Z >= 0) ? 4 : 0);
}
stack.Push(new sStkNP(root, 0));
do
{
sStkNP se = stack.Pop();
bool outp = false;
for (int i = 0, j = 1; (!outp) && (i < count); ++i, j <<= 1)
{
if (0 == (se.mask & j))
{
int side = se.node.volume.Classify(ref normals[i], offsets[i], signs[i]);
switch (side)
{
case -1: outp = true; break;
case +1: se.mask |= (uint)j; break;
}
}
}
if (!outp)
{
if ((se.mask != inside) && (se.node.IsInternal()))
{
stack.Push(new sStkNP(se.node._children[0], se.mask));
stack.Push(new sStkNP(se.node._children[1], se.mask));
}
else
{
if (collideable.AllLeaves(se.node))
{
EnumLeafs(se.node, collideable);
}
}
}
} while (stack.Count > 0);
}
}
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, List<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, ref root);
}
else
{
leafs.Add(root);
}
}
public static void CollideTU(DbvtNode root, Collide collideable)
{
if (root != null)
{
Stack<DbvtNode> stack = new Stack<DbvtNode>(SIMPLE_STACKSIZE);
stack.Push(root);
do
{
DbvtNode n = stack.Pop();
if (collideable.Descent(n))
{
if (n.IsInternal())
{
stack.Push(n._children[0]);
stack.Push(n._children[1]);
}
else
{
collideable.Process(n);
}
}
} while (stack.Count > 0);
}
}
public static void EnumLeafs(DbvtNode root, Collide collideable)
{
if (root.IsInternal())
{
EnumLeafs(root._children[0], collideable);
EnumLeafs(root._children[1], collideable);
}
else
{
collideable.Process(root);
}
}
public static DbvtNode TopDown(Dbvt pdbvt, List<DbvtNode> leafs, int bu_treshold)
{
if (leafs.Count > 1)
{
if (leafs.Count > bu_treshold)
{
DbvtAabbMm volume = Bounds(leafs);
Vector3 org = volume.Center();
List<DbvtNode>[] sets = { new List<DbvtNode>(), new List<DbvtNode>() };
int bestaxis = -1;
int bestmidp = leafs.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 };
for (int i = 0; i < leafs.Count; ++i)
{
Vector3 x = leafs[i].volume.Center() - org;
for (int j = 0; j < 3; ++j)
{
++splitcount[j][Vector3.Dot(x, axis[j]) > 0 ? 1 : 0];
}
}
for (int i = 0; i < 3; ++i)
{
if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
{
int midp = (int)System.Math.Abs((splitcount[i][0] - splitcount[i][1]));
if (midp < bestmidp)
{
bestaxis = i;
bestmidp = midp;
}
}
}
if (bestaxis >= 0)
{
sets[0].Capacity = (splitcount[bestaxis][0]);
sets[1].Capacity = (splitcount[bestaxis][1]);
Split(leafs, sets[0], sets[1], ref org, ref axis[bestaxis]);
}
else
{
sets[0].Capacity = (leafs.Count / 2 + 1);
sets[1].Capacity = (leafs.Count / 2);
for (int i = 0, ni = leafs.Count; i < ni; ++i)
{
sets[i & 1].Add(leafs[i]);
}
}
DbvtNode node = new DbvtNode(pdbvt, null, ref volume, 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, leafs);
return (leafs[0]);
}
}
return (leafs[0]);
}
public static void CollideTTpersistentStack(DbvtNode m_root0, DbvtNode m_root1, Collide collider)
{
CollideTT(m_root0, m_root1, 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, ref 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, ref parent);
return (pdbvt.Root);
}
}
}
public static void CollideTT(DbvtNode root0, DbvtNode root1, Collide collideable)
{
if (root0 != null && root1 != null)
{
Stack<sStkNN> stack = new Stack<sStkNN>(DOUBLE_STACKSIZE);
stack.Push(new sStkNN(root0, root1));
do
{
sStkNN p = stack.Pop();
if (p.a == p.b)
{
if (p.a.IsInternal())
{
stack.Push(new sStkNN(p.a._children[0], p.a._children[0]));
stack.Push(new sStkNN(p.a._children[1], p.a._children[1]));
stack.Push(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())
{
stack.Push(new sStkNN(p.a._children[0], p.b._children[0]));
stack.Push(new sStkNN(p.a._children[1], p.b._children[0]));
stack.Push(new sStkNN(p.a._children[0], p.b._children[1]));
stack.Push(new sStkNN(p.a._children[1], p.b._children[1]));
}
else
{
stack.Push(new sStkNN(p.a._children[0], p.b));
stack.Push(new sStkNN(p.a._children[1], p.b));
}
}
else
{
if (p.b.IsInternal())
{
stack.Push(new sStkNN(p.a, p.b._children[0]));
stack.Push(new sStkNN(p.a, p.b._children[1]));
}
else
{
collideable.Process(p.a, p.b);
}
}
}
} while (stack.Count > 0);
}
}
public static int IndexOf(DbvtNode node)
{
return (node.parent._children[1] == node) ? 1 : 0;
}
public void RayTestInternal(DbvtNode root,
ref Vector3 rayFrom,
ref Vector3 rayTo,
ref Vector3 rayDirectionInverse,
bool[] signs,
float lambda_max,
ref Vector3 aabbMin,
ref Vector3 aabbMax,
Collide policy)
{
// (void) rayTo;
//DBVT_CHECKTYPE
if(root != null)
{
Vector3 resultNormal = Vector3.Up;
int depth=1;
int treshold=DOUBLE_STACKSIZE-2;
ObjectArray<DbvtNode> stack = new ObjectArray<DbvtNode>(DOUBLE_STACKSIZE);
stack[0]=root;
Vector3[] bounds = new Vector3[2];
do
{
DbvtNode node=stack[--depth];
bounds[0] = node.volume.Mins()-aabbMax;
bounds[1] = node.volume.Maxs()-aabbMin;
float tmin=1.0f,lambda_min=0.0f;
bool result1 = AabbUtil2.RayAabb2(ref rayFrom,ref rayDirectionInverse,signs,bounds,ref tmin,lambda_min,lambda_max);
if(result1)
{
if(node.IsInternal())
{
//if(depth>treshold)
//{
// stack.resize(stack.size()*2);
// treshold=stack.size()-2;
//}
stack[depth++]=node._children[0];
stack[depth++]=node._children[1];
}
else
{
policy.Process(node);
}
}
} while(depth != 0);
}
}
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))
{
Vector3 worldAabbMin = Vector3.Zero;
Vector3 worldAabbMax = Vector3.Zero;
Matrix orgTrans = m_compoundColObj.GetWorldTransform();
MathUtil.TransformAabb(leaf.volume.Mins(),leaf.volume.Maxs(),0f,orgTrans,ref worldAabbMin,ref worldAabbMax);
m_dispatchInfo.getDebugDraw().DrawAabb(worldAabbMin, worldAabbMax, new Vector3(1, 0, 0));
}
ProcessChildShape(childShape,index);
}
public void OptimizeBottomUp()
{
if (Root != null)
{
List<DbvtNode> leafs = new List<DbvtNode>(m_leaves);
FetchLeafs(this, Root, leafs);
BottomUp(this, leafs);
Root = leafs[0];
}
}
