Fixed and improved hierarchical pathfinding.
Added hierarchical pathfinding test.