Beim überfliegen des Quellcodes ist mir aufgefallen, dass du sehr sorgfältig kommentierst, sollte ich mir auch mal (wieder?) angewöhnen.
Ansonsten aufpassen dass du dein Array nich zum überlaufen bekommst, besser schauen dass in der Pfad gestaltung das i nicht größer #anzahl_waypoints wird (bei dir 1000). Je nach Indexsetzung vllt auch #anzahl_waypoints - 1.
Es fiel mir nur auf, weil ich selbiges Problem schonmal hatte und man gerne andere vorm Fettnäpchen bewahrt
