Fixed navigation bug in pathfinding algorithm
Merge pull request #1 from michaeldejong/fixing-navigation

Fixed navigation bug in pathfinding algorithm