public static Vector3Int[] GetShortestPath(MapUnit unit, GameTile start, GameTile end, bool findClosest = false)
{
if (!findClosest && end.isBlocked)
{
return(null);
}
HashSet <GameTile> closedSet = new HashSet <GameTile>();
HashSet <GameTile> openSet = new HashSet <GameTile>();
GameTile closestSquare = null;
double closestDistance = 9999;
int maxLoops = 500;
int loops = 0;
openSet.Add(start);
ResetGameTile();
start.gScore = 0;
start.fScore = start.Distance(end);
double bestRouteDist = start.Distance(end);
while (openSet.Count > 0)
{
var current = openSet.OrderBy(n => n.fScore).FirstOrDefault();
if (current == end)
{
return(GenerateWaypoints(start, current));
}
openSet.Remove(current);
closedSet.Add(current);
foreach (var neighbour in current.neighbours)
{
//var neighbourInfo = pair.Value;
//var neighbour = neighbourInfo.neighbour;
var distance = 1;// neighbourInfo.distance;
if (closedSet.Contains(neighbour))
{
continue;
}
if (!IsGameTileReachable(unit, neighbour))
{
continue;
}
var alternativeDistance = current.gScore + distance;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else if (alternativeDistance >= neighbour.gScore)
{
continue;
}
var estimatedDistance = neighbour.Distance(end);
neighbour.parent = current;
neighbour.gScore = alternativeDistance;
neighbour.fScore = alternativeDistance + estimatedDistance;
if (closestDistance > estimatedDistance)
{
closestSquare = neighbour;
closestDistance = estimatedDistance;
}
}
if (loops > maxLoops)
{
break;
}
loops += 1;
}
if (closestSquare == null)
{
return(null);
}
if (findClosest && closestSquare.Distance(end) < start.Distance(end))
{
return(GenerateWaypoints(start, closestSquare));
}
else
{
return(null);
}
//return null;
}
