/// <summary> /// Find a path using the AStar algorithm /// </summary> /// <param name="start"></param> /// <param name="end"></param> /// <param name="map"></param> /// <returns></returns> public static Position <short>[] FindPath(Position <short> start, Position <short> end, IMap map) { MiniHeap <Node> delNodes = new MiniHeap <Node>(); MiniHeap <Node> nodes = new MiniHeap <Node>(); nodes.Add(new Node { Position = start }, false); while (nodes.Count() > 0) { Node node = nodes.Pop(); if (node.Position.Equals(end)) { int i = node.Distance; Position <short>[] path = new Position <short> [i]; while (i >= 1) { path[--i] = node.Position; node = node.Parent; } return(path); } node.Closed = true; foreach (Position <short> n in PathfinderHelper.GetNeighbors(node.Position, map)) { if (n == null) { break; } Node neighbor = nodes.Get(no => no.Position.Equals(n)) ?? delNodes.Get(no => no.Position.Equals(n)); if (neighbor == null) { neighbor = new Node { Position = n, G = node.G + (n.X == node.Position.X || n.Y == node.Position.Y ? 1 : 1.4), H = Heuristics.Manhattan(Math.Abs(n.X - end.Y), Math.Abs(n.Y - end.Y)), Parent = node, Distance = node.Distance + 1 }; neighbor.F = neighbor.G + neighbor.H; nodes.Add(neighbor); } else if (!neighbor.Closed) { double gScore = node.G + (n.X == node.Position.X || n.Y == node.Position.Y ? 1 : 1.4); if (gScore > neighbor.G) { continue; } neighbor.G = gScore; neighbor.F = neighbor.G + neighbor.H; neighbor.Parent = node; neighbor.Distance = node.Distance + 1; } } delNodes.Add(node, false); } return(null); }
/// <summary> /// Returns an Array with the neighbors of the given position /// </summary> /// <param name="pos"></param> /// <param name="map"></param> /// <returns></returns> public Position <short>[] GetNeighbors(Position <short> pos, IMap map) => PathfinderHelper.GetNeighbors(pos, map);
public override void ProcessNode( PathfinderNode currentNode, PathfinderNode startNode, PathfinderNode targetNode, PathfindingHeap <PathfinderNode> openSet, HashSet <PathfinderNode> closedSet, Dictionary <Point, PathfinderNode> activeNodes, Grid grid, int maxPathLength ) { Vector3 currentLocation = grid.NodeToWorldCoord(currentNode.GetGridCoord()); List <PathfinderNode> neighbors = PathfinderHelper.GetNeighbors( currentNode, activeNodes, currentNodeCreator ); foreach (PathfinderNode neighbour in neighbors) { Vector3 neighbourLocation = grid.NodeToWorldCoord(neighbour.GetGridCoord()); if (!neighbour.IsWalkable() || closedSet.Contains(neighbour)) { continue; } CostResult newStrategyCost = currentCostStrategy.GetAdditionalCostAt( currentLocation, neighbourLocation ); //neighbour.UpdateAccumulatedStrategyCost(newStrategyCost); int newPhysicalGCost = currentNode.GetPhysicalGCost() + PathfinderHelper.GetDistance(currentNode, neighbour); int newStrategyGCost = currentNode.GetStrategyGCost() + neighbour.GetExtractor().Extract(newStrategyCost); int newMovementCostToNeighbour = newPhysicalGCost + newStrategyGCost; // bool smaller = newStrategyGCost < neighbour.GetStrategyGCost(); //if (smaller) // { //DrawGizmo.AddGizmo(Color.green, newStrategyGCost + " " + neighbour.GetStrategyGCost(), neighbour.GetLocation()); //} //Debug.Log(neighbour.GetGCost()); if (newMovementCostToNeighbour < neighbour.GetGCost() || !openSet.Contains(neighbour)) { //Debug.Log(neighbour.GetGCost()); //DrawGizmo.AddGizmo(Color.green, "" + currentNode.GetExtractor().Extract(newStrategyCost), neighbour.GetLocation()); neighbour.SetStrategyCost( newStrategyCost ); neighbour.SetStrategyGCost( newStrategyGCost ); neighbour.SetPhysicalGCost(newPhysicalGCost); neighbour.SetHCost(GetDistance(neighbour, targetNode)); neighbour.SetParent(currentNode); if (!openSet.Contains(neighbour) && neighbour.WithInRangeOfStart(maxPathLength) ) { openSet.Add(neighbour); PathfinderVisualizer.Visit(neighbour); /*DrawGizmo.AddGizmo(Color.grey, "", grid.NodeToWorldCoord( * neighbour.GetGridCoord()) * );*/ } else { openSet.UpdateItem(neighbour); } } else { } } }