void GatherLargeTriangles(ref TreeNode tn, CollisionContent cc, int[] tris, ref int lo, int hi, float r, BoundingSphere[] tbs)
{
for (int i = lo; i < hi; ++i)
{
if (tbs[tris[i]].Radius >= r)
{
int j = tris[i];
tris[i] = tris[lo];
tris[lo] = j;
++lo;
}
}
}
void BuildNodes(CollisionContent cc, int[] tris, int lo, int hi, List<TreeNode> nodes, AABB bounds,
BoundingSphere[] tbs)
{
TreeNode tn = new TreeNode();
tn.TriStart = lo;
tn.Expansion = (bounds.Hi - bounds.Lo).Length() * ExpansionFactor;
GatherLargeTriangles(ref tn, cc, tris, ref lo, hi, tn.Expansion, tbs);
tn.TriEnd = lo;
int ix = nodes.Count;
nodes.Add(tn);
Vector3 lb = bounds.Lo;
Vector3 ub = bounds.Hi;
Vector3 cb = lb + (ub - lb) * 0.5f;
if (hi > lo + 4)
{
int midX = lo;
PartitionSmallerTriangles(cc, tris, lo, ref midX, hi, tbs, Vector3.Right, cb.X);
int midXlo = lo;
PartitionSmallerTriangles(cc, tris, lo, ref midXlo, midX, tbs, Vector3.Up, cb.Y);
int midXhi = midX;
PartitionSmallerTriangles(cc, tris, midX, ref midXhi, hi, tbs, Vector3.Up, cb.Y);
int midXloYlo = lo;
PartitionSmallerTriangles(cc, tris, lo, ref midXloYlo, midXlo, tbs, Vector3.Backward, cb.Z);
int midXloYhi = midXlo;
PartitionSmallerTriangles(cc, tris, midXlo, ref midXloYhi, midX, tbs, Vector3.Backward, cb.Z);
int midXhiYlo = midX;
PartitionSmallerTriangles(cc, tris, midX, ref midXhiYlo, midXhi, tbs, Vector3.Backward, cb.Z);
int midXhiYhi = midXhi;
PartitionSmallerTriangles(cc, tris, midXhi, ref midXhiYhi, hi, tbs, Vector3.Backward, cb.Z);
if (lo < midXloYlo)
{
tn.Child000 = nodes.Count;
BuildNodes(cc, tris, lo, midXloYlo, nodes, new AABB(lb, cb),
tbs);
}
if (midXloYlo < midXlo)
{
tn.Child001 = nodes.Count;
BuildNodes(cc, tris, midXloYlo, midXlo, nodes,
new AABB(new Vector3(lb.X, lb.Y, cb.Z), new Vector3(cb.X, cb.Y, ub.Z)),
tbs);
}
if (midXlo < midXloYhi)
{
tn.Child010 = nodes.Count;
BuildNodes(cc, tris, midXlo, midXloYhi, nodes,
new AABB(new Vector3(lb.X, cb.Y, lb.Z), new Vector3(cb.X, ub.Y, cb.Z)),
tbs);
}
if (midXloYhi < midX)
{
tn.Child011 = nodes.Count;
BuildNodes(cc, tris, midXloYhi, midX, nodes,
new AABB(new Vector3(lb.X, cb.Y, cb.Z), new Vector3(cb.X, ub.Y, ub.Z)),
tbs);
}
if (midX < midXhiYlo)
{
tn.Child100 = nodes.Count;
BuildNodes(cc, tris, midX, midXhiYlo, nodes,
new AABB(new Vector3(cb.X, lb.Y, lb.Z), new Vector3(ub.X, cb.Y, cb.Z)),
tbs);
}
if (midXhiYlo < midXhi)
{
tn.Child101 = nodes.Count;
BuildNodes(cc, tris, midXhiYlo, midXhi, nodes,
new AABB(new Vector3(cb.X, lb.Y, cb.Z), new Vector3(ub.X, cb.Y, ub.Z)),
tbs);
}
if (midXhi < midXhiYhi)
{
tn.Child110 = nodes.Count;
BuildNodes(cc, tris, midXhi, midXhiYhi, nodes,
new AABB(new Vector3(cb.X, cb.Y, lb.Z), new Vector3(ub.X, ub.Y, cb.Z)),
tbs);
}
if (midXhiYhi < hi)
{
tn.Child111 = nodes.Count;
BuildNodes(cc, tris, midXhiYhi, hi, nodes,
new AABB(new Vector3(cb.X, cb.Y, cb.Z), new Vector3(ub.X, ub.Y, ub.Z)),
tbs);
}
}
else
{
// include them all!
tn.TriEnd = hi;
}
nodes[ix] = tn;
}
