/// <summary>
/// Return the KDTreeNode that 'vertex' belongs to according to the split axis.
/// </summary>
/// <param name="vertex">3D vertex to bin</param>
/// <returns>KDTreeNode containing 'vertex'</returns>
public KDNodeJeremyC GetSplitNode(VertexKDTree vertex)
{
if (ChildLeft == null || ChildRight == null)
{
ChildLeft = new KDNodeJeremyC {
Parent = this
};
ChildRight = new KDNodeJeremyC {
Parent = this
};
ChildLeft.Sibling = ChildRight;
ChildRight.Sibling = ChildLeft;
}
float mid_value = Leaf.Vector[(int)SplitAxis];
float pt_value = vertex.Vector[(int)SplitAxis];
//float diff = pt_value - mid_value;
//float diff2 = Math.Abs(diff / pt_value);
//if (diff > 0)
// return ChildLeft;
//else if (diff < 0 && diff2 < 1e-7)
// return ChildLeft;
//else
// return ChildRight;
KDNodeJeremyC child = (pt_value >= mid_value) ? ChildLeft : ChildRight;
return(child);
}
/// <summary>
/// Recursively split node across its pivot axis.
/// </summary>
/// <param name="node">KDTreeNode to split</param>
/// <param name="depth">Current recursion depth, set lower if you get stack overflow</param>
/// <returns>True if split was a success or max_depth/max_node_size criterion met</returns>
bool BuildNode(KDNodeJeremyC node, int depth)
{
if (depth >= MaxNodeDepth)
{
return(true);
}
if (node.Indices.Count <= MaxNodeSize)
{
return(true);
}
foreach (var index in node.Indices)
{
VertexKDTree vertex = TreeVectors[index];
if (!node.IsBuilt)
{
node.Build();
}
KDNodeJeremyC child = node.GetSplitNode(vertex);
child.AddVertex(index, vertex);
}
// TODO: Do we need to check if either child is empty? Since we're calculating the split axis
// using raw data it's unlikely.
node.Clear();
node.ChildLeft.Build();
node.ChildRight.Build();
BuildNode(node.ChildLeft, depth + 1);
BuildNode(node.ChildRight, depth + 1);
return(true);
}
///// <summary>
///// Add a new vertex/point to the KDTree build process. Points must be added before calling KDTree.Build()
///// </summary>
///// <param name="vertex">3D vertex</param>
//public void AddPoint(VectorWithIndex vertex)
//{
// Vectors.Add(vertex);
//}
///// <summary>
///// Add a new vertex/point to the KDTree build process. Points must be added before calling KDTree.Build()
///// </summary>
///// <param name="x">VectorWithIndex.x</param>
///// <param name="y">VectorWithIndex.y</param>
///// <param name="z">VectorWithIndex.z</param>
//public void AddPoint(float x, float y, float z)
//{
// Vectors.Add(new VectorWithIndex(new Vector3(x,y,z), -1));
//}
/// <summary>
/// Add a new list of vertices/points to the KDTree build process. Points must be added before calling KDTree.Build()
/// </summary>
/// <param name="vertex">List of 3D vertices</param>
//public void AddPoints(List<VectorWithIndex> vertices)
//{
// Vectors = Vectors.Concat(vertices).ToList();
//}
///// <summary>
///// Add a new list of vertices/points to the KDTree build process. Points must be added before calling KDTree.Build()
///// </summary>
///// <param name="vertex">List of 3D vertices stored as a 1D array w/ every 3 elements defining a vertex.</param>
//public void AddPoints(List<float> vertices)
//{
// int element_size = 3;
// if (vertices.Count % element_size != 0) return; //vertex count must be a multiple of element_size
// for(int i = 0; i < vertices.Count; i+=element_size){
// float x = vertices[i];
// float y = vertices[i+1];
// float z = vertices[i+2];
// Vectors.Add(new VectorWithIndex(new Vector3(x, y, z), -1));
// }
//}
/// <summary>
/// Build KDTree
/// </summary>
/// <notes>
/// Points are added via KDTree.AddPoint(s) methods, the Build process uses
/// those points to create the final tree. If new points are added after a
/// tree has been built, a new tree must be created.
/// </notes>
/// <returns></returns>
public bool Build(PointCloud pcTarget)
{
this.targetTreee = pcTarget;
this.treePointCloud = pcTarget;
this.TreeVectors = treePointCloud.VectorsWithIndex;
if (VertexCount <= 0)
{
Console.WriteLine("[Warning] - No vertices added to KDTree, aborting build.");
return(false);
}
Root = new KDNodeJeremyC(); // { SplitAxis = largest_split_axis, AABB = new BoundingBoxAxisAligned(min,max), MidPoint = center, Parent = null};
{ // Fill the Root
int index = 0;
foreach (var vertex in TreeVectors)
{
root.AddVertex(index, vertex);
index++;
}
}
BuildNode(Root, 0);
return(true);
}
/// <summary>
/// Find closest point using an axis aligned search boundary
/// </summary>
/// <param name="node"></param>
/// <param name="point"></param>
/// <returns></returns>
VertexKDTree FindClosestPoint_Recursive(KDNodeJeremyC node, VertexKDTree vertex, BoundingBoxAxisAligned search_bounds, ref int nearest_index)
{
int tmp_index = -1;
if (node.IsLeaf)
{
tmp_index = -1;
VertexKDTree result = GetClosestVertexFromIndices(node.Indices, vertex, ref tmp_index);
if (result != null)
{
nearest_index = tmp_index;
return(result);
}
else
{
return(null);
}
}
tmp_index = -1;
KDNodeJeremyC near_child = node.GetSplitNode(vertex);
VertexKDTree retV = FindClosestPoint_Recursive(near_child, vertex, search_bounds, ref tmp_index);
//Edgar - TakenVector implementation - have to go one level up if vector is taken
float near_distance = float.MaxValue;
if (retV != null)
{
//near_distance = retV.Vector.Distance(vertex.Vector);
near_distance = retV.Vector.DistanceSquared(vertex.Vector);
nearest_index = tmp_index;
}
KDNodeJeremyC far_child = near_child.Sibling;
if (search_bounds != null && far_child.BoundingBox.Intersects(search_bounds))
{
VertexKDTree far_result = FindClosestPoint_Recursive(far_child, vertex, search_bounds, ref tmp_index);
//Edgar - TakenVector implementation - have to go one level up if vector is taken
if (far_result != null)
{
//float far_distance = far_result.Vector.Distance(vertex.Vector);
float far_distance = far_result.Vector.DistanceSquared(vertex.Vector);
if (far_distance < near_distance)
{
nearest_index = tmp_index;
retV = far_result;
}
}
}
if (retV == null)
{
}
return(retV);
}
/// <summary>
/// Find closest point using a centered bounding sphere
/// </summary>
/// <param name="node"></param>
/// <param name="vertex"></param>
/// <param name="search_bounds"></param>
/// <param name="nearest_index"></param>
/// <returns></returns>
VertexKDTree FindClosestPoint(KDNodeJeremyC node, VertexKDTree vertex, BoundingSphere search_bounds, ref int nearest_index)
{
int tmp_index = -1;
if (node.IsLeaf)
{
tmp_index = -1;
VertexKDTree result = GetClosestVertexFromIndices(node.Indices, vertex, ref tmp_index);
nearest_index = tmp_index;
return(result);
}
tmp_index = -1;
KDNodeJeremyC near_child = node.GetSplitNode(vertex);
VertexKDTree near_result = FindClosestPoint(near_child, vertex, search_bounds, ref tmp_index);
VertexKDTree ret = near_result;
//float near_distance = near_result.Vector.Distance(vertex.Vector);
float near_distance = near_result.Vector.DistanceSquared(vertex.Vector);
nearest_index = tmp_index;
KDNodeJeremyC far_child = near_child.Sibling;
if (far_child.BoundingBox.Intersects(search_bounds))
{
VertexKDTree far_result = FindClosestPoint(far_child, vertex, search_bounds, ref tmp_index);
float far_distance = far_result.Vector.DistanceSquared(vertex.Vector);
//float far_distance = far_result.Vector.Distance(vertex.Vector);
if (far_distance < near_distance)
{
nearest_index = tmp_index;
ret = far_result;
}
}
return(ret);
}
/// <summary>
/// Purge vertex memory and root
/// </summary>
public void Clear()
{
TreeVectors.Clear();
root = null;
}