Fixed out of bounds exceptions on the pathfinder.