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)); }