Walking / following path does not work in all cases

I am implementing the path following the one described below here and in this video . The only difference is that I converted it instead of Scene2D

and Stage

.

It works great, except for this part: When the Y of the next point is less than the Y of the maxillary point, the sprite basically jumps from the previous point directly to the next.

Example: In the image below, the sprite jumps from point A directly to B and the same one from C to D.

Here is a complete class that implements Screen

.

import java.util.ArrayList;
import com.badlogic.gdx.Gdx;
import com.badlogic.gdx.InputMultiplexer;
import com.badlogic.gdx.Screen;
import com.badlogic.gdx.graphics.Color;
import com.badlogic.gdx.graphics.GL20;
import com.badlogic.gdx.graphics.Texture;
import com.badlogic.gdx.graphics.g2d.Batch;
import com.badlogic.gdx.graphics.g2d.Sprite;
import com.badlogic.gdx.graphics.g2d.SpriteBatch;
import com.badlogic.gdx.graphics.glutils.ShapeRenderer;
import com.badlogic.gdx.graphics.glutils.ShapeRenderer.ShapeType;
import com.badlogic.gdx.math.MathUtils;
import com.badlogic.gdx.math.Vector2;
import com.badlogic.gdx.scenes.scene2d.Actor;
import com.badlogic.gdx.scenes.scene2d.Stage;
import com.badlogic.gdx.utils.viewport.StretchViewport;

public class WayPointsTutorial implements Screen {

    private Stage stage;

    private SpriteBatch batch;

    private ShapeRenderer renderer;
    private Sprite sprite;

    private TestPlayer player;

    public static ArrayList<Vector2> pathPoints;

    @Override
    public void show() {

        stage = new Stage(new StretchViewport(800,1280));

        InputMultiplexer inputM = new InputMultiplexer();
        inputM.addProcessor(stage);
        Gdx.input.setInputProcessor(inputM);

        renderer = new ShapeRenderer();
        batch = new SpriteBatch();

        sprite = new Sprite(new Texture("data/bt_comecar.png"));
        sprite.setSize(50, 50);
        sprite.setOrigin(0, 0);

        pathPoints = new ArrayList<Vector2>();

        pathPoints.add(new Vector2(170,550));
        pathPoints.add(new Vector2(40,40));
        pathPoints.add(new Vector2(150,40));
        pathPoints.add(new Vector2(608,1000));
        pathPoints.add(new Vector2(400,208));

        player = new TestPlayer(sprite, pathPoints);
        player.setPosition(400, 640);

        stage.addActor(player);
    }

    @Override
    public void render(float delta) {
        Gdx.gl.glClear(GL20.GL_COLOR_BUFFER_BIT);

        //Update the stage
        stage.draw();
        stage.act(delta);

        if(Gdx.input.isTouched()){show();}

        stage.getCamera().update();
        batch.setProjectionMatrix(stage.getCamera().combined);
        renderer.setProjectionMatrix(batch.getProjectionMatrix());
        renderer.setTransformMatrix(batch.getTransformMatrix());

        renderer.setColor(Color.CYAN);
        renderer.begin(ShapeType.Line);
        renderer.line(new Vector2(player.getX(), player.getY()), player.getPath().get(player.getWaypoint()));
        renderer.end();

        Vector2 previous = player.getPath().get(0);
        for(Vector2 waypoint : player.getPath()) {
            renderer.setColor(Color.WHITE);
            renderer.begin(ShapeType.Line);
            renderer.line(previous, waypoint);
            renderer.end();

            renderer.begin(ShapeType.Filled);
            renderer.circle(waypoint.x, waypoint.y, 15);
            renderer.end();

            previous = waypoint;
        }
    }

    @Override
    public void resize(int width, int height) {}

    @Override
    public void dispose() {
        stage.dispose();
        renderer.dispose();
    }

    @Override
    public void hide() {dispose();}

    @Override
    public void pause() {}

    @Override
    public void resume() {}

    public class TestPlayer extends Actor {

        private Vector2 velocity = new Vector2();
        private float speed = 300, tolerance = 50;

        private ArrayList<Vector2> path;
        private int waypoint = 0;
        Sprite sprite;

        public TestPlayer(Sprite sprite, ArrayList<Vector2> pathPoints) {
            //super(sprite);
            this.path = pathPoints;
            this.sprite = sprite;
        }

        @Override
        public void draw(Batch batch, float parentAlpha) {
            update(Gdx.graphics.getDeltaTime());
            sprite.setPosition(this.getX(), this.getY());
            sprite.setRotation(this.getRotation());
            sprite.draw(batch);
        }

        public void update(float delta) {

            float angle = (float) Math.atan2(path.get(waypoint).y - getY(), path.get(waypoint).x - getX());
            velocity.set((float) Math.cos(angle) * speed, (float) Math.sin(angle) * speed);

            setPosition(getX() + velocity.x * delta, getY() + velocity.y * delta);
            setRotation(angle * MathUtils.radiansToDegrees);

            if(isWaypointReached(waypoint)) {
                setPosition(path.get(waypoint).x, path.get(waypoint).y);
                if(waypoint + 1 >= path.size()){
                    waypoint=0; 
                }else{
                    waypoint++;
                }
            }

        }

        public boolean isWaypointReached(int waypoint) {
            return path.get(waypoint).x - getX() <= speed / tolerance * Gdx.graphics.getDeltaTime() && path.get(waypoint).y - getY() <= speed / tolerance * Gdx.graphics.getDeltaTime();
        }

        public ArrayList<Vector2> getPath() {
            return path;
        }

        public int getWaypoint() {
            return waypoint;
        }
    }
}