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;
}
