Заметки о вращении вектора кватернионом

Структура публикации


  • Получение кватерниона из вектора и величины угла разворота
  • Обратный кватернион
  • Умножение кватернионов
  • Поворот вектора
  • Рысканье, тангаж, крен
  • Серия поворотов


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


Ещё раз – что такое кватернион? Для разработчика – это прежде всего инструмент, описывающий действие – поворот вокруг оси на заданный угол:

(w, vx, vy, vz),

где v – ось, выраженная вектором;
w – компонента, описывающая поворот (косинус половины угла).

Положительное значение угла разворота означает поворот вдоль вектора по часовой стрелке, если смотреть с конца вектора в его начало.

Например, кватернион поворота вдоль оси Х на 90 градусов имеет следующие значения своих компонент: w = 0,7071; x = 0,7071; y = 0; z = 0. Левая или правая система координат, разницы нет – главное, чтобы все операции выполнялись в одинаковых системах координат, или все в левых или все в правых.



С помощью следующего кода (под рукой был Visual Basic), мы можем получить кватернион из вектора и угла разворота вокруг него:

Public Function create_quat(rotate_vector As TVector, rotate_angle As Double) As TQuat
    
    rotate_vector = normal(rotate_vector)
    create_quat.w = Cos(rotate_angle / 2)
    create_quat.x = rotate_vector.x * Sin(rotate_angle / 2)
    create_quat.y = rotate_vector.y * Sin(rotate_angle / 2)
    create_quat.z = rotate_vector.z * Sin(rotate_angle / 2)
    
End Function

В коде rotate_vector – это вектор, описывающий ось разворота, а rotate_angle – это угол разворота в радианах. Вектор должен быть нормализован. То есть его длина должа быть равна 1.

Public Function normal(v As TVector) As TVector

    Dim length As Double 
    length = (v.x ^ 2 + v.y ^ 2 + v.z ^ 2) ^ 0.5    
    normal.x = v.x / length
    normal.y = v.y / length
    normal.z = v.z / length
    
End Function

Не забывайте про ситуацию, когда длина может быть 0. Вместо ошибки вам может понадобиться обработать эту ситуацию индивидуально.

Обратный кватернион


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

Чтобы получить обратный кватернион от заданного, достаточно развернуть вектор оси в другую сторону и при необходимости нормализовать кватернион. Нормализация кватерниона так же как и в векторах, это просто приведение к длине = 1.

Public Function quat_scale(q As TQuat, val As Double) As TQuat

    q.w = q.w * val ' x
    q.x = q.x * val ' x
    q.y = q.y * val ' y
    q.z = q.z * val ' z
    quat_scale = q

End Function

Public Function quat_length(q As TQuat) As Double

    quat_length = (q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z) ^ 0.5

End Function

Public Function quat_normalize(q As TQuat) As TQuat

    Dim n As Double
    n = quat_length(q)
    quat_normalize = quat_scale(q, 1 / n)

End Function

Public Function quat_invert(q As TQuat) As TQuat
    Dim res As TQuat
    
    res.w = q.w
    res.x = -q.x
    res.y = -q.y
    res.z = -q.z
    quat_invert = quat_normalize(res)
End Function

Например, если разворот вокруг оси Y на 90 градусов = (w=0,707; x = 0; y = 0,707; z=0), то обратный = (w=0,707; x = 0; y = -0,707; z=0). Казалось бы, можно инвертировать только компоненту W, но при поворотах на 180 градусов она = 0. Кватернион, который означает «нет разворота» = (w=1; x = 0; y = 0; z=0), то есть у него длина вектора оси = 0.

Умножение кватернионов


Умножение кватернионов крайне полезная штука. Результатом умножения является кватернион, который после поворота даёт такой же результат, если последовательно выполнить развороты умножаемыми кватернионами. Причём разворот будет происходить в локальной для поворачиваемого вектора системе отчёта, т. е. система отчёта поворачиваемого вектора также двигается.



Умножение кватернионов выполняется следующим образом:

Public Function quat_mul_quat(a As TQuat, b As TQuat) As TQuat

    Dim res As TQuat
    res.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
    res.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y
    res.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x
    res.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w
    quat_mul_quat = res

End Function

Для того, чтобы умножить кватернион на 3D вектор, нужно вектор преобразовать в кватернион присвоив компоненте W = 0 и умножить кватернион на кватернион. Или подставить ноль и выразить это в виде функции:

Public Function quat_mul_vector(a As TQuat, b As TVector) As TQuat
    
    Dim res As TQuat
    res.w = -a.x * b.x - a.y * b.y - a.z * b.z
    res.x = a.w * b.x + a.y * b.z - a.z * b.y
    res.y = a.w * b.y - a.x * b.z + a.z * b.x
    res.z = a.w * b.z + a.x * b.y - a.y * b.x
    quat_mul_vector = res
    
