Agent movement algorithm improved