https://1library.org/document/zxv1gd4y-generating-time-optimal-trajectory-predefined-d-waypoint-networks.html