Model Predictive Planning and Control Applied to Critical Traffic Situations