Moving according to WayPoints

Here is a class of Node that provides waypoint navigation features. Such nodes are given a set of destination points through the addPoint() method. They will then move towards each of these destinations, sequentially. The step parameter specifies how much distance is covered in each round (i.e. in each clock pulse).

import io.jbotsim.core.Node;
import io.jbotsim.core.Point;

import java.util.LinkedList;
import java.util.Queue;

public class WayPointNode extends Node{
    Queue<Point> waypoint = new LinkedList<Point>();
    double step = 1;

    public void addPoint(Point p){
        waypoint.add(p);
    }
    public void setStep(double step){
        this.step = step;
    }
    public void onClock() {
        if (! waypoint.isEmpty()) {
            Point next = waypoint.peek();
            setDirection(next);
            if (distance(next) > step)
            move(step);
            else {
                move(distance(next));
                waypoint.poll();
            }
        }
    }
}

Below is a possible main() method, in which such a node is created and given a squared itinerary. Here all the waypoints are given at once. They are queued and will be processed one by one. One could alternatively design a onArrival() event that the node triggers every time it reaches a destination.

public static void main(String[] args) {
    Topology tp = new Topology();
    WayPointNode node = new WayPointNode();
    tp.addNode(100, 100, node);
    node.addPoint(new Point(300, 100));
    node.addPoint(new Point(300, 300));
    node.addPoint(new Point(100, 300));
    node.addPoint(new Point(100, 100));
    new JViewer(tp);
    tp.start();
}