/// <summary>
/// Insert a new data item at a given key.
/// </summary>
/// <param name="key">The value which represents our data (i.e. a distance).</param>
/// <param name="value">The data we want to store.</param>
public void Insert(double key, KDRectangle value)
{
// If more room is needed, double the array size.
if (Size >= Capacity)
{
// Double the capacity.
Capacity *= 2;
// Expand the data array.
var newData = new KDRectangle[Capacity];
Array.Copy(tData, newData, tData.Length);
tData = newData;
// Expand the key array.
var newKeys = new double[Capacity];
Array.Copy(tKeys, newKeys, tKeys.Length);
tKeys = newKeys;
}
// Insert the new value at the end.
Size++;
tData[Size - 1] = value;
tKeys[Size - 1] = key;
// Ensure it is in the right place.
SiftInsertedValueUp();
}
/// <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 Node();
pLeft = new Node();
// Move each item in this leaf into the children.
for (int i = 0; i < Size; ++i)
{
// Store.
double[] tOldPoint = tPoints[i];
KDRectangle 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>
/// 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(double[] tPoint, KDRectangle kValue)
{
// Find the correct leaf node.
Node 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>
/// Internal helper method which swaps two values in the arrays.
/// This swaps both data and key entries.
/// </summary>
/// <param name="x">The first index.</param>
/// <param name="y">The second index.</param>
/// <returns>The second index.</returns>
private int Swap(int x, int y)
{
// Store temp.
KDRectangle yData = tData[y];
double yDist = tKeys[y];
// Swap
tData[y] = tData[x];
tKeys[y] = tKeys[x];
tData[x] = yData;
tKeys[x] = yDist;
// Return.
return(y);
}
/// <summary>
/// Swap out the item with the largest key in the queue.
/// </summary>
/// <param name="key">The new key for the largest item.</param>
/// <param name="value">The new data for the largest item.</param>
public void ReplaceMax(double key, KDRectangle value)
{
if (Size == 0)
{
throw new Exception();
}
else if (Size == 1)
{
ReplaceMin(key, value);
return;
}
tData[1] = value;
tKeys[1] = key;
// Swap with pair if necessary
if (key < tKeys[0])
{
Swap(0, 1);
}
SiftDownMax(1);
}
/// <summary>
/// Replace the item with the smallest key in the queue.
/// </summary>
/// <param name="key">The new minimum key.</param>
/// <param name="value">The new minumum data value.</param>
public void ReplaceMin(double key, KDRectangle value)
{
// Check for errors.
if (Size == 0)
{
throw new Exception();
}
// Add the data.
tData[0] = value;
tKeys[0] = key;
// If we have more than one item.
if (Size > 1)
{
// Swap with pair if necessary.
if (tKeys[1] < key)
{
Swap(0, 1);
}
SiftDownMin(0);
}
}
/// <summary>
/// Insert the point into the leaf.
/// </summary>
/// <param name="tPoint">The point to insert the data at.</param>
/// <param name="kValue">The value at the point.</param>
private void AddLeafPoint(double[] tPoint, KDRectangle kValue)
{
// Add the data point to this node.
tPoints[Size] = tPoint;
tData[Size] = kValue;
ExtendBounds(tPoint);
Size++;
// Split if the node is getting too large in terms of data.
if (Size == tPoints.Length - 1)
{
// If the node is getting too physically large.
if (CalculateSplit())
{
// If the node successfully had it's split value calculated, split node.
SplitLeafNode();
}
else
{
// If the node could not be split, enlarge node data capacity.
IncreaseLeafCapacity();
}
}
}
/// <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 = default(KDRectangle);
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
Node pCursor = pPending.Min;
pPending.RemoveMin();
// Descend the tree, recording paths not taken
while (!pCursor.IsLeaf)
{
Node 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 = this.DistanceToRectangle(tSearchPoint, pNotTaken.tMinBound, pNotTaken.tMaxBound);
// If it is greater than the threshold, skip.
if (-1 >= 0 && fDistance > -1)
{
//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 = this.Distance(pCursor.tPoints[0], tSearchPoint);
// Skip if the point exceeds the threshold.
// Technically this should never happen, but be prescise.
if (-1 >= 0 && fDistance >= -1)
{
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 = this.Distance(pCursor.tPoints[i], tSearchPoint);
// Skip if it exceeds the threshold.
if (-1 >= 0 && fDistance >= -1)
{
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);
}
