public static List <Vector> SearchPath(Vector startVector, Vector goalVector)
{
cameFrom = new Dictionary <Point, Point>();
costSoFar = new Dictionary <Point, float>();
var frontier = new PriorityQueue <Point>();
var start = TryGetCorrectPoint((startVector / 32).GetPoint());
var goal = TryGetCorrectPoint((goalVector / 32).GetPoint());
if (mesh.walls.Contains(start) || mesh.walls.Contains(goal))
{
return(new List <Vector>());
}
frontier.Enqueue(start, 0);
cameFrom[start] = start;
costSoFar[start] = 0;
var current = frontier.Dequeue();
while (current != goal)
{
if (current == goal)
{
break;
}
foreach (var next in mesh.Neighbors(current))
{
var newCost = costSoFar[current] + mesh.Cost(current, next);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = (float)newCost;
var priority = newCost + Heuristic(next, goal);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
}
current = frontier.Dequeue();
}
return(GetCurrentPath(ReconstructPath(start, goal)));
}