//Add a new position into the tree
public void AddPosition(Vector2 pos)
{
if(root == null)
{
root = new node(pos);
count++;
return;
}
//Find where to put the new node
node current = root;
node next;
if (pos.x > root.Position.x)
next = root.Right;
else
next = root.Left;
int dimension = 0;
while(next != null)
{
//Cycle the dimension checked
if (dimension == 0)
dimension = 1;
else
dimension = 0;
//Proceed to the next node
current = next;
if (dimension == 0)
{
if(pos.x > current.Position.x)
{
next = current.Right;
}
else
{
next = current.Left;
}
}
else
{
if (pos.y > current.Position.y)
{
next = current.Right;
}
else
{
next = current.Left;
}
}
}
//Add the new node
if (dimension == 0)
{
if (pos.x > current.Position.x)
{
current.Right = new node(pos);
}
else
{
current.Left = new node(pos);
}
}
else
{
if (pos.y > current.Position.y)
{
current.Right = new node(pos);
}
else
{
current.Left = new node(pos);
}
}
count++;
}
void Remove(Vector2 pos)
{
//Find the one to remove
node parent = root;
node current = root;
int dimension = 0;
while(current.Position != pos)
{
parent = current;
if (dimension == 0)
{
if (pos.x > current.Position.x)
{
current = current.Right;
}
else
{
current = current.Left;
}
}
else
{
if (pos.y > current.Position.y)
{
current = current.Right;
}
else
{
current = current.Left;
}
}
if (dimension == 0)
dimension = 1;
else
dimension = 0;
}
//Create a list of all of its children
List<Vector2> children = getChildren(current);
//Remove all of its children
current.Right = null;
current.Left = null;
//Delete the node
if (parent.Left != null && parent.Left.Position == current.Position)
parent.Left = null;
else if (parent.Right != null && parent.Right.Position == current.Position)
parent.Right = null;
else if (current.Position == root.Position)
root = null;
current = null;
//Add the list of children back in
foreach(Vector2 v in children)
{
count--;
AddPosition(v);
}
count--;
}
List<Vector2> getChildren(node parent)
{
List<Vector2> children = new List<Vector2>();
List<node> closed = new List<node>();
List<node> open = new List<node>();
if (parent.Left != null)
open.Add(parent.Left);
if (parent.Right != null)
open.Add(parent.Right);
while(open.Count != 0)
{
for(int i = open.Count-1; i >= 0; i--)
{
if (open[i].Left != null)
open.Add(open[i].Left);
if (open[i].Right != null)
open.Add(open[i].Right);
closed.Add(open[i]);
open.Remove(open[i]);
}
}
foreach (node n in closed)
{
children.Add(n.Position);
}
return children;
}
void PrintTree(node n)
{
Console.WriteLine("Node = " + n.Position.x + ", " + n.Position.y);
if (n.Left != null)
{
Console.WriteLine("Left = " + n.Left.Position.x + ", " + n.Left.Position.y);
}
if (n.Right != null)
{
Console.WriteLine("Right = " + n.Right.Position.x + ", " + n.Right.Position.y);
}
if (n.Left != null)
{
PrintTree(n.Left);
}
if (n.Right != null)
{
PrintTree(n.Right);
}
}
public Vector2 GetClosestPoint(Vector2 pos, Vector2 currentBest, node current, int dimension)
{
//recurse down the tree to a leaf node, save that leaf as the current best
if (dimension == 0)
{
if (pos.x > current.Position.x)
{
if (current.Right != null)
currentBest = GetClosestPoint(pos, currentBest, current.Right, 1);
else
return current.Position;
}
else
{
if (current.Left != null)
currentBest = GetClosestPoint(pos, currentBest, current.Left, 1);
else
return current.Position;
}
}
else
{
if (pos.y > current.Position.y)
{
if (current.Right != null)
currentBest = GetClosestPoint(pos, currentBest, current.Right, 1);
else
return current.Position;
}
else
{
if (current.Left != null)
currentBest = GetClosestPoint(pos, currentBest, current.Left, 1);
else
return current.Position;
}
}
//While recursing back up...
//check if the current node is closer than the current best
if (Distance(pos, currentBest) > Distance(pos, current.Position))
currentBest = current.Position;
//See if there are any points on the other path which may be closer
if(dimension == 0)
{
if(Math.Abs(pos.x - current.Position.x) < Distance(pos, currentBest))
{
Vector2 potentialBest = new Vector2();
if (pos.x > current.Position.x)
{
if(current.Left != null)
potentialBest = GetClosestPoint(pos, currentBest, current.Left, 1);
}
else
{
if(current.Right != null)
potentialBest = GetClosestPoint(pos, currentBest, current.Right, 1);
}
if (Distance(potentialBest, pos) < Distance(currentBest, pos))
currentBest = potentialBest;
}
}
else
{
if (Math.Abs(pos.y - current.Position.y) < Distance(pos, currentBest))
{
Vector2 potentialBest = new Vector2();
if (pos.x > current.Position.y)
{
if (current.Left != null)
potentialBest = GetClosestPoint(pos, currentBest, current.Left, 0);
}
else
{
if (current.Right != null)
potentialBest = GetClosestPoint(pos, currentBest, current.Right, 0);
}
if (Distance(potentialBest, pos) < Distance(currentBest, pos))
currentBest = potentialBest;
}
}
return currentBest;
}
