private void AddNodeToStringBuilder(KdTreeNavNode <TKey> node, StringBuilder sb, int depth, int maxCount)
{
if (maxCount <= 0)
{
return;
}
sb.AppendLine(node.ToString());
for (var side = -1; side <= 1; side += 2)
{
for (var index = 0; index <= depth; index++)
{
sb.Append("\t");
}
sb.Append(side == -1 ? "L " : "R ");
if (node[side] == null)
{
sb.AppendLine("");
}
else
{
AddNodeToStringBuilder(node[side], sb, depth + 1, maxCount--);
}
}
}
internal KdTreeNavNode <TKey> this[int compare]
{
get
{
if (compare <= 0)
{
return(LeftChild);
}
else
{
return(RightChild);
}
}
set
{
if (compare <= 0)
{
LeftChild = value;
}
else
{
RightChild = value;
}
}
}
public void Clear()
{
if (root != null)
{
RemoveChildNodes(root);
}
Count = 0;
root = null;
}
private void RemoveChildNodes(KdTreeNavNode <TKey> node)
{
for (var side = -1; side <= 1; side += 2)
{
if (node[side] != null)
{
RemoveChildNodes(node[side]);
node[side] = null;
}
}
}
public KdTreeNode <TKey, TValue> Remove(KdTreeNode <TKey, TValue> removedNode)//may be not working correct
{
// Is tree empty?
if (root == null)
{
return(null);
}
KdTreeNavNode <TKey> navNode = root;
KdTreeNavNode <TKey> parentNode = root;
var node = ConvertNavNode(root);
if (typeMath.AreEqual(removedNode.Points, node.Points))
{
root = null;
Count--;
ReaddChildNodes(navNode);
return(node);
}
int dimension = -1;
int compare = 0;
do
{
dimension = (dimension + 1) % dimensions;
if (isNode(dimension))
{
node = ConvertNavNode(navNode);//todo: may be optimized without using cast
if (typeMath.AreEqual(removedNode.Points, node.Points) &&
removedNode.Value.Equals(node.Value))
{
var nodeToRemove = node;
parentNode[compare] = null;
Count--;
ReaddChildNodes(nodeToRemove);
return(nodeToRemove);
}
}
compare = typeMath.Compare(removedNode.Points[dimension], navNode.Point);
if (navNode[compare] == null)
{
// Can't find node
return(null);
}
parentNode = navNode;
navNode = navNode[compare];
}while (navNode != null);
return(null);
}
private void ReaddChildNodes(KdTreeNavNode <TKey> removedNode)//todo: Maybe bug
{
if (removedNode.IsLeaf)
{
return;
}
// The folllowing code might seem a little redundant but we're using
// 2 queues so we can add the child nodes back in, in (more or less)
// the same order they were added in the first place
var nodesToReadd = new Queue <KdTreeNavNode <TKey> >();
var nodesToReaddQueue = new Queue <KdTreeNavNode <TKey> >();
if (removedNode.LeftChild != null)
{
nodesToReaddQueue.Enqueue(removedNode.LeftChild);
}
if (removedNode.RightChild != null)
{
nodesToReaddQueue.Enqueue(removedNode.RightChild);
}
while (nodesToReaddQueue.Count > 0)
{
var nodeToReadd = nodesToReaddQueue.Dequeue();
nodesToReadd.Enqueue(nodeToReadd);
for (int side = -1; side <= 1; side += 2)
{
if (nodeToReadd[side] != null)
{
nodesToReaddQueue.Enqueue(nodeToReadd[side]);
nodeToReadd[side] = null;
}
}
}
while (nodesToReadd.Count > 0)
{
var navNodeToReadd = nodesToReadd.Dequeue();
if (navNodeToReadd.GetType() == typeof(KdTreeNavNode <TKey>))
{
continue;
}
var nodeToReadd = (KdTreeNode <TKey, TValue>)navNodeToReadd;
Count--;
Add(nodeToReadd.Points, nodeToReadd.Value);
}
}
private void AddNodesToList(KdTreeNavNode <TKey> node, List <KdTreeNode <TKey, TValue> > nodes)
{
if (node == null)
{
return;
}
if (isNode(node))
{
nodes.Add((KdTreeNode <TKey, TValue>)node);
}
for (var side = -1; side <= 1; side += 2)
{
if (node[side] != null)
{
AddNodesToList(node[side], nodes);
node[side] = null;
}
}
}
private bool TryFindValueAt(TKey[] point, KdTreeNavNode <TKey> navParent, int dimension, out TValue value, List <Vector2> path)
{
do
{
dimension = (dimension + 1) % dimensions;
if (navParent == null)
{
value = default(TValue);
//Debug.Log(log);
return(false);
}
if (isNode(dimension))
{
var parent = ConvertNavNode(navParent);
path.Add((parent.Value as Cell).Position);
if (typeMath.AreEqual(point, parent.Points))
{
value = parent.Value;
return(true);
}
}
// Keep searching
int compare = typeMath.Compare(point[dimension], navParent.Point);
if (compare == 0)
{
if (TryFindValueAt(point, navParent[-1], dimension, out value, path))
{
return(true);
}
if (TryFindValueAt(point, navParent[1], dimension, out value, path))
{
return(true);
}
return(false);
}
navParent = navParent[compare];
}while (true);
}
/// <summary>
/// Add new node to tree
/// </summary>
/// <param name="point">Key</param>
/// <param name="value">Value</param>
/// <returns>Return <code>true</code> if node was added</returns>
public bool Add(TKey[] point, TValue value)
{
var nodeToAdd = new KdTreeNode <TKey, TValue>(point, value);
if (root == null)
{
root = nodeToAdd;
}
else
{
int dimension = -1;
var navParent = root;
do
{
dimension = (dimension + 1) % dimensions;
if (isNode(dimension))
{
var parent = (KdTreeNode <TKey, TValue>)navParent;
if (typeMath.AreEqual(point, parent.Points))
{
switch (AddDuplicateBehavior)
{
case AddDuplicateBehavior.Skip:
return(false);
case AddDuplicateBehavior.Continue:
break;
case AddDuplicateBehavior.Error:
throw new DuplicateNodeError();
case AddDuplicateBehavior.Update:
if (OnNodeUpdate != null)
{
if (OnNodeUpdate.Invoke(this, parent, nodeToAdd))
{
return(true);
}
}
else
{
parent.Value = value;
return(true);
}
break;
default:
// Should never happen
throw new Exception("Unexpected AddDuplicateBehavior");
}
}
}
// Which side does this node sit under in relation to it's parent at this level?
int compare = typeMath.Compare(point[dimension], navParent.Point);
if (navParent[compare] == null)
{
if (isNavNode(dimension + 1))
{
navParent[compare] = new KdTreeNavNode <TKey>(point[(dimension + 1) % dimensions]);
navParent = navParent[compare];
}
else
{
navParent[compare] = nodeToAdd;
break;
}
}
else
{
navParent = navParent[compare];
}
} while (true);
}
Count++;
return(true);
}
private bool TryFindValueAt(TKey[] point, onAdditionalCheckEqualsDelegate additionalCheckEquals, KdTreeNavNode <TKey> navParent, int dimension, out TValue value)
{
do
{
dimension = (dimension + 1) % dimensions;
if (navParent == null)
{
value = default(TValue);
return(false);
}
else if (isNode(dimension))
{
var parent = (KdTreeNode <TKey, TValue>)navParent;
if (typeMath.AreEqual(point, parent.Points))
{
if (additionalCheckEquals(parent, point))
{
value = parent.Value;
return(true);
}
else
{
if (TryFindValueAt(point, additionalCheckEquals, parent[-1], dimension,
out value))
{
return(true);
}
if (TryFindValueAt(point, additionalCheckEquals, parent[1], dimension,
out value))
{
return(true);
}
return(false);
}
}
}
// Keep searching
int compare = typeMath.Compare(point[dimension], navParent.Point);
navParent = navParent[compare];
}while (true);
}
private void AddNearestNeighbours(//todo: not working correct. Very slow
KdTreeNavNode <TKey> navNode,
TKey[] target,
int depth,
//TKey[] prevDistance,
Stack <TKey>[] prevDistances,
LinkedList <TValue> nearestNeighbours,
Stack <bool>[] wasLess)
{
if (navNode == null)
{
return;
}
int dimension = (depth) % dimensions;
if (isNode(dimension))
{
var node = ConvertNavNode(navNode);
var canAdd = true;
for (int dim = 0; dim < dimensions; dim++)
{
var distance = typeMath.DistanceBetweenPoints(node.Points[dim], target[dim]);
var prevDistance = prevDistances[dim].Peek();
var prevWasLess = wasLess[dim].Peek();
if (typeMath.Compare(distance, prevDistance) > 0)
{
if (prevWasLess)
{
for (int dim1 = 0; dim1 < dim; dim1++)
{
prevDistances[dim1].Pop();
wasLess[dim1].Pop();
}
return;
}
wasLess[dim].Push(true);
}
else
{
wasLess[dim].Push(false);
}
prevDistances[dim].Push(distance);
if (typeMath.Compare(
distance,
typeMath.Zero) > 0)
{
canAdd = false;
}
//break;
}
if (canAdd)
{
nearestNeighbours.AddFirst(node.Value);
}
AddNearestNeighbours(navNode.LeftChild,
target,
depth + 1,
prevDistances,
nearestNeighbours,
wasLess);
AddNearestNeighbours(navNode.RightChild,
target,
depth + 1,
prevDistances,
nearestNeighbours,
wasLess);
for (int dim = 0; dim < dimensions; dim++)
{
prevDistances[dim].Pop();
wasLess[dim].Pop();
}
}
else
{
AddNearestNeighbours(navNode.LeftChild,
target,
depth + 1,
prevDistances,
nearestNeighbours,
wasLess);
AddNearestNeighbours(navNode.RightChild,
target,
depth + 1,
prevDistances,
nearestNeighbours,
wasLess);
}
}
protected KdTreeNode <TKey, TValue> ConvertNavNode(KdTreeNavNode <TKey> node)
{
return((KdTreeNode <TKey, TValue>)node);
}
protected bool isNode(KdTreeNavNode <TKey> node)
{
return(node.GetType() == typeof(KdTreeNode <TKey, TValue>));
}