End Function

Поворот вектора


Теперь, собственно, поворот вектора кватернионом:

Public Function quat_transform_vector(q As TQuat, v As TVector) As TVector
    
    Dim t As TQuat
    
    t = quat_mul_vector(q, v)
    t = quat_mul_quat(t, quat_invert(q))
    
    quat_transform_vector.x = t.x
    quat_transform_vector.y = t.y
    quat_transform_vector.z = t.z
    
End Function

Пример:

Вектор описывающий ось (x=1; y=0; z=1). Угол поворота 180 градусов.
Поворачиваемый вектор (x=0; y=0; z=1). Результат равен (x=1; y=0; z=0).



Рысканье, тангаж, крен


Рассмотрим инструмент формирования кватерниона с помощью поворотов вокруг одной из осей:
Рысканье = heading = yaw = вокруг оси Z; тангаж = altitude = pitch = вокруг оси Y; крен = bank = roll = вокруг оси X.

Public Function quat_from_angles_rad(angles As TKrylov) As TQuat
    
    Dim q_heading As TQuat
    Dim q_alt As TQuat
    Dim q_bank As TQuat
    Dim q_tmp As TQuat
    
    q_heading.x = 0
    q_heading.y = 0
    q_heading.z = Sin(angles.heading / 2)
    q_heading.w = Cos(angles.heading / 2)
    
    q_alt.x = 0
    q_alt.y = Sin(angles.altitude / 2)
    q_alt.z = 0
    q_alt.w = Cos(angles.altitude / 2)
    
    q_bank.x = Sin(angles.bank / 2)
    q_bank.y = 0
    q_bank.z = 0
    q_bank.w = Cos(angles.bank / 2)
    
    q_tmp = quat_mul_quat(q_heading, q_alt)
    quat_from_angles_rad = quat_mul_quat(q_tmp, q_bank)
         
End Function

И в обратную сторону, из кватерниона:

Public Function quat_to_krylov(q As TQuat) As TKrylov
    Dim qx2 As Double
    Dim qy2 As Double
    Dim qz2 As Double    
    q = quat_normalize(q) 'кватернион должен быть нормализован
    qx2 = q.x * q.x
    qy2 = q.y * q.y
    qz2 = q.z * q.z
    quat_to_krylov_v3.bank = atan2(2 * (q.x * q.w + q.y * q.z), 1 - 2 * (qx2 + qy2))
    quat_to_krylov_v3.altitude = Application.WorksheetFunction.Asin(2 * (q.y * q.w - q.z * q.x))
    quat_to_krylov_v3.heading = atan2(2 * (q.z * q.w + q.x * q.y), 1 - 2 * (qy2 + qz2))
End Function

Формулы преобразования зависят от принятой системы координат.

Серия поворотов


Рассмотрим пример:
1. Первый поворот – рысканье (вокруг Z) 90 градусов по часовой;
2. Второй поворот – тангаж (вокруг Y) 90 градусов по часовой;
3. Третий поворот – крен (вокруг X) 90 градусов по часовой.

Рисунки, изображающие поворот и подписанные как «global» демонстрируют повороты относительно неподвижных осей XYZ. Такой результат мы получим, если будем использовать кватернионы разворота по отдельности. Четвёртый рисунок демонстрирует, где окажется вектор, если начальные координаты у него были X=1; Y=0; Z=0.

Рисунки, подписанные как «local» демонстрируют вращение осей вместе с самолетом. То есть все вращения происходят относительно пилота, а не относительно неподвижной системы координат. Четвёртый рисунок показывает, где окажется тот же самый вектор (1; 0; 0) в итоге всех трёх поворотов. Такой результат мы получим, перемножив кватернионы разворота и применив полученный кватернион.







Поделиться публикацией
Похожие публикации
AdBlock похитил этот баннер, но баннеры не зубы — отрастут

