/// <summary>
/// Insert a new point into this leaf node.
/// </summary>
/// <param name="tPoint">The position which represents the data.</param>
/// <param name="kValue">The value of the data.</param>
public void AddPoint(Point3D kValue)
{
Vector3d tPoint = kValue.Position;
// Find the correct leaf node.
KDNode pCursor = this;
while (!pCursor.IsLeaf)
{
// Extend the size of the leaf.
pCursor.ExtendBounds(tPoint);
pCursor.Size++;
// If it is larger select the right, or lower, select the left.
if (tPoint[pCursor.iSplitDimension] > pCursor.fSplitValue)
{
pCursor = pCursor.pRight;
}
else
{
pCursor = pCursor.pLeft;
}
}
// Insert it into the leaf.
pCursor.AddLeafPoint(tPoint, kValue);
}
/// <summary>
/// Split this leaf node by creating left and right children, then moving all the children of
/// this node into the respective buckets.
/// </summary>
private void SplitLeafNode()
{
// Create the new children.
pRight = new KDNode(iBucketCapacity);
pLeft = new KDNode(iBucketCapacity);
// Move each item in this leaf into the children.
for (int i = 0; i < Size; ++i)
{
// Store.
Vector3d tOldPoint = tPoints[i];
Point3D kOldData = tData[i];
// If larger, put it in the right.
if (tOldPoint[iSplitDimension] > fSplitValue)
{
pRight.AddLeafPoint(tOldPoint, kOldData);
}
// If smaller, put it in the left.
else
{
pLeft.AddLeafPoint(tOldPoint, kOldData);
}
}
// Wipe the data from this KDNode.
tPoints = null;
tData = null;
}
/// <summary>
/// Bubble a parent down through the children so it goes in the right place.
/// </summary>
/// <param name="iParent">The index of the parent.</param>
private void SiftDown(int iParent)
{
// For each child.
for (int iChild = iParent * 2 + 1; iChild < Size; iParent = iChild, iChild = iParent * 2 + 1)
{
// If the child is larger, select the next child.
if (iChild + 1 < Size && tKeys[iChild] > tKeys[iChild + 1])
{
iChild++;
}
// If the parent is larger than the largest child, swap.
if (tKeys[iParent] > tKeys[iChild])
{
// Swap the points
KDNode pData = tData[iParent];
double pDist = tKeys[iParent];
tData[iParent] = tData[iChild];
tKeys[iParent] = tKeys[iChild];
tData[iChild] = pData;
tKeys[iChild] = pDist;
}
// TODO: REMOVE THE BREAK
else
{
break;
}
}
}
/// <summary>
/// Insert a new element.
/// </summary>
/// <param name="key">The key which represents its position in the priority queue (ie. distance).</param>
/// <param name="value">The value to be stored at the key.</param>
public void Insert(double key, KDNode value)
{
// If we need more room, double the space.
if (Size >= Capacity)
{
// Calcualte the new capacity.
Capacity *= 2;
// Copy the data array.
var newData = new KDNode[Capacity];
Array.Copy(tData, newData, tData.Length);
tData = newData;
// Copy the key array.
var newKeys = new double[Capacity];
Array.Copy(tKeys, newKeys, tKeys.Length);
tKeys = newKeys;
}
// Insert new value at the end
tData[Size] = value;
tKeys[Size] = key;
SiftUp(Size);
Size++;
}
/// <summary>
/// Insert a new element.
/// </summary>
/// <param name="key">The key which represents its position in the priority queue (ie. distance).</param>
/// <param name="value">The value to be stored at the key.</param>
public void Insert(double key, KDNode value)
{
// If we need more room, double the space.
if (Size >= Capacity)
{
// Calcualte the new capacity.
Capacity *= 2;
// Copy the data array.
var newData = new KDNode[Capacity];
Array.Copy(tData, newData, tData.Length);
tData = newData;
// Copy the key array.
var newKeys = new double[Capacity];
Array.Copy(tKeys, newKeys, tKeys.Length);
tKeys = newKeys;
}
// Insert new value at the end
tData[Size] = value;
tKeys[Size] = key;
SiftUp(Size);
Size++;
}
/// <summary>
/// Bubble a child item up the tree.
/// </summary>
/// <param name="iChild"></param>
private void SiftUp(int iChild)
{
// For each parent above the child, if the parent is smaller then bubble it up.
for (int iParent = (iChild - 1) / 2;
iChild != 0 && tKeys[iChild] < tKeys[iParent];
iChild = iParent, iParent = (iChild - 1) / 2)
{
KDNode kData = tData[iParent];
double dDist = tKeys[iParent];
tData[iParent] = tData[iChild];
tKeys[iParent] = tKeys[iChild];
tData[iChild] = kData;
tKeys[iChild] = dDist;
}
}
/// <summary>
/// Construct a new nearest neighbour iterator.
/// </summary>
/// <param name="pRoot">The root of the tree to begin searching from.</param>
/// <param name="tSearchPoint">The point in n-dimensional space to search.</param>
/// <param name="kDistance">The distance function used to evaluate the points.</param>
/// <param name="iMaxPoints">The max number of points which can be returned by this iterator. Capped to max in tree.</param>
/// <param name="fThreshold">Threshold to apply to the search space. Negative numbers indicate that no threshold is applied.</param>
public NearestNeighbour(KDNode pRoot, Vector3d tSearchPoint, DistanceFunctions kDistance, int iMaxPoints, double fThreshold)
{
// Store the search point.
this.tSearchPoint = new Vector3d(tSearchPoint);
// Store the point count, distance function and tree root.
this.iPointsRemaining = Math.Min(iMaxPoints, pRoot.Size);
this.fThreshold = fThreshold;
this.kDistanceFunction = kDistance;
this.pRoot = pRoot;
this.iMaxPointsReturned = iMaxPoints;
_CurrentDistance = -1;
// Create an interval heap for the points we check.
this.pEvaluated = new IntervalHeap();
// Create a min heap for the things we need to check.
this.pPending = new MinHeap();
this.pPending.Insert(0, pRoot);
}
/// <summary>
/// Construct a new nearest neighbour iterator.
/// </summary>
/// <param name="pRoot">The root of the tree to begin searching from.</param>
/// <param name="tSearchPoint">The point in n-dimensional space to search.</param>
/// <param name="kDistance">The distance function used to evaluate the points.</param>
/// <param name="iMaxPoints">The max number of points which can be returned by this iterator. Capped to max in tree.</param>
/// <param name="fThreshold">Threshold to apply to the search space. Negative numbers indicate that no threshold is applied.</param>
public NearestNeighbour(KDNode pRoot, Vector3d tSearchPoint, DistanceFunctions kDistance, int iMaxPoints, double fThreshold)
{
// Store the search point.
this.tSearchPoint = new Vector3d(tSearchPoint);
// Store the point count, distance function and tree root.
this.iPointsRemaining = Math.Min(iMaxPoints, pRoot.Size);
this.fThreshold = fThreshold;
this.kDistanceFunction = kDistance;
this.pRoot = pRoot;
this.iMaxPointsReturned = iMaxPoints;
_CurrentDistance = -1;
// Create an interval heap for the points we check.
this.pEvaluated = new IntervalHeap();
// Create a min heap for the things we need to check.
this.pPending = new MinHeap();
this.pPending.Insert(0, pRoot);
}
/// <summary>
/// Split this leaf node by creating left and right children, then moving all the children of
/// this node into the respective buckets.
/// </summary>
private void SplitLeafNode()
{
// Create the new children.
pRight = new KDNode(iBucketCapacity);
pLeft = new KDNode(iBucketCapacity);
// Move each item in this leaf into the children.
for (int i = 0; i < Size; ++i)
{
// Store.
Vector3d tOldPoint = tPoints[i];
Point3D kOldData = tData[i];
// If larger, put it in the right.
if (tOldPoint[iSplitDimension] > fSplitValue)
pRight.AddLeafPoint(tOldPoint, kOldData);
// If smaller, put it in the left.
else
pLeft.AddLeafPoint(tOldPoint, kOldData);
}
// Wipe the data from this KDNode.
tPoints = null;
tData = null;
}
/// <summary>
/// Check for the next iterator item.
/// </summary>
/// <returns>True if we have one, false if not.</returns>
public bool MoveNext()
{
// Bail if we are finished.
if (iPointsRemaining == 0)
{
_Current = new Point3D();
return(false);
}
// While we still have paths to evaluate.
while (pPending.Size > 0 && (pEvaluated.Size == 0 || (pPending.MinKey < pEvaluated.MinKey)))
{
// If there are pending paths possibly closer than the nearest evaluated point, check it out
KDNode pCursor = pPending.Min;
pPending.RemoveMin();
// Descend the tree, recording paths not taken
while (!pCursor.IsLeaf)
{
KDNode pNotTaken;
// If the seach point is larger, select the right path.
if (tSearchPoint[pCursor.iSplitDimension] > pCursor.fSplitValue)
{
pNotTaken = pCursor.pLeft;
pCursor = pCursor.pRight;
}
else
{
pNotTaken = pCursor.pRight;
pCursor = pCursor.pLeft;
}
// Calculate the shortest distance between the search point and the min and max bounds of the kd-node.
double fDistance = kDistanceFunction.DistanceToRectangle(tSearchPoint, pNotTaken.tMinBound, pNotTaken.tMaxBound);
// If it is greater than the threshold, skip.
if (fThreshold >= 0 && fDistance > fThreshold)
{
//pPending.Insert(fDistance, pNotTaken);
continue;
}
// Only add the path we need more points or the node is closer than furthest point on list so far.
if (pEvaluated.Size < iPointsRemaining || fDistance <= pEvaluated.MaxKey)
{
pPending.Insert(fDistance, pNotTaken);
}
}
// If all the points in this KD node are in one place.
if (pCursor.bSinglePoint)
{
// Work out the distance between this point and the search point.
double fDistance = kDistanceFunction.Distance(pCursor.tPoints[0], tSearchPoint);
// Skip if the point exceeds the threshold.
// Technically this should never happen, but be prescise.
if (fThreshold >= 0 && fDistance >= fThreshold)
{
continue;
}
// Add the point if either need more points or it's closer than furthest on list so far.
if (pEvaluated.Size < iPointsRemaining || fDistance <= pEvaluated.MaxKey)
{
for (int i = 0; i < pCursor.Size; ++i)
{
// If we don't need any more, replace max
if (pEvaluated.Size == iPointsRemaining)
{
pEvaluated.ReplaceMax(fDistance, pCursor.tData[i]);
}
// Otherwise insert.
else
{
pEvaluated.Insert(fDistance, pCursor.tData[i]);
}
}
}
}
// If the points in the KD node are spread out.
else
{
// Treat the distance of each point seperately.
for (int i = 0; i < pCursor.Size; ++i)
{
// Compute the distance between the points.
double fDistance = kDistanceFunction.Distance(pCursor.tPoints[i], tSearchPoint);
// Skip if it exceeds the threshold.
if (fThreshold >= 0 && fDistance >= fThreshold)
{
continue;
}
// Insert the point if we have more to take.
if (pEvaluated.Size < iPointsRemaining)
{
pEvaluated.Insert(fDistance, pCursor.tData[i]);
}
// Otherwise replace the max.
else if (fDistance < pEvaluated.MaxKey)
{
pEvaluated.ReplaceMax(fDistance, pCursor.tData[i]);
}
}
}
}
// Select the point with the smallest distance.
if (pEvaluated.Size == 0)
{
return(false);
}
iPointsRemaining--;
_CurrentDistance = pEvaluated.MinKey;
_Current = pEvaluated.Min;
pEvaluated.RemoveMin();
return(true);
}
