- PVSM.RU - https://www.pvsm.ru -
Ещё раз – что такое кватернион [1]? Для разработчика – это прежде всего инструмент, описывающий действие – поворот вокруг оси на заданный угол:
(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
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.
yaw = angles.altitude
pitch = angles.bank
roll = angles.heading
hr = roll * 0.5
shr = Sin(hr)
chr = Cos(hr)
hp = pitch * 0.5
shp = Sin(hp)
chp = Cos(hp)
hy = yaw * 0.5
shy = Sin(hy)
chy = Cos(hy)
chy_shp = chy * shp
shy_chp = shy * chp
chy_chp = chy * chp
shy_shp = shy * shp
res.w = (chy_chp * chr) + (shy_shp * shr)
res.x = (chy_shp * chr) + (shy_chp * shr)
res.y = (shy_chp * chr) - (chy_shp * shr)
res.z = (chy_chp * shr) - (shy_shp * chr)
quat_from_angles_rad = res
И в обратную сторону, из кватерниона:
k.bank = atan2(2 * (w * x + y * z), 1 - 2 * (sqx + sqy))
k.altitude = Application.WorksheetFunction.Asin(2 * (w * y - x * z))
k.heading = atan2(2 * (w * z + x * y), 1 - 2 * (sqy + sqz))
Рассмотрим пример:
1. Первый поворот – рысканье (вокруг Z) 90 градусов по часовой;
2. Второй поворот – тангаж (вокруг Y) 90 градусов по часовой;
3. Третий поворот – крен (вокруг X) 90 градусов по часовой.
Рисунки, изображающие поворот и подписанные как «global» демонстрируют повороты относительно неподвижных осей XYZ. Такой результат мы получим, если будем использовать кватернионы разворота по отдельности. Четвёртый рисунок демонстрирует, где окажется вектор, если начальные координаты у него были X=1; Y=0; Z=0.
Рисунки, подписанные как «local» демонстрируют вращение осей вместе с самолетом. То есть все вращения происходят относительно пилота, а не относительно неподвижной системы координат. Четвёртый рисунок показывает, где окажется тот же самый вектор (1; 0; 0) в итоге всех трёх поворотов. Такой результат мы получим, перемножив кватернионы разворота и применив полученный кватернион.
Автор: 1div0
Источник [2]
Сайт-источник PVSM.RU: https://www.pvsm.ru
Путь до страницы источника: https://www.pvsm.ru/matematika/88039
Ссылки в тексте:
[1] кватернион: https://ru.wikipedia.org/wiki/%D0%9A%D0%B2%D0%B0%D1%82%D0%B5%D1%80%D0%BD%D0%B8%D0%BE%D0%BD
[2] Источник: http://habrahabr.ru/post/255005/
Нажмите здесь для печати.