private void BubbleDown(int index)
{
TItem element = items[index];
TPriority priority = priorities[index];
int child;
for (; index * 2 <= Count; index = child)
{
child = index * 2;
if (child != Count)
{
if (priorityMath.Compare(priorities[child], priorities[child + 1]) < 0)
{
child++;
}
}
if (priorityMath.Compare(priority, priorities[child]) < 0)
{
priorities[index] = priorities[child];
items[index] = items[child];
}
else
{
break;
}
}
priorities[index] = priority;
items[index] = element;
}
public bool Add(TKey[] point, TValue value)
{
var nodeToAdd = new KdTreeNode <TKey, TValue>(point, value);
if (root == null)
{
root = new KdTreeNode <TKey, TValue>(point, value);
}
else
{
int dimension = -1;
KdTreeNode <TKey, TValue> parent = root;
do
{
// Increment the dimension we're searching in
dimension = (dimension + 1) % dimensions;
// Does the node we're adding have the same hyperpoint as this node?
if (typeMath.AreEqual(point, parent.Point))
{
switch (AddDuplicateBehavior)
{
case AddDuplicateBehavior.Skip:
return(false);
case AddDuplicateBehavior.Error:
throw new DuplicateNodeError();
case AddDuplicateBehavior.Update:
parent.Value = value;
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], parent.Point[dimension]);
if (parent[compare] == null)
{
parent[compare] = nodeToAdd;
break;
}
else
{
parent = parent[compare];
}
}while (true);
}
Count++;
return(true);
}
public T[] GetClosestPoint(T[] toPoint, ITypeMath <T> math)
{
T[] closest = new T[toPoint.Length];
for (var dimension = 0; dimension < toPoint.Length; dimension++)
{
if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0)
{
closest[dimension] = minPoint[dimension];
}
else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0)
{
closest[dimension] = maxPoint[dimension];
}
else
{
// Point is within rectangle, at least on this dimension
closest[dimension] = toPoint[dimension];
}
}
return(closest);
}
public bool Add(TKey[] point, TValue value)
{
var nodeToAdd = new KdTreeNode <TKey, TValue>(point, value);
if (root == null)
{
root = new KdTreeNode <TKey, TValue>(point, value);
}
else
{
int dimension = -1;
KdTreeNode <TKey, TValue> parent = root;
do
{
// Increment the dimension we're searching in
dimension = (dimension + 1) % dimensions;
// Does the node we're adding have the same hyperpoint as this node?
if (typeMath.AreEqual(point, parent.Point))
{
return(false);
}
// Which side does this node sit under in relation to it's parent at this level?
int compare = typeMath.Compare(point[dimension], parent.Point[dimension]);
if (parent[compare] == null)
{
parent[compare] = nodeToAdd;
break;
}
else
{
parent = parent[compare];
}
}while (true);
}
Count++;
return(true);
}
private void ReorderItem(int index, int direction)
{
if ((direction != -1) && (direction != 1))
{
throw new ArgumentException("Invalid Direction");
}
var item = queue[index];
int nextIndex = index + direction;
while ((nextIndex >= 0) && (nextIndex < count))
{
var next = queue[nextIndex];
int compare = priorityMath.Compare(item.Priority, next.Priority);
// If we're moving up and our priority is higher than the next priority then swap
// Or if we're moving down and our priority is lower than the next priority then swap
if (
((direction == -1) && (compare > 0))
||
((direction == 1) && (compare < 0))
)
{
queue[index] = next;
queue[nextIndex] = item;
index += direction;
nextIndex += direction;
}
else
{
break;
}
}
}
public bool Add(TItem item, TDistance distance)
{
if (queue.Count >= maxCapacity)
{
// If the distance of this item is less than the distance of the last item
// in our neighbour list then pop that neighbour off and push this one on
// otherwise don't even bother adding this item
if (distanceMath.Compare(distance, queue.GetHighestPriority()) < 0)
{
queue.Dequeue();
queue.Enqueue(item, distance);
return(true);
}
else
{
return(false);
}
}
else
{
queue.Enqueue(item, distance);
return(true);
}
}
public int Compare(KdTreeNode <TKey, TValue> x, KdTreeNode <TKey, TValue> y)
{
return(typeComparer.Compare(x.Points[byDimension], y.Points[byDimension]));
}
/// <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);
}