Подробнее
Реклама
Комментарии 22
  • +2
    Зачем после инверсии кватерниона делать его нормализацию?
    • 0
      Обратный кватернион предполагает только обратный разворот без масштабирования, поэтому нормализуется.

      То есть если все компоненты кватерниона умножить на число, например на 3, то после поворота вектора таким кватернионом, длина вектора увеличится в 3 раза.

      Давайте рассмотрим прикладную сторону вопроса и посмотрим на исходники от NVIDIA. Вот эта функция только отражает кватернион:

      inline Quaternion conjugate(Quaternion::Arg q)
      {
      	return scale(q, Vector4(-1, -1, -1, 1));
      }
      


      А эта функция отражает и нормализует:
      inline Quaternion inverse(Quaternion::Arg q)
      {
      	return conjugate(normalize(q));
      }
      


      Где функция нормализации выглядит так:
      inline Quaternion normalize(Quaternion::Arg q, float epsilon = NV_EPSILON)
      {
      	float l = length(q);
      	nvDebugCheck(!isZero(l, epsilon));
      	Quaternion n = scale(q, 1.0f / l);
      	nvDebugCheck(isNormalized(n));
      	return n;
      }
      
      • 0
        Вы приводили функцию, создающую кватернион поворота — длина этого кватерниона всегда равна 1. Обратный кватернион для него можно найти без нормализации.

        Откуда вообще взялись кватернионы масштабирования? В статье про них — ни слова.
        • 0
          Вы правы. В рамках данной статьи длина кватерниона не меняется и в принципе можно без нормализации.
          • 0
            В одной из публикаций на тему БИНС встречал также нормализацию кватерниона в месте, где вроде бы она не требуется. Автор специально уточнил, что в кватернионной арифметике с плавающей точкой, как и везде, могут иметь место вычислительные ошибки. Уже по этой причине в ключевых местах может иметь смысл нормализацию проводить.
            • 0
              Смена знака — эта не та операция, на которой могут накопиться погрешности.
              • +1
                Вы приводили функцию, создающую кватернион поворота — длина этого кватерниона всегда равна 1. Обратный кватернион для него можно найти без нормализации.


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

                Но в контексте статьи выглядит как излишняя предосторожность.
      • +2
        Очень неполная статья, вообще не затронуты темы
        — двойного покрытия пространства вращений единичными кватернионами
        — того, что одному кватерниону соответствуют два набора углов Эйлера (ничего общего с первым пунктом)
        — складывания рамок для углов Эйлера

        Соответствующая статья в википедии полнее и доступнее.
        • 0
          Спасибо за замечание. Не хотел загромождать статью теоретическими деталями.

          Про то, что одно и то же вращение можно описать двумя кватернионами, вроде бы и так понятно.
          Например давайте опишем движение стрелки часов, если ось стрелки совпадает с осью X.

          В первом кватернионе вектор оси будет направлен от лицевой стороны циферблата (w = 0,7071; x = 0,7071; y = 0; z = 0), а разворот будет осуществляться по часовой стрелке. Во втором кватернионе вектор оси и направление поворота осуществляются в обратную сторону.

          Про наборы углов Эйлера и складывание рамок — это скорее для статьи про углы Эйлера-Крылова. Про наборы углов может быть интересной деталью — приведите пожалуйста пример. Если я правильно понимаю, речь о том. что например, только тангаж 90 равен курс 180, тангаж 90, крен 180.
        • 0
          А зачем нужен Gimbals lock в статье про кватернионы? Это матричная проблема, причём матрицы вообще не тема статьи.

          У меня другой вопрос к автору. Как вы вычисляете Cx и Cy?
          • 0
            Спасибо. Действительно статья о кватернионах не предполагает освещение вопроса о складывании рамок.

            Должен спросить — что Вы понимаете под Cx и Cy?
            • 0
              Скриншот с F-14D Tomcat в блендере ваш собственный — в гугле дублей нет.
              Подумал, что вы делаете что-то об аэродинамике.
              Подумал, что вы имеете подход к этому вопросу.
              Если нет, то извините.
            • 0
              В матрицах нет складывания рамок, это проблема рамок, которые вполне себе упоминаются — тангаж, крен и тп.
            • 0
              Если интересно — вот Excel-файл с макросами на котором я тренировался: файл.
              • 0
                Можно ли говорить, повороты в глобальной неподвижной системе(global) координат — одновременные, а локальной(local) — последовательные? Если да, то можно ли повернуть вектор в неподвижной системе последовательно чередуя повороты на бесконечно малые углы в локальной?
                • 0
                  «Одновременных» поворотов не бывает.
                  • 0
                    одновременные в смысле, полученные суммированием векторов поворота
                    насколько понял, чередуя повороты на бесконечно малые углы(в неподвижной системе), можно повернуть на суммарный угол(тоже в неподвижной системе)
                    • 0
                      Что такое «суммарный» угол в вашем понимании?
                      • 0
                        на вопросы из первого комментария, можно не отвечать, разобрался
                        суммарный угол — тот, который соответствует суммарному вектору поворота(длина соответствует углу поворота, направление — оси)
                        • 0
                          Хорошо. А то я сижу перечитываю раз десятый ).
                • 0
                  Можно ли как-то для кватерниона ограничить вращение вокруг какой-то оси не переводя в углы эйлера и назад? (например вращение вокруг оси Z ограничивается -30 + 30).
                  • 0
                    Путём сравнения с другим кватернионом.

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