https://5dok.org/document/6zk11p4q-drive-drive-discrete-motion-plans-smooth-drivable-trajectories.html