https://5dok.net/document/qvlxvn41-nonlinear-model-predictive-control-distributed-motion-planning-intersections.html