public bool Build(PointCloud pcTarget)
{
this.targetTreee = pcTarget;
this.TreeVectors = pcTarget.VectorsWithIndex;
this.sort_results = false;
this.root = null;
this.Indices = new uint[pcTarget.Vectors.Length];
this.TreeVectors = pcTarget.VectorsWithIndex;
for (uint i = 0; i < this.TreeVectors.Count; i++)
{
Indices[i] = i;
}
root = new KDTreeNodeKennell(this);
root = root.build_tree_for_range(0, TreeVectors.Count - 1, null);
return(true);
}
// utility
//public static void swap(ref uint a, ref uint b)
//{
// uint tmp;
// tmp = a;
// a = b;
// b = tmp;
//}
//public static void swap(ref float a, ref float b)
//{
// float tmp;
// tmp = a;
// a = b;
// b = tmp;
//}
#endregion
#region base implementation
public void Dispose()
{
root = null;
}
public KDTreeNodeKennell build_tree_for_range(int startIndex, int endIndex, KDTreeNodeKennell parent)
{
// recursive function to build
KDTreeNodeKennell node = new KDTreeNodeKennell(this.Parent);
// the newly created node.
if (endIndex < startIndex)
{
return(null); // no data in this node.
}
if ((endIndex - startIndex) <= thresholdForApproximateTree)
{
// create a leaf node.
// always compute true bounding box for leaf node.
for (int i = 0; i < 3; i++)
{
node.box[i] = spread_in_coordinate(i, startIndex, endIndex);
}
node.cutAxis = 0;
node.cut_val = 0f;
node.IndexLeafVector = startIndex;
node.endIndex = endIndex;
node.ChildLeft = node.ChildRight = null;
}
else
{
//
// Compute an APPROXIMATE bounding box for this node.
// if parent == NULL, then this is the root node, and
// we compute for all dimensions.
// Otherwise, we copy the bounding box from the parent for
// all coordinates except for the parent's cut dimension.
// That, we recompute ourself.
//
int c = -1;
//int c = 0;
float maxspread = 0.0F;
int mEndIndexToBuild;
for (int i = 0; i < 3; i++)
{
if ((parent == null) || (parent.cutAxis == i))
{
node.box[i] = spread_in_coordinate(i, startIndex, endIndex);
}
else
{
node.box[i] = parent.box[i];
}
float spread = node.box[i].upper - node.box[i].lower;
if (spread > maxspread)
{
maxspread = spread;
c = i;
}
}
if (c == -1)
{
System.Windows.Forms.MessageBox.Show("Error in building the tree: The point cloud contains duplicates. Please remove the duplicates (see help)");
return(null);
}
float sum;
float average;
sum = 0.0F;
for (int k = startIndex; k <= endIndex; k++)
{
sum += this.Parent.TreeVectors[Convert.ToInt32(this.Parent.Indices[k])].Vector[c];
}
average = sum / (float)(endIndex - startIndex + 1);
mEndIndexToBuild = select_on_coordinate_value(c, average, startIndex, endIndex);
// move the indices around to cut on dim 'c'.
node.cutAxis = c;
node.IndexLeafVector = startIndex;
node.endIndex = endIndex;
node.ChildLeft = build_tree_for_range(startIndex, mEndIndexToBuild, node);
node.ChildRight = build_tree_for_range(mEndIndexToBuild + 1, endIndex, node);
if (node.ChildRight == null && node.ChildLeft == null)
{
System.Windows.Forms.MessageBox.Show("Error in building tree");
}
if (node.ChildRight == null)
{
for (int i = 0; i < 3; i++)
{
node.box[i] = node.ChildLeft.box[i];
}
node.cut_val = node.ChildLeft.box[c].upper;
node.cut_val_left = node.cut_val_right = node.cut_val;
}
else if (node.ChildLeft == null)
{
for (int i = 0; i < 3; i++)
{
node.box[i] = node.ChildRight.box[i];
}
node.cut_val = node.ChildRight.box[c].upper;
node.cut_val_left = node.cut_val_right = node.cut_val;
}
else
{
node.cut_val_right = node.ChildRight.box[c].lower;
node.cut_val_left = node.ChildLeft.box[c].upper;
node.cut_val = (node.cut_val_left + node.cut_val_right) / 2f;
//
// now recompute true bounding box as union of subtree boxes.
// This is now faster having built the tree, being logarithmic in
// N, not linear as would be from naive method.
//
for (int i = 0; i < 3; i++)
{
interval intv = new interval(
Math.Min(node.ChildLeft.box[i].lower, node.ChildRight.box[i].lower),
Math.Max(node.ChildLeft.box[i].upper, node.ChildRight.box[i].upper)
);
node.box[i] = intv;
}
}
}
return(node);
}
