public static List <Coordinate3D> FindShortestPath(IWeightedGraph graph, Coordinate3D start, Coordinate3D goal, MovementType unitMovementType) { var costSoFar = PoolProvider.CoordinateIntDictionaryPool.Get(); var cameFrom = PoolProvider.CoordinateDictionaryPool.Get(); var frontier = PoolProvider.CoordinateQueuePool.Get(); var path = graph.GetListFromPool(); frontier.Enqueue(start, 0); cameFrom[start] = start; costSoFar[start] = 0; while (frontier.Count > 0) { var current = frontier.Dequeue(); if (current.Equals(goal)) { break; } var neighbors = graph.GetPassableNeighbors(current); foreach (var next in neighbors) { var newCost = costSoFar[current] + graph.GetMovementCostForTheType(next, unitMovementType); if (costSoFar.ContainsKey(next) && newCost >= costSoFar[next]) { continue; } costSoFar[next] = newCost; var priority = newCost + Heuristic(next, goal); frontier.Enqueue(next, priority); cameFrom[next] = current; } graph.ReturnListToPool(neighbors); } var pathWasFound = cameFrom.ContainsKey(goal); if (!pathWasFound) { return(path); } // Reconstructing path var curr = goal; while (!curr.Equals(start)) { path.Add(curr); curr = cameFrom[curr]; } // Reverse it to start at actual start point path.Reverse(); PoolProvider.CoordinateIntDictionaryPool.Return(costSoFar); PoolProvider.CoordinateDictionaryPool.Return(cameFrom); PoolProvider.CoordinateQueuePool.Return(frontier); return(path); }
public int GetMovementTypeId(MovementType movementType) { return(movementType.GetId()); }
public List <Coordinate2D> GetShortestPath(Coordinate2D start, Coordinate2D goal, MovementType unitMovementType) { var path = GetShortestPath(start.To3D(), goal.To3D(), unitMovementType); var result = PoolProvider.OffsetCoordinateListPool.Get(); Coordinate3D.To2D( path, start.OffsetType, result ); ReturnListToPool(path); return(result); }
/// <summary> /// This methods gets movement costs to the coordinate for the movement type in range = 1. /// Used internally by path finding and range finding. /// </summary> /// <param name="coordinate"></param> /// <param name="unitMovementType"></param> /// <returns></returns> /// <exception cref="InvalidOperationException"></exception> public int GetMovementCostForTheType(Coordinate3D coordinate, MovementType unitMovementType) { var cellState = GetCellState(coordinate); return(MovementTypes.GetMovementCost(unitMovementType, cellState.TerrainType)); }
/// <summary> /// Finds shortest path from the start to the goal. Requires pawn's movement type to operate /// </summary> /// <param name="start"></param> /// <param name="goal"></param> /// <param name="unitMovementType"></param> /// <returns></returns> public List <Coordinate3D> GetShortestPath(Coordinate3D start, Coordinate3D goal, MovementType unitMovementType) { return(AStarSearch.FindShortestPath(this, start, goal, unitMovementType)); }