protected int queryIndex = 0; // current index at stack
/// <summary>
/// Returns initialized node from stack that also acts as a pool
/// The returned reference to node stays in stack
/// </summary>
/// <returns>Reference to pooled node</returns>
private KDQueryNode PushGetQueue()
{
KDQueryNode node = null;
if (count < queueArray.Length)
{
if (queueArray[count] == null)
{
queueArray[count] = node = new KDQueryNode();
}
else
{
node = queueArray[count];
}
}
else
{
// automatic resize of pool
Array.Resize(ref queueArray, queueArray.Length * 2);
node = queueArray[count] = new KDQueryNode();
}
count++;
return(node);
}
protected KDQueryNode PopFromHeap()
{
KDQueryNode heapNode = minHeap.PopObj();
queueArray[queryIndex] = heapNode;
queryIndex++;
return(heapNode);
}
public void ClosestPoint(KDTree tree, Vector3 queryPosition, List <int> resultIndices, List <float> resultDistances = null)
{
Reset();
Vector3[] points = tree.Points;
int[] permutation = tree.Permutation;
int smallestIndex = 0;
/// Smallest Squared Radius
float SSR = Single.PositiveInfinity;
var rootNode = tree.RootNode;
Vector3 rootClosestPoint = rootNode.bounds.ClosestPoint(queryPosition);
PushToHeap(rootNode, rootClosestPoint, queryPosition);
KDQueryNode queryNode = null;
KDNode node = null;
int partitionAxis;
float partitionCoord;
Vector3 tempClosestPoint;
// searching
while (minHeap.Count > 0)
{
queryNode = PopFromHeap();
if (queryNode.distance > SSR)
{
continue;
}
node = queryNode.node;
if (!node.Leaf)
{
partitionAxis = node.partitionAxis;
partitionCoord = node.partitionCoordinate;
tempClosestPoint = queryNode.tempClosestPoint;
if ((tempClosestPoint[partitionAxis] - partitionCoord) < 0)
{
// we already know we are on the side of negative bound/node,
// so we don't need to test for distance
// push to stack for later querying
PushToHeap(node.negativeChild, tempClosestPoint, queryPosition);
// project the tempClosestPoint to other bound
tempClosestPoint[partitionAxis] = partitionCoord;
if (node.positiveChild.Count != 0)
{
PushToHeap(node.positiveChild, tempClosestPoint, queryPosition);
}
}
else
{
// we already know we are on the side of positive bound/node,
// so we don't need to test for distance
// push to stack for later querying
PushToHeap(node.positiveChild, tempClosestPoint, queryPosition);
// project the tempClosestPoint to other bound
tempClosestPoint[partitionAxis] = partitionCoord;
if (node.positiveChild.Count != 0)
{
PushToHeap(node.negativeChild, tempClosestPoint, queryPosition);
}
}
}
else
{
float sqrDist;
// LEAF
for (int i = node.start; i < node.end; i++)
{
int index = permutation[i];
sqrDist = Vector3.SqrMagnitude(points[index] - queryPosition);
if (sqrDist <= SSR)
{
SSR = sqrDist;
smallestIndex = index;
}
}
}
}
resultIndices.Add(smallestIndex);
if (resultDistances != null)
{
resultDistances.Add(SSR);
}
}
/// <summary>
/// Returns indices to k closest points, and optionaly can return distances
/// </summary>
/// <param name="tree">Tree to do search on</param>
/// <param name="queryPosition">Position</param>
/// <param name="k">Max number of points</param>
/// <param name="resultIndices">List where resulting indices will be stored</param>
/// <param name="resultDistances">Optional list where resulting distances will be stored</param>
public void KNearest(KDTree tree, Vector3 queryPosition, int k, List <int> resultIndices, List <float> resultDistances = null)
{
// pooled heap arrays
KSmallestHeap <int> kHeap;
_heaps.TryGetValue(k, out kHeap);
if (kHeap == null)
{
kHeap = new KSmallestHeap <int>(k);
_heaps.Add(k, kHeap);
}
kHeap.Clear();
Reset();
Vector3[] points = tree.Points;
int[] permutation = tree.Permutation;
///Biggest Smallest Squared Radius
float BSSR = Single.PositiveInfinity;
var rootNode = tree.RootNode;
Vector3 rootClosestPoint = rootNode.bounds.ClosestPoint(queryPosition);
PushToHeap(rootNode, rootClosestPoint, queryPosition);
KDQueryNode queryNode = null;
KDNode node = null;
int partitionAxis;
float partitionCoord;
Vector3 tempClosestPoint;
// searching
while (minHeap.Count > 0)
{
queryNode = PopFromHeap();
if (queryNode.distance > BSSR)
{
continue;
}
node = queryNode.node;
if (!node.Leaf)
{
partitionAxis = node.partitionAxis;
partitionCoord = node.partitionCoordinate;
tempClosestPoint = queryNode.tempClosestPoint;
if ((tempClosestPoint[partitionAxis] - partitionCoord) < 0)
{
// we already know we are on the side of negative bound/node,
// so we don't need to test for distance
// push to stack for later querying
PushToHeap(node.negativeChild, tempClosestPoint, queryPosition);
// project the tempClosestPoint to other bound
tempClosestPoint[partitionAxis] = partitionCoord;
if (node.positiveChild.Count != 0)
{
PushToHeap(node.positiveChild, tempClosestPoint, queryPosition);
}
}
else
{
// we already know we are on the side of positive bound/node,
// so we don't need to test for distance
// push to stack for later querying
PushToHeap(node.positiveChild, tempClosestPoint, queryPosition);
// project the tempClosestPoint to other bound
tempClosestPoint[partitionAxis] = partitionCoord;
if (node.positiveChild.Count != 0)
{
PushToHeap(node.negativeChild, tempClosestPoint, queryPosition);
}
}
}
else
{
float sqrDist;
// LEAF
for (int i = node.start; i < node.end; i++)
{
int index = permutation[i];
sqrDist = Vector3.SqrMagnitude(points[index] - queryPosition);
if (sqrDist <= BSSR)
{
kHeap.PushObj(index, sqrDist);
if (kHeap.Full)
{
BSSR = kHeap.HeadValue;
}
}
}
}
}
kHeap.FlushResult(resultIndices, resultDistances);
}
/// <summary>
/// Search by radius method.
/// </summary>
/// <param name="tree">Tree to do search on</param>
/// <param name="queryPosition">Position</param>
/// <param name="queryRadius">Radius</param>
/// <param name="resultIndices">Initialized list, cleared.</param>
public void Radius(KDTree tree, Vector3 queryPosition, float queryRadius, List <int> resultIndices)
{
Reset();
Vector3[] points = tree.Points;
int[] permutation = tree.Permutation;
float squaredRadius = queryRadius * queryRadius;
var rootNode = tree.RootNode;
PushToQueue(rootNode, rootNode.bounds.ClosestPoint(queryPosition));
KDQueryNode queryNode = null;
KDNode node = null;
// KD search with pruning (don't visit areas which distance is more away than range)
// Recursion done on Stack
while (LeftToProcess > 0)
{
queryNode = PopFromQueue();
node = queryNode.node;
if (!node.Leaf)
{
int partitionAxis = node.partitionAxis;
float partitionCoord = node.partitionCoordinate;
Vector3 tempClosestPoint = queryNode.tempClosestPoint;
if ((tempClosestPoint[partitionAxis] - partitionCoord) < 0)
{
// we already know we are inside negative bound/node,
// so we don't need to test for distance
// push to stack for later querying
// tempClosestPoint is inside negative side
// assign it to negativeChild
PushToQueue(node.negativeChild, tempClosestPoint);
tempClosestPoint[partitionAxis] = partitionCoord;
float sqrDist = Vector3.SqrMagnitude(tempClosestPoint - queryPosition);
// testing other side
if (node.positiveChild.Count != 0 &&
sqrDist <= squaredRadius)
{
PushToQueue(node.positiveChild, tempClosestPoint);
}
}
else
{
// we already know we are inside positive bound/node,
// so we don't need to test for distance
// push to stack for later querying
// tempClosestPoint is inside positive side
// assign it to positiveChild
PushToQueue(node.positiveChild, tempClosestPoint);
// project the tempClosestPoint to other bound
tempClosestPoint[partitionAxis] = partitionCoord;
float sqrDist = Vector3.SqrMagnitude(tempClosestPoint - queryPosition);
// testing other side
if (node.negativeChild.Count != 0 &&
sqrDist <= squaredRadius)
{
PushToQueue(node.negativeChild, tempClosestPoint);
}
}
}
else
{
// LEAF
for (int i = node.start; i < node.end; i++)
{
int index = permutation[i];
if (Vector3.SqrMagnitude(points[index] - queryPosition) <= squaredRadius)
{
resultIndices.Add(index);
}
}
}
}
}
public void Interval(KDTree tree, Vector3 min, Vector3 max, List <int> resultIndices)
{
Reset();
Vector3[] points = tree.Points;
int[] permutation = tree.Permutation;
var rootNode = tree.RootNode;
PushToQueue(
rootNode,
rootNode.bounds.ClosestPoint((min + max) / 2)
);
KDQueryNode queryNode = null;
KDNode node = null;
// KD search with pruning (don't visit areas which distance is more away than range)
// Recursion done on Stack
while (LeftToProcess > 0)
{
queryNode = PopFromQueue();
node = queryNode.node;
if (!node.Leaf)
{
int partitionAxis = node.partitionAxis;
float partitionCoord = node.partitionCoordinate;
Vector3 tempClosestPoint = queryNode.tempClosestPoint;
if ((tempClosestPoint[partitionAxis] - partitionCoord) < 0)
{
// we already know we are inside negative bound/node,
// so we don't need to test for distance
// push to stack for later querying
// tempClosestPoint is inside negative side
// assign it to negativeChild
PushToQueue(node.negativeChild, tempClosestPoint);
tempClosestPoint[partitionAxis] = partitionCoord;
// testing other side
if (node.positiveChild.Count != 0 &&
tempClosestPoint[partitionAxis] <= max[partitionAxis])
{
PushToQueue(node.positiveChild, tempClosestPoint);
}
}
else
{
// we already know we are inside positive bound/node,
// so we don't need to test for distance
// push to stack for later querying
// tempClosestPoint is inside positive side
// assign it to positiveChild
PushToQueue(node.positiveChild, tempClosestPoint);
// project the tempClosestPoint to other bound
tempClosestPoint[partitionAxis] = partitionCoord;
// testing other side
if (node.negativeChild.Count != 0 &&
tempClosestPoint[partitionAxis] >= min[partitionAxis])
{
PushToQueue(node.negativeChild, tempClosestPoint);
}
}
}
else
{
// LEAF
// testing if node bounds are inside the query interval
if (node.bounds.min[0] >= min[0] &&
node.bounds.min[1] >= min[1] &&
node.bounds.min[2] >= min[2]
&& node.bounds.max[0] <= max[0] &&
node.bounds.max[1] <= max[1] &&
node.bounds.max[2] <= max[2])
{
for (int i = node.start; i < node.end; i++)
{
resultIndices.Add(permutation[i]);
}
}
// node is not inside query interval, need to do test on each point separately
else
{
for (int i = node.start; i < node.end; i++)
{
int index = permutation[i];
Vector3 v = points[index];
if (v[0] >= min[0] &&
v[1] >= min[1] &&
v[2] >= min[2]
&& v[0] <= max[0] &&
v[1] <= max[1] &&
v[2] <= max[2])
{
resultIndices.Add(index);
}
}
}
}
}
}
