public Waypoint getNextWayPoint(int index) { Waypoint nextWayPoint = null; if (motionPath.isCycle()) { // if path is cyclic, the successor of the last WP will be the first WP nextWayPoint = waypointList.get((index + 1) % waypointList.size()); } else if (motionPath.getNbWayPoints() > index + 1) { // if not cyclic, only successors for way points 0 .. n-1 exist nextWayPoint = waypointList.get(index + 1); } return nextWayPoint; }