private IDirectedPath GetPath() { VectorGoal = Target.Coords.Canon - Source.Coords.Canon; OpenSet = new HashSet <HexCoords>(); Queue = new DictionaryPriorityQueue <int, IDirectedPath>(); TraceFindPathDetailInit(Source.Coords, Target.Coords); Queue.Enqueue(0, new DirectedPath(Target)); HexKeyValuePair <int, IDirectedPath> item; while (Queue.TryDequeue(out item)) { var path = item.Value; var step = path.PathStep.Hex; OpenSet.Add(step.Coords); if (ClosedSet.Contains(step.Coords)) { continue; } TraceFindPathDequeue("Rev", step.Coords, path.TotalCost, path.HexsideExit, item.Key >> 16, (int)((int)(item.Key & 0xFFFFu) - 0x7FFF)); if (step.Equals(Source)) { return(path); } ClosedSet.Add(step.Coords); foreach (var neighbour in Board.GetNeighbourHexes(step)) { ExpandNeighbour(path, neighbour); } } return(null); }
internal void FillLandmark(IBoard <IHex> board) { if (board != null) { backingStore = new BoardStorage <short> .BlockedBoardStorage32x32(board.MapSizeHexes, c => - 1); var start = board[Coords]; var queue = DictionaryPriorityQueue <int, IHex> .NewQueue(); TraceFlags.FindPathDetail.Trace(true, "Find distances from {0}", start.Coords); queue.Enqueue(0, start); HexKeyValuePair <int, IHex> item; while (queue.TryDequeue(out item)) { var here = item.Value; var key = item.Key; if (backingStore[here.Coords] > 0) { continue; } TraceFlags.FindPathDetail.Trace("Dequeue Path at {0} w/ cost={1,4}.", here, key); backingStore[here.Coords] = (short)key; foreach (var there in here.GetAllNeighbours().Where(n => n != null && n.Hex.IsOnboard())) { var cost = board.DirectedStepCost(here, there.HexsideEntry); if (cost > 0 && backingStore[there.Hex.Coords] == -1) { TraceFlags.FindPathDetail.Trace(" Enqueue {0}: {1,4}", there.Hex.Coords, cost); queue.Enqueue(key + cost, there.Hex); } } } } }
/// <summary>Returns an <c>IPath</c> for the optimal path from coordinates <c>start</c> to <c>goal</c>.</summary> /// <param name="start">Coordinates for the <c>last</c> step on the desired path.</param> /// <param name="goal">Coordinates for the <c>first</c> step on the desired path.</param> /// <param name="stepCost">Cost to extend path by hex at <c>coords</c> from hex at direction <c>hexside</c>.</param> /// <param name="heuristic">Returns a monotonic (ie locally admissible) cost estimate from a range value.</param> /// <returns></returns> /// ///<remarks>Note that <c>heuristic</c> <b>must</b> be monotonic in order for the algorithm to perform properly.</remarks> #pragma warning disable 1658, 1584 /// <seealso cref="http://www.cs.trincoll.edu/~ram/cpsc352/notes/astar.html"/> #pragma warning restore 1658, 1584 public static IDirectedPath FindDirectedPathFwd( IHex start, IHex goal, Func <IHex, Hexside, int> stepCost, Func <int, int> heuristic ) { if (start == null) { throw new ArgumentNullException("start"); } if (goal == null) { throw new ArgumentNullException("goal"); } if (stepCost == null) { throw new ArgumentNullException("stepCost"); } var vectorGoal = goal.Coords.Canon - start.Coords.Canon; var closed = new HashSet <HexCoords>(); var open = new HashSet <HexCoords>(); var queue = new DictionaryPriorityQueue <int, DirectedPath>(); TraceFlags.FindPathDetail.Trace(true, "Find path from {0} to {1}; vectorGoal = {2}", start.Coords, goal.Coords, vectorGoal); queue.Enqueue(0, new DirectedPath(goal)); HexKeyValuePair <int, DirectedPath> item; while (queue.TryDequeue(out item)) { var path = item.Value; open.Add(path.PathStep.Hex.Coords); if (closed.Contains(path.PathStep.Hex.Coords)) { continue; } TraceFlags.FindPathDequeue.Trace( "Dequeue Path at {0} w/ cost={1,4} at {2}; estimate={3,4}:{4,4}.", path.PathStep.Hex.Coords, path.TotalCost, path.HexsideExit, item.Key >> 16, (int)((int)(item.Key & 0xFFFFu) - 0x7FFF)); if (path.PathStep.Hex.Equals(start)) { TraceFlags.FindPathDequeue.Trace("Closed: {0,7}", closed.Count); return(path); } closed.Add(path.PathStep.Hex.Coords); foreach (var neighbour in path.PathStep.Hex.GetNeighbourHexes() ) { if (!open.Contains(neighbour.Hex.Coords)) { var cost = stepCost(neighbour.Hex, neighbour.HexsideExit); if (cost > 0) { var newPath = path.AddStep(neighbour, cost); var key = Estimate(heuristic, vectorGoal, start.Coords, neighbour.Hex.Coords, newPath.TotalCost); TraceFlags.FindPathEnqueue.Trace(" Enqueue {0}: estimate={1,4}:{2,3}", neighbour.Hex.Coords, key >> 16, key & 0xFFFFu); queue.Enqueue(key, newPath); } } } } return(null); }
/// <summary>Returns an <c>IPath</c> for the optimal path from coordinates <c>start</c> to <c>goal</c>.</summary> /// <param name="start">Coordinates for the <c>last</c> step on the desired path.</param> /// <param name="goal">Coordinates for the <c>first</c> step on the desired path.</param> /// <param name="stepCost">Cost to extend path by hex at <c>coords</c> from hex at direction <c>hexside</c>.</param> /// <param name="heuristic">Returns a cost estimate from a range value.</param> /// <param name="isOnboard">Returns whether the coordinates specified are "on board".</param> public static IPath FindPath( HexCoords start, HexCoords goal, Func <HexCoords, Hexside, int> stepCost, Func <int, int> heuristic, Func <HexCoords, bool> isOnboard ) { if (stepCost == null) { throw new ArgumentNullException("stepCost"); } if (heuristic == null) { throw new ArgumentNullException("heuristic"); } if (isOnboard == null) { throw new ArgumentNullException("isOnboard"); } var vectorGoal = goal.Canon - start.Canon; var closed = new HashSet <HexCoords>(); var open = new HashSet <HexCoords>(); var queue = DictionaryPriorityQueue <int, Path> .NewQueue(); TraceFlags.FindPathDetail.Trace(true, "Find path from {0} to {1}; vectorGoal = {0}", start.Canon, goal.Canon, vectorGoal); queue.Enqueue(0, new Path(start)); HexKeyValuePair <int, Path> item; while (queue.TryDequeue(out item)) { var path = item.Value; if (closed.Contains(path.StepCoords)) { continue; } open.Add(item.Value.StepCoords); TraceFlags.FindPathDetail.Trace( "Dequeue Path at {0} w/ cost={1,4} at {2}; estimate={3,4}:{4,4}.", item.Value.StepCoords, item.Value.TotalCost, item.Value.HexsideExit, item.Key >> 16, item.Key & 0xFFFF); if (path.StepCoords.Equals(goal)) { return(path); } closed.Add(path.StepCoords); foreach (var neighbour in path.StepCoords.GetNeighbours() .Where(n => isOnboard(n.Coords)) ) { var cost = stepCost(path.StepCoords, neighbour.Hexside); if (cost > 0) { var newPath = path.AddStep(neighbour, (ushort)cost); var estimate = Estimate(heuristic, vectorGoal, goal, newPath.StepCoords, newPath.TotalCost); TraceFlags.FindPathDetail.Trace(" Enqueue {0}: {1,4}:{2,3}", neighbour.Coords, estimate >> 16, estimate & 0xFFFF); queue.Enqueue(estimate, newPath); } } } return(null); }