Make sure the direction of the motion is preserved, unless the depth is
higher than the margin, which means the body needs depenetration in any
direction.
Also changed move_and_slide to avoid sliding on the first motion, in
order to avoid issues with unstable position on ground when jumping.
Co-authored-by: fabriceci <fabricecipolla@gmail.com>