void Reset() { foreach (Node node in touchedNodes) { node.Reset(); } touchedNodes.Clear(); priorityQueue.Clear(); targetNode = null; distCalc = null; }
Node FindPathInNodes(Vector3 source, INode target, INodeDistCalculator nodeDistCalculator) { Node startNode = GetTileNode(source).GetClosestNode(source); distCalc = nodeDistCalculator as NodeDistCalculator; if (distCalc == null) { throw new ArgumentException("Cannot calculate node distances with calculator for a different algorithm", nameof(nodeDistCalculator)); } targetNode = target as Node; if (targetNode == null) { throw new ArgumentException("Target node was not created by this pathfinding algorithm", nameof(target)); } // Enque the starting node priorityQueue.Enqueue(startNode, 0); // Add the starting node to touched nodes, so it does not get enqued again touchedNodes.Add(startNode); double minDistToTarget = Vector3.Distance(startNode.Position, targetNode.Position); //Main loop while (priorityQueue.Count != 0) { Node currentNode = priorityQueue.Dequeue(); //If we hit the target, finish and return the sourceNode if (currentNode == targetNode) { if (MHUrhoApp.Instance.Config.PathFindingVisualization == Visualization.TouchedNodes) { VisualizeTouchedNodes(targetNode.Time); } return(currentNode); } //If not finished, add untouched neighbours to the queue and touched nodes currentNode.ProcessNeighbours(currentNode, priorityQueue, touchedNodes, targetNode, distCalc, ref minDistToTarget); } if (MHUrhoApp.Instance.Config.PathFindingVisualization == Visualization.TouchedNodes) { VisualizeTouchedNodes(targetNode.Time); } //Did not find path return(null); }
public override void ProcessNeighbours(Node source, FastPriorityQueue <Node> priorityQueue, List <Node> touchedNodes, Node targetNode, NodeDistCalculator distCalc, ref double minDistToTarget) { State = NodeState.Closed; foreach (var neighbour in outgoingEdges) { ProcessNeighbour(neighbour.Key, priorityQueue, touchedNodes, targetNode, distCalc, neighbour.Value, ref minDistToTarget); } }
public override IEnumerable <Waypoint> GetWaypoints(NodeDistCalculator nodeDist) { if (PreviousNode.NodeType == NodeType.Tile && PreviousNode.GetMovementTypeToNeighbour(this) == MovementType.Linear) { var borderNode = new TempNode(GetEdgePosition((ITileNode)PreviousNode), Map); float totalTime = Time - PreviousNode.Time; if (!nodeDist.GetTime(PreviousNode, borderNode, MovementType.Linear, out float firstTime)) { throw new ArgumentException($"Wrong {nameof(nodeDist)} implementation, does not give the same result on path building."); } return(new[] { new Waypoint(borderNode, firstTime, MovementType.Linear), new Waypoint(this, totalTime - firstTime, MovementType.Linear) }); } else { return(new[] { new Waypoint(this, Time - PreviousNode.Time, PreviousNode.GetMovementTypeToNeighbour(this)) }); } }
/// <summary> /// Returns waypoints to get from <see cref="previousNode"/> to this node /// </summary> /// <returns>Returns waypoints to get from <see cref="previousNode"/> to this node</returns> public abstract IEnumerable <Waypoint> GetWaypoints(NodeDistCalculator nodeDist);
public abstract void ProcessNeighbours(Node source, FastPriorityQueue <Node> priorityQueue, List <Node> touchedNodes, Node targetNode, NodeDistCalculator distCalc, ref double minDistToTarget);
protected void ProcessNeighbour(Node neighbour, FastPriorityQueue <Node> priorityQueue, List <Node> touchedNodes, Node targetNode, NodeDistCalculator distCalc, MovementType movementType, ref double minDistToTarget) { if (neighbour.State == NodeState.Closed) { //Already closed, either not passable or the best path there can be found return; } double distance = Vector3.Distance(neighbour.Position, targetNode.Position); //If the neighbor is too far out of the way from the closest path we found yet if (distance > minDistToTarget + AStar.Cutoff) { if (neighbour.State == NodeState.Untouched) { touchedNodes.Add(neighbour); } neighbour.State = NodeState.Closed; return; } else if (distance < minDistToTarget) { minDistToTarget = distance; } //If Unit can pass to target node from source node if (distCalc.GetTime(this, neighbour, movementType, out float timeToTarget)) { if (neighbour.State == NodeState.Untouched) { // Compute the heuristic for the new node float heuristic = distCalc.GetMinimalAproxTime(neighbour.Position.XZ(), targetNode.Position.XZ()); neighbour.State = NodeState.Opened; neighbour.Heuristic = heuristic; neighbour.PreviousNode = this; neighbour.Time = Time + timeToTarget; //Unit can pass through this node, enqueue it priorityQueue.Enqueue(neighbour, neighbour.Value); touchedNodes.Add(neighbour); } else if (neighbour.State == NodeState.Opened) { float newTime = Time + timeToTarget; //if it is closer through the current sourceNode if (newTime < neighbour.Time) { neighbour.Time = newTime; neighbour.PreviousNode = this; priorityQueue.UpdatePriority(neighbour, neighbour.Value); } } } }
public override IEnumerable <Waypoint> GetWaypoints(NodeDistCalculator distCalc) { return(new[] { new Waypoint(this, Time - PreviousNode.Time, PreviousNode.GetMovementTypeToNeighbour(this)) }); }