https://9lib.org/document/q05gmgvy-safe-nonlinear-trajectory-generation-parallel-autonomy-dynamic-vehicle.html