private static float Estimate(Vector2 floatCoordFrom, EHeuristicType type)
{
switch (type)
{
case EHeuristicType.ManhattanDistance:
return(Heuristic.ManhattanEstimation(floatCoordFrom, _floatCoordGoal));
case EHeuristicType.OctileDistance:
return(Heuristic.OctileEstimation(floatCoordFrom, _floatCoordGoal));
default:
return(Heuristic.ManhattanEstimation(floatCoordFrom, _floatCoordGoal));
}
}
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));
}
private static float EstimateCoord(Vector2i from, EHeuristicType type)
{
return(Estimate(from, type));
}
private static float EstimatePosition(Vector2 from, EHeuristicType type)
{
Vector2 floatCoord = GetFloatCoordFrom(from);
return(Estimate(floatCoord, type));
}