Вычислить заданные крены, углы наклона и рыскания с учетом начальных и заданных положений и начальных углов крена, наклона и рыскания

Question

У меня есть модель, которая движется в мире симуляции. Кадр операции правосторонний с X - вперед, Y - влево и Z - вверх.

Я знаю текущую позицию, P1 (x1, y1, z1) и текущие углы RPY, (R1, P1, Y1). У меня есть цель, P2 (x2, y2, z2). Я хотел бы переместить мою модель с P1 на P2. Я застрял в вычислении углов цели RPY. Я бы хотел, чтобы моя модель сначала развернулась в направлении от P1 к P2 и двигалась в этом направлении, чтобы достичь цели.

Как рассчитать целевые углы RPY, (R2, P2, Y2), с учетом вышеуказанной информации?

Answer 1

• Рассчитать вектор направления D = P2 - P1 = (x2 - x1, y2 - y1, z2 - z1)

• Шаг P2 = asin(Dz / |D|)

• Зевок Y2 = atan2(Dy, Dx)

(вместо этого можно использовать P2 = atan2(Dz, sqrt(Dx^2 + Dy^2)) для большей устойчивости около сферических полюсов)