/// <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); }