Added second element and the line from current goal to next waypoint