public Vector2[] Navigate(Vector2 start, Vector2 end)
{
var startTile = new WeightedTile()
{
Position = start
};
var closedList = new List <WeightedTile>();
var openList = new List <WeightedTile>()
{
startTile
};
while (openList.Any())
{
var quickestTile = openList.OrderBy(t => t.Weight).First();
if (quickestTile.Position == end) // We're done! Found the path.
{
return(FindPath(startTile, quickestTile));
}
openList.Remove(quickestTile);
closedList.Add(quickestTile);
foreach (var neighbor in _searchableDirections.Select(d => d.ToVector2())
.Select(d => quickestTile.Position + d)
.Where(d => IsTraversable(d)))
{
if (closedList.Any(c => c.Position == neighbor))
{
continue;
}
var movementCost = Mathf.RoundToInt(
Mathf2.DistanceTo(neighbor, startTile.Position)
+ quickestTile.MovementCost);
var existingNeighbor = openList.FirstOrDefault(o => o.Position == neighbor);
if (existingNeighbor == null)
{
openList.Add(new WeightedTile()
{
MovementCost = movementCost,
Heuristic = Mathf.RoundToInt(Mathf2.DistanceTo(neighbor, end)),
Parent = quickestTile,
Position = neighbor
});
}
else if (existingNeighbor.MovementCost > movementCost)
{
existingNeighbor.MovementCost = movementCost;
existingNeighbor.Parent = quickestTile;
}
}
}
return(new Vector2[0]);
}
