Calculate optimal path