Расчет разности вращения в полярных координатах

Question

У меня проблема с вычитанием двух вращений полярных координат. Проблема в том, что моя машина должна проехать 6 часов. Из того, что я заметил, вращение в направлении оси X равно 0, в направлении отрицательной оси X равно + -pi. (РЕДАКТИРОВАТЬ: ось X является вертикальной осью)

У меня есть текущее вращение машины "пси" и полярная координата положения цели. Я не могу вычесть их без возникновения этой проблемы.

Answer 1

Я решил это с помощью нормализации вращения автомобиля с помощью rem(u,2*pi) (не знаю, почему это не было с самого начала), затем также использовал rem(u,pi) после вычитания ТОКА из вращения ЦЕЛИ (чтобы оно не перескочило).