Avoid pointless calls to FindSafePositionOnPath

Avoid pointless calls to FindSafePositionOnPath

Calling CYapfReserveTrack::FindSafePositionOnPath only makes sense
if you intend to later call CYapfReserveTrack::TryReservePath to
reserve the path up to the position found; otherwise, it is just
wasting cycles. Even if the path has to be walked back to the
starting point, it is better to do it manually without checking
each intervening node for a safe position. Also add some assertions
for conditions involving FindSafePositionOnPath that should always
hold.