private List<PathfindingNode> DoFindRoute(List<NodeConnection> connections, PathfindingNode startNode, PathfindingNode endNode)
{
System.Diagnostics.Debug.WriteLine("PathGraph DoFindRoute(connections, startNode, endNode)");
DateTime startTime = DateTime.Now;
PathStep destinationPoint = new PathStep() { Node = endNode, Cost = 0, Ancestor = null };
List<PathStep> visitedPoints = new List<PathStep>();
visitedPoints.Add(destinationPoint);
List<PathStep> pointsLeftToVisit = GetNewMoveableCoordinatesFromPoint(destinationPoint, visitedPoints);
List<PathStep> newPoints;
PathStep startPoint = null;
PathStep currentPoint = null;
while (pointsLeftToVisit.Count > 0 && startPoint == null)
{
currentPoint = pointsLeftToVisit[0];
if (currentPoint.Node == startNode)
{ startPoint = currentPoint; }
else
{
pointsLeftToVisit.RemoveAt(0);
visitedPoints.Add(currentPoint);
newPoints = GetNewMoveableCoordinatesFromPoint(currentPoint, visitedPoints);
foreach (PathStep point in newPoints)
{
pointsLeftToVisit.Add(point);
if (point.Node == startNode)
{
startPoint = point;
break;
}
}
}
}
if (startPoint == null)
{ return null; } // Then we could not find a path!
List<PathfindingNode> path = new List<PathfindingNode>();
PathStep pathStep = startPoint;
while (pathStep != null)
{
path.Add(pathStep.Node);
pathStep = pathStep.Ancestor;
}
DateTime endTime = DateTime.Now;
TimeSpan duration = endTime.Subtract(startTime);
System.Diagnostics.Debug.WriteLine("PathGraph DoFindRoute duration: "+duration.TotalMilliseconds+"ms");
return path;
}
private List<PathStep> GetNewMoveableCoordinatesFromPoint(PathStep parentPoint, List<PathStep> visitedPoints)
{
List<PathStep> moves = new List<PathStep>();
foreach (NodeConnection connection in _connections)
{
if (connection.ContainsNode(parentPoint.Node))
{
// Make sure we have not visited the OTHER node yet.
PathfindingNode otherNode = connection.GetOppositeNode(parentPoint.Node);
PathStep existingPathStep = null;
foreach (PathStep point in visitedPoints)
{
if (point.Node == otherNode)
{ existingPathStep = point; }
}
double totalCost = parentPoint.Cost + connection.Cost;
if (existingPathStep != null)
{
if (existingPathStep.Cost > totalCost)
{
existingPathStep.Cost = totalCost;
existingPathStep.Ancestor = parentPoint;
}
}
else
{ moves.Add(new PathStep() { Node = otherNode, Cost = totalCost, Ancestor = parentPoint }); }
}
}
return moves;
}
