Better fix for formation waltzing, revert rP24831.

rP24831 proved unsufficient to fix formation 'waltzing'. This is a better fix, which makes sure units actually try to reach their designated offset in the first place.
Further, it removes code that recalculated offsets un-necessarily, which led to an issue with "sloppy" formations such as open and closed orders.

Fixes #5997

Differential Revision: https://code.wildfiregames.com/D3543