void FindClosest(Point p)
{
float dist = xSize + ySize;
Vector2 n = Vector2.zero;
int node = 0;
for (int i = 0; i < nodes.Count; i++) {
if (useGridDistance) {
if (dist>(Mathf.Abs (nodes [i].point.x - p.xPos) + Mathf.Abs (nodes [i].point.y - p.yPos))) {
dist = Mathf.Abs (nodes [i].point.x - p.xPos) + Mathf.Abs (nodes [i].point.y - p.yPos);
n = nodes [i].point;
node =i;
}
} else {
if (dist > Vector2.Distance (nodes [i].point, p.Get ())) {
dist = Vector2.Distance (nodes [i].point, p.Get ());
n = nodes [i].point;
node=i;
}
}
}
p.SetNode (nodes[node]);
nodes [node].AddPoint (p.Get ());
}