seekWaypoint.java

void seekWaypoint(Vector2 waypoint) {
        Vector2 pos = body.getPosition();
        float dist = waypoint.dst(pos);
        // you should set a threshold for arrival distance
        // that is good enough to be considered "arrived" for your purposes
        if (dist < arrivalThreshold) {
            if (waypoints.contains(waypoint)) {
                // if we are chasing a target, don't remove the waypoint upon reaching arrival distance
                waypoints.remove(0);
                waypoint = getCurrentWaypoint();
                if (waypoint == null) {
                    return; // no more waypoints
                }
            } else if (waypoint.equals(chasePoint)) {
                // just keep chasing!
            }
        }
        // accelerate towards the waypoint, since it is beyond arrival distance threshold

        // here is where we used the velocity correction method from above
        Vector2 correctiveSteerpoint = getVelocityCorrectionSteerpoint(waypoint);
        Vector2 destination = correctiveSteerpoint;
        // to avoid over correction, we have a minimum distance from target for correction to occur at
        // if we are under the minimum, just head towards the original waypoint
        if (correctiveSteerpoint.dst(pos) < minSteeringDistance) {
            destination = waypoint;
        }
        // we set the desired facing, so we now where to turn to face our destination
        setDesiredFacing(getAngleFromAtoB(pos, destination));
        // only accelerate if we are pointed at the destination
        if (getSteeringDifference() < maxSteeringErrorForAcceleration) {
            accelerate();
        }
    }