Fixes to movement algorithm