Cleaned up RoutingAlgorithm
More conveinent classes for Pathing

 - Route now deals with PathNodes
 - RouteFollower added to keep track of progress along a route
 - RouteFinder added to find a route given a graph, algorithm and optional heuristic.
Merge branch 'feature/pathing-ai-refactor' into develop

 - Adds RouteFollower and RouteFinder
 - Refactors pathing stuff a bit