int split_point_set_sorted(int[] pt_indices, Vector3d[] positions,
int iStart, int iCount, int depth, int minIndexCount,
boxes_set leafs, boxes_set nodes, out AxisAlignedBox3d box)
{
box = AxisAlignedBox3d.Empty;
int iBox = -1;
if (iCount < minIndexCount)
{
// append new points box
iBox = leafs.iBoxCur++;
leafs.box_to_index.insert(leafs.iIndicesCur, iBox);
leafs.index_list.insert(iCount, leafs.iIndicesCur++);
for (int i = 0; i < iCount; ++i)
{
leafs.index_list.insert(pt_indices[iStart + i], leafs.iIndicesCur++);
box.Contain(points.GetVertex(pt_indices[iStart + i]));
}
leafs.box_centers.insert(box.Center, iBox);
leafs.box_extents.insert(box.Extents, iBox);
return(-(iBox + 1));
}
AxisComp c = new AxisComp()
{
Axis = depth % 3
};
Array.Sort(positions, pt_indices, iStart, iCount, c);
int mid = iCount / 2;
int n0 = mid;
int n1 = iCount - mid;
// create child boxes
AxisAlignedBox3d box1;
int child0 = split_point_set_sorted(pt_indices, positions, iStart, n0, depth + 1, minIndexCount, leafs, nodes, out box);
int child1 = split_point_set_sorted(pt_indices, positions, iStart + mid, n1, depth + 1, minIndexCount, leafs, nodes, out box1);
box.Contain(box1);
// append new box
iBox = nodes.iBoxCur++;
nodes.box_to_index.insert(nodes.iIndicesCur, iBox);
nodes.index_list.insert(child0, nodes.iIndicesCur++);
nodes.index_list.insert(child1, nodes.iIndicesCur++);
nodes.box_centers.insert(box.Center, iBox);
nodes.box_extents.insert(box.Extents, iBox);
return(iBox);
}
int split_tri_set_sorted(int[] triangles, Vector3d[] centers, int iStart, int iCount, int depth, int minTriCount,
boxes_set tris, boxes_set nodes, out AxisAlignedBox3f box)
{
box = AxisAlignedBox3f.Empty;
int iBox = -1;
if (iCount < minTriCount)
{
// append new triangles box
iBox = tris.iBoxCur++;
tris.box_to_index.insert(tris.iIndicesCur, iBox);
tris.index_list.insert(iCount, tris.iIndicesCur++);
for (int i = 0; i < iCount; ++i)
{
tris.index_list.insert(triangles[iStart + i], tris.iIndicesCur++);
box.Contain(mesh.GetTriBounds(triangles[iStart + i]));
}
tris.box_centers.insert(box.Center, iBox);
tris.box_extents.insert(box.Extents, iBox);
return(-(iBox + 1));
}
AxisComp c = new AxisComp()
{
Axis = depth % 3
};
Array.Sort(centers, triangles, iStart, iCount, c);
int mid = iCount / 2;
int n0 = mid;
int n1 = iCount - mid;
// create child boxes
AxisAlignedBox3f box1;
int child0 = split_tri_set_sorted(triangles, centers, iStart, n0, depth + 1, minTriCount, tris, nodes, out box);
int child1 = split_tri_set_sorted(triangles, centers, iStart + mid, n1, depth + 1, minTriCount, tris, nodes, out box1);
box.Contain(box1);
// append new box
iBox = nodes.iBoxCur++;
nodes.box_to_index.insert(nodes.iIndicesCur, iBox);
nodes.index_list.insert(child0, nodes.iIndicesCur++);
nodes.index_list.insert(child1, nodes.iIndicesCur++);
nodes.box_centers.insert(box.Center, iBox);
nodes.box_extents.insert(box.Extents, iBox);
return(iBox);
}
void build_top_down(bool bSorted)
{
int i = 0;
int[] triangles = new int[mesh.TriangleCount];
Vector3d[] centers = new Vector3d[mesh.TriangleCount];
foreach (int ti in mesh.TriangleIndices())
{
triangles[i] = ti;
centers[i++] = mesh.GetTriCentroid(ti);
}
boxes_set tris = new boxes_set();
boxes_set nodes = new boxes_set();
AxisAlignedBox3f rootBox;
int rootnode = (bSorted) ?
split_tri_set_sorted(triangles, centers, 0, mesh.TriangleCount, 0, TopDownLeafMaxTriCount, tris, nodes, out rootBox)
: split_tri_set_midpoint(triangles, centers, 0, mesh.TriangleCount, 0, TopDownLeafMaxTriCount, tris, nodes, out rootBox);
box_to_index = tris.box_to_index;
box_centers = tris.box_centers;
box_extents = tris.box_extents;
index_list = tris.index_list;
triangles_end = tris.iIndicesCur;
int iIndexShift = triangles_end;
int iBoxShift = tris.iBoxCur;
// ok now append internal node boxes & index ptrs
for (i = 0; i < nodes.iBoxCur; ++i)
{
box_centers.insert(nodes.box_centers[i], iBoxShift + i);
box_extents.insert(nodes.box_extents[i], iBoxShift + i);
// internal node indices are shifted
box_to_index.insert(iIndexShift + nodes.box_to_index[i], iBoxShift + i);
}
// now append index list
for (i = 0; i < nodes.iIndicesCur; ++i)
{
int child_box = nodes.index_list[i];
if (child_box < 0) // this is a triangles box
{
child_box = (-child_box) - 1;
}
else
{
child_box += iBoxShift;
}
child_box = child_box + 1;
index_list.insert(child_box, iIndexShift + i);
}
root_index = rootnode + iBoxShift;
}
int split_tri_set_midpoint(int[] triangles, Vector3d[] centers, int iStart, int iCount, int depth, int minTriCount,
boxes_set tris, boxes_set nodes, out AxisAlignedBox3f box)
{
box = AxisAlignedBox3f.Empty;
int iBox = -1;
if (iCount < minTriCount)
{
// append new triangles box
iBox = tris.iBoxCur++;
tris.box_to_index.insert(tris.iIndicesCur, iBox);
tris.index_list.insert(iCount, tris.iIndicesCur++);
for (int i = 0; i < iCount; ++i)
{
tris.index_list.insert(triangles[iStart + i], tris.iIndicesCur++);
box.Contain(mesh.GetTriBounds(triangles[iStart + i]));
}
tris.box_centers.insert(box.Center, iBox);
tris.box_extents.insert(box.Extents, iBox);
return(-(iBox + 1));
}
//compute interval along an axis and find midpoint
int axis = depth % 3;
Interval1d interval = Interval1d.Empty;
for (int i = 0; i < iCount; ++i)
{
interval.Contain(centers[iStart + i][axis]);
}
double midpoint = interval.Center;
int n0, n1;
if (Math.Abs(interval.a - interval.b) > MathUtil.ZeroTolerance)
{
// we have to re-sort the centers & triangles lists so that centers < midpoint
// are first, so that we can recurse on the two subsets. We walk in from each side,
// until we find two out-of-order locations, then we swap them.
int l = 0;
int r = iCount - 1;
while (l < r)
{
// [RMS] is <= right here? if v.axis == midpoint, then this loop
// can get stuck unless one of these has an equality test. But
// I did not think enough about if this is the right thing to do...
while (centers[iStart + l][axis] <= midpoint)
{
l++;
}
while (centers[iStart + r][axis] > midpoint)
{
r--;
}
if (l >= r)
{
break; //done!
}
//swap
Vector3d tmpc = centers[iStart + l]; centers[iStart + l] = centers[iStart + r]; centers[iStart + r] = tmpc;
int tmpt = triangles[iStart + l]; triangles[iStart + l] = triangles[iStart + r]; triangles[iStart + r] = tmpt;
}
n0 = l;
n1 = iCount - n0;
Debug.Assert(n0 >= 1 && n1 >= 1);
}
else
{
// interval is near-empty, so no point trying to do sorting, just split half and half
n0 = iCount / 2;
n1 = iCount - n0;
}
// create child boxes
AxisAlignedBox3f box1;
int child0 = split_tri_set_midpoint(triangles, centers, iStart, n0, depth + 1, minTriCount, tris, nodes, out box);
int child1 = split_tri_set_midpoint(triangles, centers, iStart + n0, n1, depth + 1, minTriCount, tris, nodes, out box1);
box.Contain(box1);
// append new box
iBox = nodes.iBoxCur++;
nodes.box_to_index.insert(nodes.iIndicesCur, iBox);
nodes.index_list.insert(child0, nodes.iIndicesCur++);
nodes.index_list.insert(child1, nodes.iIndicesCur++);
nodes.box_centers.insert(box.Center, iBox);
nodes.box_extents.insert(box.Extents, iBox);
return(iBox);
}
void build_top_down(bool bSorted)
{
// build list of valid triangles & centers. We skip any
// triangles that have infinite/garbage vertices...
int i = 0;
int[] triangles = new int[mesh.TriangleCount];
Vector3d[] centers = new Vector3d[mesh.TriangleCount];
foreach (int ti in mesh.TriangleIndices())
{
Vector3d centroid = mesh.GetTriCentroid(ti);
double d2 = centroid.LengthSquared;
bool bInvalid = double.IsNaN(d2) || double.IsInfinity(d2);
Debug.Assert(bInvalid == false);
if (bInvalid == false)
{
triangles[i] = ti;
centers[i] = mesh.GetTriCentroid(ti);
i++;
} // otherwise skip this tri
}
boxes_set tris = new boxes_set();
boxes_set nodes = new boxes_set();
AxisAlignedBox3f rootBox;
int rootnode = (bSorted) ?
split_tri_set_sorted(triangles, centers, 0, mesh.TriangleCount, 0, TopDownLeafMaxTriCount, tris, nodes, out rootBox)
: split_tri_set_midpoint(triangles, centers, 0, mesh.TriangleCount, 0, TopDownLeafMaxTriCount, tris, nodes, out rootBox);
box_to_index = tris.box_to_index;
box_centers = tris.box_centers;
box_extents = tris.box_extents;
index_list = tris.index_list;
triangles_end = tris.iIndicesCur;
int iIndexShift = triangles_end;
int iBoxShift = tris.iBoxCur;
// ok now append internal node boxes & index ptrs
for (i = 0; i < nodes.iBoxCur; ++i)
{
box_centers.insert(nodes.box_centers[i], iBoxShift + i);
box_extents.insert(nodes.box_extents[i], iBoxShift + i);
// internal node indices are shifted
box_to_index.insert(iIndexShift + nodes.box_to_index[i], iBoxShift + i);
}
// now append index list
for (i = 0; i < nodes.iIndicesCur; ++i)
{
int child_box = nodes.index_list[i];
if (child_box < 0) // this is a triangles box
{
child_box = (-child_box) - 1;
}
else
{
child_box += iBoxShift;
}
child_box = child_box + 1;
index_list.insert(child_box, iIndexShift + i);
}
root_index = rootnode + iBoxShift;
}
void build_top_down(bool bSorted)
{
// build list of valid elements. We skip any
// elements that have infinite/garbage positoins...
int i = 0;
int N = points.VertexCount;
int[] valid_indices = new int[N];
Vector3d[] valid_points = new Vector3d[N];
foreach (int vi in points.VertexIndices())
{
Vector3d pt = points.GetVertex(vi);
double d2 = pt.LengthSquared;
bool bInvalid = double.IsNaN(d2) || double.IsInfinity(d2);
Debug.Assert(bInvalid == false);
if (bInvalid == false)
{
valid_indices[i] = vi;
valid_points[i] = pt;
i++;
} // otherwise skip this element
}
boxes_set leafs = new boxes_set();
boxes_set nodes = new boxes_set();
AxisAlignedBox3d rootBox;
int rootnode = (bSorted) ?
split_point_set_sorted(valid_indices, valid_points, 0, N, 0, LeafMaxPointCount, leafs, nodes, out rootBox)
: split_point_set_midpoint(valid_indices, valid_points, 0, N, 0, LeafMaxPointCount, leafs, nodes, out rootBox);
box_to_index = leafs.box_to_index;
box_centers = leafs.box_centers;
box_extents = leafs.box_extents;
index_list = leafs.index_list;
points_end = leafs.iIndicesCur;
int iIndexShift = points_end;
int iBoxShift = leafs.iBoxCur;
// ok now append internal node boxes & index ptrs
for (i = 0; i < nodes.iBoxCur; ++i)
{
box_centers.insert(nodes.box_centers[i], iBoxShift + i);
box_extents.insert(nodes.box_extents[i], iBoxShift + i);
// internal node indices are shifted
box_to_index.insert(iIndexShift + nodes.box_to_index[i], iBoxShift + i);
}
// now append index list
for (i = 0; i < nodes.iIndicesCur; ++i)
{
int child_box = nodes.index_list[i];
if (child_box < 0) // this is a points box
{
child_box = (-child_box) - 1;
}
else
{
child_box += iBoxShift;
}
child_box = child_box + 1;
index_list.insert(child_box, iIndexShift + i);
}
root_index = rootnode + iBoxShift;
}
