KDNode GetKDNode()
{
KDNode node = null;
if (kdNodesCount < kdNodesStack.Length)
{
if (kdNodesStack[kdNodesCount] == null)
{
kdNodesStack[kdNodesCount] = node = new KDNode();
}
else
{
node = kdNodesStack[kdNodesCount];
node.partitionAxis = -1;
}
}
else
{
// automatic resize of KDNode pool array
Array.Resize(ref kdNodesStack, kdNodesStack.Length * 2);
node = kdNodesStack[kdNodesCount] = new KDNode();
}
kdNodesCount++;
return(node);
}
protected void PushToQueue(KDNode node, Vector3 tempClosestPoint)
{
var queryNode = PushGetQueue();
queryNode.node = node;
queryNode.tempClosestPoint = tempClosestPoint;
}
protected void PushToHeap(KDNode node, Vector3 tempClosestPoint, Vector3 queryPosition)
{
var queryNode = PushGetQueue();
queryNode.node = node;
queryNode.tempClosestPoint = tempClosestPoint;
float sqrDist = Vector3.SqrMagnitude(tempClosestPoint - queryPosition);
queryNode.distance = sqrDist;
minHeap.PushObj(queryNode, sqrDist);
}
/// <summary>
/// Constraint function. You can add custom constraints here - if you have some other data/classes binded to Vector3 points
/// Can hardcode it into
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
bool ContinueSplit(KDNode node)
{
return(node.Count > maxPointsPerLeafNode);
}
/// <summary>
/// Recursive splitting procedure
/// </summary>
/// <param name="parent">This is where root node goes</param>
/// <param name="depth"></param>
///
void SplitNode(KDNode parent)
{
// center of bounding box
KDBounds parentBounds = parent.bounds;
Vector3 parentBoundsSize = parentBounds.size;
// Find axis where bounds are largest
int splitAxis = 0;
float axisSize = parentBoundsSize.x;
if (axisSize < parentBoundsSize.y)
{
splitAxis = 1;
axisSize = parentBoundsSize.y;
}
if (axisSize < parentBoundsSize.z)
{
splitAxis = 2;
}
// Our axis min-max bounds
float boundsStart = parentBounds.min[splitAxis];
float boundsEnd = parentBounds.max[splitAxis];
// Calculate the spliting coords
float splitPivot = CalculatePivot(parent.start, parent.end, boundsStart, boundsEnd, splitAxis);
parent.partitionAxis = splitAxis;
parent.partitionCoordinate = splitPivot;
// 'Spliting' array to two subarrays
int splittingIndex = Partition(parent.start, parent.end, splitPivot, splitAxis);
// Negative / Left node
Vector3 negMax = parentBounds.max;
negMax[splitAxis] = splitPivot;
KDNode negNode = GetKDNode();
negNode.bounds = parentBounds;
negNode.bounds.max = negMax;
negNode.start = parent.start;
negNode.end = splittingIndex;
parent.negativeChild = negNode;
// Positive / Right node
Vector3 posMin = parentBounds.min;
posMin[splitAxis] = splitPivot;
KDNode posNode = GetKDNode();
posNode.bounds = parentBounds;
posNode.bounds.min = posMin;
posNode.start = splittingIndex;
posNode.end = parent.end;
parent.positiveChild = posNode;
// check if we are actually splitting it anything
// this if check enables duplicate coordinates, but makes construction a bit slower
#if KDTREE_DUPLICATES
if (negNode.Count != 0 && posNode.Count != 0)
{
#endif
// Constraint function deciding if split should be continued
if (ContinueSplit(negNode))
{
SplitNode(negNode);
}
if (ContinueSplit(posNode))
{
SplitNode(posNode);
}
#if KDTREE_DUPLICATES
}
#endif
}
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);
}
}
public KDQueryNode(KDNode node, Vector3 tempClosestPoint)
{
this.node = node;
this.tempClosestPoint = tempClosestPoint;
}
/// <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);
}
}
}
}
}
}
