public WaypointSearchData(double distanceTraveled, double distanceToGoal, DTOWaypoint waypoint, WaypointSearchData parentMove, int depth) { DistanceTraveled = distanceTraveled; DistanceToGoal = distanceToGoal; Waypoint = waypoint; ParentMove = parentMove; Depth = depth; }
public WaypointSearchResult PlanPath(DTOWaypoint startNode, DTOWaypoint destinationNode) { Dictionary <DTOWaypoint, WaypointSearchData> openLocations = new Dictionary <DTOWaypoint, WaypointSearchData>(); Dictionary <DTOWaypoint, WaypointSearchData> closedLocations = new Dictionary <DTOWaypoint, WaypointSearchData>(); openLocations[startNode] = new WaypointSearchData(0.0, GetDistance(startNode, destinationNode), startNode, null, 0); // Don't move if already at destination if (startNode == destinationNode) { return(null); } // Maximum number of waypoints to look at in search int maxNumIterations = 3000; int numIterations = 0; // Loop until end is found while (true) { // Find lowest cost waypoint in openLocations DTOWaypoint currentNode = null; double lowestCost = double.PositiveInfinity; foreach (var w in openLocations.Keys) { if (openLocations[w].DistanceTraveled + openLocations[w].DistanceToGoal < lowestCost) { currentNode = w; lowestCost = openLocations[w].DistanceTraveled + openLocations[w].DistanceToGoal; } } // Something wrong happened -can't find the end if (currentNode == null) { return(null); } // Grab the details about the current waypoint WaypointSearchData currentNodeData = openLocations[currentNode]; // If the closest is also the destination or out of iterations if (currentNode == destinationNode || numIterations++ == maxNumIterations) { // Init result WaypointSearchResult result = new WaypointSearchResult(); // Found it on the first move if (currentNodeData.ParentMove == null) { return(null); } // Go back to the first move made while (currentNodeData != null) { result.AddStep(currentNodeData.Waypoint); currentNodeData = currentNodeData.ParentMove; } return(result); } // Transfer closest from open to closed list closedLocations[currentNode] = openLocations[currentNode]; openLocations.Remove(currentNode); // Expand all the moves foreach (var successorNode in currentNode.Paths.Select(p => _waypoints[p])) { // Check whether the node is already on closed if (closedLocations.ContainsKey(successorNode)) { // Don't deal with anything already on the closed list (don't want loops) continue; } // If it's not in the open list, add it if (!openLocations.ContainsKey(successorNode)) { openLocations[successorNode] = new WaypointSearchData( currentNodeData.DistanceTraveled + GetDistance(currentNode, successorNode), // The distance already traveled GetDistance(successorNode, destinationNode), // The approximate distance to the goal successorNode, // The node itself currentNodeData, // Parent data currentNodeData.Depth + 1); // The current depth } else { // It's already in the open list, but see if this new path is better WaypointSearchData oldPath = openLocations[successorNode]; // Replace it with the new one if (oldPath.DistanceTraveled + oldPath.DistanceToGoal > currentNodeData.DistanceTraveled + GetDistance(currentNode, successorNode)) { openLocations[successorNode] = new WaypointSearchData( currentNodeData.DistanceTraveled + GetDistance(currentNode, successorNode), // The distance already traveled GetDistance(successorNode, destinationNode), // The approximate distance to the goal successorNode, // The node itself currentNodeData, // Parent data currentNodeData.Depth + 1); // The current depth } } } } }