private static Vector2 GetFloatCoordFrom(Vector2 worldPosition)
{
Vector2i coord = _grid.GetCoordAt(worldPosition).Value;
Vector2 casePosition = _grid.GetCasePosition(coord);
Vector2 delta = worldPosition - casePosition;
delta /= _grid.CaseSize;
return(coord + delta);
}
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));
}