Pathfinder tweaked, plus a testcase. Will test as soon as I get my debugger working.