Improved pathing algorithms