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

Вращение вектора кватернионом
--- - id: 0 body0: |

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

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



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


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

на заданный угол:


(w, vx, vy, vz),



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

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

в его начало.



Например, кватернион[9] поворота вдоль оси Х на 90 градусов имеет следующие значения своих компонент:

w = 0,7071; x = 0,7071; y = 0; z = 0.

Левая или правая система координат, разницы нет – главное, чтобы все операции выполнялись в одинаковых системах

координат,

или все в левых или все в правых.





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

вокруг него:



<code><span class="hljs-keyword"
    data-redactor-tag="span">Public</span> <span class="hljs-keyword">Function</span>
    create_quat(rotate_vector As TVector, rotate_angle As Double)
    As TQuat rotate_vector = normal(rotate_vector)
    create_quat.w = <span class="hljs-built_in">Cos</span>(rotate_angle / <span class="hljs-number">2</span>)
    create_quat.x = rotate_vector.x * <span class="hljs-built_in">
    Sin</span>(rotate_angle / <span class="hljs-number">2</span>)
    create_quat.y = rotate_vector.y * <span class="hljs-built_in">
    Sin</span>(rotate_angle / <span class="hljs-number">2</span>)
    create_quat.z = rotate_vector.z * <span class="hljs-built_in">
    Sin</span>(rotate_angle / <span class="hljs-number">2</span>)
    <span class="hljs-keyword">End</span> <span class="hljs-keyword">Function</span> 

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



<code><span class="hljs-keyword" data-redactor-tag="span">Public</span>
    <span class="hljs-keyword">Function</span> normal(v As TVector)
    As TVector <span class="hljs-keyword">Dim</span>
    length As Double length = (v.x ^ <span class="hljs-number">2</span> + v.y ^
    <span class="hljs-number">2</span> + v.z ^
    <span class="hljs-number">2</span>) ^ <span class="hljs-number">0.5</span>
    normal.x = v.x / length normal.y = v.y / length normal.z = v.z / length
    <span class="hljs-keyword">End</span> <span class="hljs-keyword">Function</span> 

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



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


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



Чтобы получить обратный кватернион[9] от заданного, достаточно развернуть вектор оси в другую сторону и при

необходимости нормализовать кватернион[9]. Нормализация кватерниона так же как и в векторах, это просто

приведение к длине = 1.



<code><span class="hljs-keyword" data-redactor-tag="span">Public</span>
    <span class="hljs-keyword">Function</span> quat_scale(q As TQuat, val As Double) As TQuat q.w = q.w *
    val <span class="hljs-comment">' x</span> q.x = q.x * val <span class="hljs-comment">' x</span> q.y = q.y *
    val <span class="hljs-comment">' y</span> q.z = q.z * val <span class="hljs-comment">' z</span> quat_scale = q
    <span class="hljs-keyword">End</span> <span class="hljs-keyword">Function</span>
    <span class="hljs-keyword">Public</span> <span class="hljs-keyword">Function</span> 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) ^
    <span class="hljs-number">0.5</span>
    <span class="hljs-keyword">End</span> <span class="hljs-keyword">Function</span>
    <span class="hljs-keyword">Public</span> <span class="hljs-keyword">Function</span> quat_normalize(q As TQuat)
    As TQuat <span class="hljs-keyword">Dim</span> n As Double n = quat_length(q)
    quat_normalize = quat_scale(q, <span class="hljs-number">1</span> / n)
    <span class="hljs-keyword">End</span> <span class="hljs-keyword">Function</span>
    <span class="hljs-keyword">Public</span> <span class="hljs-keyword">Function</span> quat_invert(q As TQuat)
    As TQuat <span class="hljs-keyword">Dim</span> res As TQuat
    res.w = q.w
    res.x = -q.x
    res.y = -q.y
    res.z = -q.z
    quat_invert = quat_normalize(res)
    <span class="hljs-keyword">End</span> <span class="hljs-keyword">Function</span> 

Например, если разворот вокруг оси 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. Кватернион[9], который означает «нет разворота» = (w=1; x = 0; y = 0; z=0), то есть у него длина вектора оси = 0.



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


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





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



