/// <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;
}
