Improved pathfinding (seems to be working and natural enough)