Added comparison functions and made pathfinder more robust.