private void knearest(KQueue _priQueue, Vector3 point, Point p, int depth) { if (p == null) { return; } var axis = depth % 3; var distOnAxis = points[p.mid].pos[axis] - point[axis]; if (distOnAxis > 0) { knearest(_priQueue, point, p.smaller, depth + 1); var sqDist2leaf = sqDist(point, _priQueue.head().ipos); if (sqDist2leaf > distOnAxis * distOnAxis) { knearest(_priQueue, point, p.larger, depth + 1); } } else { knearest(_priQueue, point, p.larger, depth + 1); var sqDist2leaf = sqDist(point, _priQueue.head().ipos); if (sqDist2leaf > distOnAxis * distOnAxis) { knearest(_priQueue, point, p.smaller, depth + 1); } } _priQueue.enqueue(new PriorityPoint(p.mid, sqDist(point, p.mid))); }
public int[] knearest(KQueue _priQueue, Vector3 point, int k) { _priQueue.reset(); _priQueue.resize(k); _priQueue.enqueue(new PriorityPoint(-1, float.PositiveInfinity)); knearest(_priQueue, point, _root, 0); return((from node in _priQueue.Reverse() where node.ipos >= 0 select points[node.ipos].id).ToArray()); }