Получение углов наклона плоскости из кватерниона

[Devoter]

Доброго времени суток, уважаемые формучане.

Столкнулся со следующей задачей: имеется датчик абсолютной ориентации, который умеет возвращать готовое значение в углах Эйлера $(Roll, Pitch, Head)$ или в виде кватерниона $(W, X, Y, Z)$. С подобной задачей ранее сталкиваться не приходилось, либо матан успешно выветрился за прошедшие годы. Собственно, есть условно плоская прямоугольная поверхность, а требуется получить углы наклона. Пошерстив интернет, пришел к следующему решению:

Отложим на осях $OX$ ($\vec{a}$) и $OY$ ($\vec{b}$) единичные векторы, то бишь с координатами $(1, 0, 0)$ и $(0, 1, 0)$ соответственно. Теперь, при помощи кватерниона вращаем эти векторы, чтоб получить координаты точек концов векторов после поворота, но в исходной системе координат. Нашел вот такое решение:

На примере $OX$. Сперва получаем промежуточный вектор.
$\vec{t_{a}} = 2 \cdot \begin{pmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} \times \begin{bmatrix} X_{a} \\ Y_{a} \\ Z_{a} \end{bmatrix} \end{pmatrix}$

Затем вычисляем результирующий ветор ($\vec{a^\prime}$)
$ \vec{a^\prime} = \begin{bmatrix} X_{a} \\ Y_{a} \\ Z_{a} \end{bmatrix} + W \cdot \begin{bmatrix} X_{t} \\ Y_{t} \\ Z_{t} \end{bmatrix} + \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} \times \begin{bmatrix} X_{t} \\ Y_{t} \\ Z_{t} \end{bmatrix} $

Далее, благодаря формуле $\cos{\alpha} = \frac{Z}{\sqrt{X^{2} + Y^{2} + Z^{2}}}$, находим косинус угла между вектором $\vec{a^\prime}$ и осью $OZ$. Ну а угол между $XOY$ и $\vec{a^\prime}$ будет его дополнением, то есть $\arcsin({\cos{\alpha}})$.

С вектором $\vec{b}$ ситуация аналогичная.

Собственно, полученные результаты меня смущают. Либо криво закодил, либо решение ошибочное, так что прошу подтвердить или исправить меня. Если с векторами и матрицами могли быть разве что глупые ошибки вроде опечаток, то с кватернионами все плохо - вроде бы, понимаю геометрический смысл, но вот как были выведены формулы выше - для меня темный лес. Везде написано - как складывать, умножать и так далее кватернионы, а вот как получить координаты точки после поворота, зная кватернион - нигде не нашел, хотя, это, вроде бы, очевидный кейс.

Заранее благодарен всем неравнодушным.

[arseniiv]

Хм, но у плоскости же вроде только один угол наклона (угол между ней и горизонтальной плоскостью), а какие именно углы вам нужны?

[Devoter]

Есть прямоугольная платформа, в центре расположен датчик, нужно узнать углы наклона бортов (сторон). По сути, получается, надо угол наклона плоскости разложить на составляющие (правильне, наверное, будет сказать - проекции) по осям $OX$ и $OY$.

[arseniiv]

(Ага, понятно, значит ниже я правильно угадал.)

Общий совет: раз кватернион применяется к нескольким векторам, разумно его сначала сконвертировать в матрицу, а потом действовать уже ей — насколько помню, это дешевле уже начиная с трёх векторов, а с двух равнозатратно; но если хочется действовать на базисные векторы, то это ещё намного дешевле, потому что будут просто браться столбцы матрицы.

Давайте лучше обозначим векторы ортонормированного базиса $\vec e_x,\vec e_y,\vec e_z$. Если горизонтальная плоскость — это $\langle \vec e_x, \vec e_y\rangle$, и интересуют углы между $\vec e_x$ и ортогональной проекцией его на повёрнутую плоскость (и то же для $\vec e_y$), то просто повернуть их и взять углы между образами $\vec e'_x, \vec e'_y$ и исходными $\vec e_x, \vec e_y$ будет неправильно — это не обязательно даст те проекции. Но образы нам помогут, а точнее нам поможет только $\vec e'_z$: если мы вычтем из $\vec e_x, \vec e_y$ их проекции на него, то получим как раз нужные проекции на плоскость. Раз мы выбрали векторы единичными, эти проекции равны просто $\vec v - (\vec v\cdot\vec e'_z)\vec e'_z$. Угол между $\vec v$ и этим будет равен

$$\arccos(\vec v\cdot\vec v - (\vec v\cdot\vec e'_z)^2) = \arccos(1 - (\vec v\cdot\vec e'_z)^2),$$

ну и наконец $\vec v\cdot\vec e'_z$ — это просто соответствующая координата (первая для $\vec v = \vec e_x$ и вторая для $\vec e_y$) третьего столбца матрицы поворота.

Чтобы получить матрицу поворота по кватерниону, есть известные формулы. Раз нужны только два элемента (13 и 23) из этой матрицы, можно считать даже только их.

[Devoter]

Огромное спасибо за разъяснение и помощь, хотя, признаюсь, читал ваше сообщение как ребус. Моей идеей было не взять углы между образами $\vec{e_{x}\prime}$, $\vec{e_{y}\prime}$ и $\vec e_x$,$\vec e_y$, а между $\vec{e_{x}\prime}$, $\vec{e_{y}\prime}$ и $\vec e_z$.

Если я правильно понимаю, то ваше решение имеет похожую суть, но должно быть проще, потому повернуть нужно только один $\vec e_z$. С матрицей поворота все тоже понятно, спасибо за ссылку, а вот с проекциями - не очень. У вас там везде скалярные произведения векторов указаны? Хотелось бы не просто скопировать решение, а разобраться и понять. Расписывать не прошу, хотелось бы узнать исходный метод или формулу, а то это уже приведенный вариант.

[arseniiv]

Да, тут я обозначил точками скалярные произведения*, а произведения без точек — просто умножение вектора на скаляр. Когда некоторое направление задано единичным вектором $\vec\tau$, то ортогональная проекция какого-то вектора $\vec v$ на это направление (векторная) будет равна $(\vec v\cdot\vec\tau)\vec\tau$. В принципе я мог бы получить тот результат без проекций, сразу беря углы (тоже беря дополнительный угол как в вашей идее), но так привычнее.

* Вообще я какое-то время назад полюбил обозначение $(\vec u,\vec v)$ для скалярного произведения, но оно вроде не особо часто встречается в текстах по приложениям и я решил по-стародавнему взять точку для большей понятности.

[Devoter]

Еще раз благодарю за разъяснения, кажется, все встало на свои места. Пока что решение пошло по другому пути, но если дело все-таки дойдет до программной реализации в коде - обязательно отпишусь, дабы упростить в дальнейшем работу тем, кто столкнется с подобной проблемой после меня.