/// <summary> /// Reconstructs the path when given the last Node /// </summary> /// <param name="target">Last Node of the path</param> /// <returns>Path in correct order, from first point to the last point</returns> Path MakePath(Vector3 source, Node target) { List <Node> nodes = new List <Node>(); while (target != null) { nodes.Add(target); target = target.PreviousNode; } //Reverse so that source is first and target is last nodes.Reverse(); if (MHUrhoApp.Instance.Config.PathFindingVisualization == Visualization.FinalPath) { VisualizePath(nodes); } if (nodes.Count == 1) { return(StartIsFinish(nodes[0], source)); } List <Waypoint> waypoints = new List <Waypoint>(); waypoints.Add(new Waypoint(new TempNode(source, Map), 0, MovementType.None)); for (int i = 1; i < nodes.Count; i++) { waypoints.AddRange(nodes[i].GetWaypoints(distCalc)); } switch (waypoints[1].MovementType) { case MovementType.None: throw new InvalidOperationException("Waypoint on a path cannot have a MovementType of None"); case MovementType.Teleport: //NOTHING break; default: //Default to linear movement Waypoint sourceWaypoint = waypoints[0]; Waypoint firstWaypoint = waypoints[1]; if (distCalc.GetTime(sourceWaypoint.Node, firstWaypoint.Node, MovementType.Linear, out float time)) { waypoints[1] = firstWaypoint.WithTimeToWaypointSet(time); } else { waypoints[1] = firstWaypoint.WithTimeToWaypointSet(distCalc.GetMinimalAproxTime(source, firstWaypoint.Position)); } break; } return(Path.CreateFrom(waypoints, Map)); }
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); } } } }