public static Vector3[] FindRoute(Vector3 s, Vector3 e)
{
PriorityQueue q = new PriorityQueue();
Dictionary <Square, bool> dic = new Dictionary <Square, bool>();
PathElement start = new PathElement(PathGrid.FromReal(s.x), PathGrid.FromReal(s.z), null);
PathElement end = new PathElement(PathGrid.FromReal(e.x), PathGrid.FromReal(e.z), null);
q.Enqueue(new PriorityElement(DistFromEnd(start, end), start));
//Debug.Log("Calculating for " + start.x + ", " + start.y + " :: " + end.x + ", " + end.y);
int tick = 0;
while (!q.IsEmpty() && tick < 100000)
{
tick++;
PathElement p = (PathElement)q.Dequeue().stuff;
float dist = DistFromEnd(p, end);
if (dist < 1.0f)
{
PathElement cur = p;
int size = 1;
while (cur != start)
{
//Debug.Log(cur.x + " " + cur.y);
cur = cur.prev;
size++;
}
cur = p;
Vector3[] route = new Vector3[size - 1];
while (cur != start)
{
route[size - 2] = PathGrid.ToReal(cur.x, cur.y);
cur = cur.prev;
size--;
}
//SDebug.Log("START");
return(route);
}
else
{
AddOptions(q, p, end, dic);
}
}
Debug.Log("Cannot find route!");
Vector3[] route2 = new Vector3[1];
route2[0] = Vector3.zero;
return(route2);
}
