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 jbotsim.Node;

import java.awt.geom.Point2D;
import java.util.LinkedList;
import java.util.Queue;

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

    public void addPoint(Point2D p){
        waypoint.add(p);
    }
    public void setStep(double step){
        this.step = step;
    }
    public void onClock() {
        if (! waypoint.isEmpty()) {
            Point2D 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(false);
    WayPointNode node = new WayPointNode();
    tp.addNode(100, 100, node);
    node.addPoint(new Point2D.Double(300, 100));
    node.addPoint(new Point2D.Double(300, 300));
    node.addPoint(new Point2D.Double(100, 300));
    node.addPoint(new Point2D.Double(100, 100));
    new JViewer(tp);
    tp.start();
}    

Note that the topology is created here in a stopped state (false argument) and only started at the end of the initialization. The reason is that nodes are added by program and their execution must not interfere.