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