/// <summary> /// Uses a cached path to try and find the shortest path faster /// </summary> /// <param name="startNode"></param> /// <param name="endNode"></param> /// <param name="cachedPath"></param> /// <returns></returns> private List<Node> DStarLite(Node startNode, Node endNode, List<Node> cachedPath) { Debug.Print("Using Cached Pathfinding" + counter++); int middleIndex = cachedPath.Count() / 2; List<Node> startPath; int startIndex = 1; if(startNode.HasLineOfSight(Obstacles, cachedPath.First())) { startPath = new List<Node>() { startNode }; while (startIndex + 1< middleIndex && cachedPath[startIndex + 1].HasLineOfSight(Obstacles, startNode)) startIndex++; } else { // First we get the first part of the path, we are going to connect this to the start node startIndex = middleIndex; while (cachedPath.First().Distance(cachedPath[startIndex]) > _cacheAcceptanceDistance) startIndex--; // Use A* to get the start of the path startPath = AStar(startNode, new Node(cachedPath[startIndex])); if (startPath == null) return null; } int endIndex = cachedPath.Count - 1; List<Node> endPath; if (endNode.HasLineOfSight(Obstacles, cachedPath.Last())) { endPath = new List<Node>() { endNode }; while (endIndex - 1 > middleIndex && cachedPath[endIndex - 1].HasLineOfSight(Obstacles, endNode)) endIndex--; } else { // Next we get the last part of the path endIndex = middleIndex; while (cachedPath.Last().Distance(cachedPath[endIndex]) > _cacheAcceptanceDistance) endIndex++; // Use A* to get the end of the path endPath = AStar(new Node(cachedPath[endIndex]), endNode); if (endPath == null) return null; } // Now we build the new path List<Node> path = startPath; for (int i = startIndex; i <= endIndex; i++) path.Add(cachedPath[i]); path.AddRange(endPath); return path; }
public List<Node> GetShortestLineOfSight(Point startPoint, Point endPoint) { if (startPoint == null || endPoint == null) return null; // We create and place the start a goal node into the graph Node startNode = new Node(startPoint); Node endNode = new Node(endPoint); // if the end node is in an obstacle, return null if (!endNode.Clear(Obstacles)) return null; // Make the start node connect ForceConnection(startNode); // Set the initial node values startNode.Value = 0; foreach (Node milestone in Milestones) milestone.Value = Double.MaxValue; startNode.SetConnectedLineOfSightValues(Obstacles, endNode); List<Node> path; if (startNode.HasLineOfSight(Obstacles, endNode)) { // We have line of sight so we have a path of the start and end node path = new List<Node>() { startNode, endNode }; } else // Get the shortest path path = endNode.ShortestPath(); // Remove the start and end connections startNode.RemoveConnections(); endNode.RemoveConnections(); // Return the path if there is one return (path != null && path.Contains(startNode)) ? SmoothPath(path) : path; }