public static List <Vector2i> A_Star(Vector2 startPosition,
Vector2 goalPosition,
EHeuristicType heuristicType = EHeuristicType.OctileDistance)
{
_grid = Map.instance.navGrid;
// find the start
Vector2i?startCoord = _grid.GetCoordAt(startPosition);
Vector2i?goalCoord = _grid.GetCoordAt(goalPosition);
if (!startCoord.HasValue || !goalCoord.HasValue)
{
return(new List <Vector2i>());
}
NodeRecord start = _grid.GetCaseAt(startCoord.Value);
// verify if it's in the grid
if (start == null)
{
return(new List <Vector2i>());
}
// buffer this because it will be used for each estimation
_floatCoordGoal = GetFloatCoordFrom(goalPosition);
// prepare the record
Profiler.BeginSample("clear grid");
_grid.ClearRecords();
Profiler.EndSample();
PriorityQueue <NodeRecord, float> frontier = new PriorityQueue <NodeRecord, float>();
start.EstimatedTotalCost = EstimatePosition(startPosition, heuristicType);
EnqueueNodeRecord(start, frontier);
NodeRecord endNode = null;
while (frontier.Count > 0)
{
NodeRecord current = frontier.Dequeue().Value;
current.State = ENodeRecordState.Closed;
if (current.Coord == goalCoord)
{
endNode = current;
break;
}
Profiler.BeginSample("Get neighbour");
_neighbourList = _grid.GetNeighbour(current.Coord);
Profiler.EndSample();
for (int i = 0; i < _neighbourList.Count; i++)
{
NodeRecord currentNeighbour = _neighbourList[i].Node;
float newCost = current.CostSoFar + _neighbourList[i].ImmediateCost + currentNeighbour.NodeCost;
if (currentNeighbour.State == ENodeRecordState.Unvisited ||
currentNeighbour.CostSoFar > newCost)
{
Profiler.BeginSample("Estimate");
float estimateCost = EstimateCoord(currentNeighbour.Coord, heuristicType);
Profiler.EndSample();
currentNeighbour.CostSoFar = newCost;
currentNeighbour.EstimatedTotalCost = newCost + estimateCost;
EnqueueNodeRecord(currentNeighbour, frontier);
currentNeighbour.CameFrom = current;
}
}
}
return(ReconstructPath(endNode));
}
public static List <Vector2i> SmoothPath(List <Vector2i> path)
{
List <Vector2i> smoothPath = new List <Vector2i>();
if (path.Count == 0)
{
return(smoothPath);
}
// To keep the current path structure, we need
// to keep the start and the end nodes in the smoothed path
smoothPath.Add(path[0]);
NavGrid navGrid = Map.instance.navGrid;
// we run through each node of the previous path
for (int i = 0; i < path.Count - 1; i++)
{
int bestClearIndex = -1;
// And compare them with the next nodes
// (we don't need to check the immediate next node
// because, thank's to the grid structure, it's necessarily
// reachable)
for (int j = i + 2; j < path.Count; j++)
{
// if we can't find a clear line bewteen them, we stop the process
if (!navGrid.IsClearLine(path[i], path[j]))
{
break;
}
bestClearIndex = j;
}
// If it's true, this means that we found a shortcut, then we can
// directly add the found index
if (bestClearIndex > 0)
{
smoothPath.Add(path[bestClearIndex]);
// we want to restart at the last added node
// -1 : to count the for-incrementation
i = bestClearIndex - 1;
}
else // if (i < path.Count - 3)
{
// no shortcut found, we add the immediate next node
smoothPath.Add(path[i + 1]);
}
}
// The last node doesn't need to be add because it's necessarily added to the smoothed path.
// When i = path.count - 2, j = i + 2
// then j > path.count
// then bestclearindex still is -1
// then we add the i + 1 = path.count - 1 which is the last node
// [end of the demonstration]
return(smoothPath);
}
