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