Rework Pathfinder MakePathMap logic for limit path len
Merge branch 'pathfinder_rework'