Извлекаем вектор и угол из матрицы поворота

В продолжение темы кватерниона…

Итак, есть матрица 4×4, где три оси ортогональны друг другу (по-нашенски, по-деревенски — это означает, что три оси перпендикулярны каждая с каждой, представьте оси прямоугольной системы координат, так вот это оно и есть).

Выглядит матрица вот так:

$M=\begin{bmatrix}A&E&I&M\\B&F&J&N\\C&G&K&O\\D&H&L&P\end{bmatrix}$

В ней, как я уже рассказывал, «живут» три вектора ABC (ось X), EFG (ось Y), IJK (ось Z). Также есть три смещения вдоль XYZ — это M, N и O соответственно.

Для того, чтобы извлечь вектор и угол из этой матрицы я использую вот такую конструкцию (псевдокод):

def getAngleAxis(m):
    xx = m[A]
    yy = m[F]
    zz = m[K]
 
    # Сумма элементов главной диагонали
    traceR = xx + yy + zz
 
    # Угол поворота
    theta = acos((traceR - 1) * 0.5)
 
    # Упростим вычисление каждого элемента вектора
    omegaPreCalc = 1.0 / (2 * sin(theta))
 
    # Вычисляем вектор
    w.x = omegaPreCalc * (m[J] - m[G])
    w.y = omegaPreCalc * (m[C] - m[I])
    w.z = omegaPreCalc * (m[E] - m[B])
 
    # Получаем угол поворота и ось, 
    # относительно которой был поворот
    return (theta, w)

Мне довелось применять этот метод на матрице, где вектора ABC, EFG и IJK нормализованы. Масштаб хранится отдельно. Если вы храните масштаб внутри матрицы, то перед применением формулы надо нормализовать вектора ABC, EFG и IJK).