<code><span class="hljs-keyword" data-redactor-tag="span">Public</span> <span class="hljs-keyword">
    Function</span> quat_mul_quat(a As TQuat, b As TQuat)
    As TQuat <span class="hljs-keyword">Dim</span> 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
    <span class="hljs-keyword">End</span> <span class="hljs-keyword">Function</span> 

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



<code><span class="hljs-keyword" data-redactor-tag="span">Public</span> <span class="hljs-keyword">
    Function</span> quat_mul_vector(a As TQuat, b As TVector)
    As TQuat <span class="hljs-keyword">Dim</span> 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
    <span class="hljs-keyword">End</span> <span class="hljs-keyword">Function</span> 

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


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



<code><span class="hljs-keyword" data-redactor-tag="span">Public</span> <span class="hljs-keyword">
    Function</span> quat_transform_vector(q As TQuat, v As TVector)
    As TVector <span class="hljs-keyword">Dim</span> 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
    <span class="hljs-keyword">End</span> <span class="hljs-keyword">Function</span> 

Пример:



Вектор описывающий ось (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.



<code><span class="hljs-keyword" data-redactor-tag="span">Public</span> <span class="hljs-keyword">
    Function</span> quat_from_angles_rad(angles As TKrylov)
    As TQuat <span class="hljs-keyword">Dim</span> q_heading As TQuat <span class="hljs-keyword">Dim</span>
    q_alt As TQuat <span class="hljs-keyword">Dim</span> q_bank As TQuat <span class="hljs-keyword">Dim</span>
    q_tmp As TQuat
    q_heading.x = <span class="hljs-number">0</span> q_heading.y = <span class="hljs-number">0</span>
    q_heading.z = <span class="hljs-built_in">Sin</span>(angles.heading / <span class="hljs-number">2</span>)
    q_heading.w = <span class="hljs-built_in">Cos</span>(angles.heading / <span class="hljs-number">2</span>)
    q_alt.x = <span class="hljs-number">0</span> q_alt.y = <span class="hljs-built_in"
    >Sin</span>(angles.altitude / <span class="hljs-number">2</span>)
    q_alt.z = <span class="hljs-number">0</span> q_alt.w = <span class="hljs-built_in"
    >Cos</span>(angles.altitude / <span class="hljs-number">2</span>)
    q_bank.x = <span class="hljs-built_in">Sin</span>(angles.bank / <span class="hljs-number">2</span>)
    q_bank.y = <span class="hljs-number">0</span> q_bank.z = <span class="hljs-number">0</span>
    q_bank.w = <span class="hljs-built_in">Cos</span>(angles.bank / <span class="hljs-number">2</span>)
    q_tmp = quat_mul_quat(q_heading, q_alt)
    quat_from_angles_rad = quat_mul_quat(q_tmp, q_bank) <span class="hljs-keyword">End</span>
    <span class="hljs-keyword">Function</span> 

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



<code><span class="hljs-keyword" data-redactor-tag="span">Public</span> <span class="hljs-keyword">
    Function</span> quat_to_krylov(q As TQuat)
    As TKrylov <span class="hljs-keyword">Dim</span> qx2 As Double <span class="hljs-keyword">Dim</span>
    qy2 As Double <span class="hljs-keyword">Dim</span> qz2 As Double
    q = quat_normalize(q) <span class="hljs-comment">'кватернион[9] должен быть нормализован</span>
    qx2 = q.x * q.x qy2 = q.y * q.y
    qz2 = q.z * q.z
    quat_to_krylov_v3.bank = atan2(<span class="hljs-number">2</span> * (q.x * q.w + q.y * q.z),
    <span class="hljs-number">1</span> - <span class="hljs-number">2</span> * (qx2 + qy2))
    quat_to_krylov_v3.altitude =
    Application.WorksheetFunction.Asin(<span class="hljs-number">2</span> * (q.y * q.w - q.z * q.x))
    quat_to_krylov_v3.heading =
    atan2(<span class="hljs-number">2</span> * (q.z * q.w + q.x * q.y),
    <span class="hljs-number">1</span> - <span class="hljs-number">2</span> * (qy2 + qz2))
    <span class="hljs-keyword">End</span> <span class="hljs-keyword">Function</span> 

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



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


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



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



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









Написать:
13:20
1933
Нет комментариев. Ваш будет первым!