improved pathfinding v2

improved pathfinding v2