Я работаю над симуляцией движения самолета.На данный момент я использовал углы Эйлера, чтобы преобразовать «кадр тела» в «кадр мира», и он отлично работает.
Недавно я узнал о кватернионах и их преимуществах по сравнению с матрицей вращения (блокировка карданного подвеса) и попытался реализовать ее с использованием углов рыскания / тангажа / кренаиз симулятора.
Quaternion
Если я правильно понимаю, кватернион представляет две вещи.Он имеет компонент x, y, и z , представляющий ось, вокруг которой будет происходить вращение.Он также имеет компонент w , который представляет величину вращения, которая произойдет вокруг этой оси.Короче говоря, вектор и поплавок.Кватернион может быть представлен как вектор из 4 элементов:
q = [w, x, y, z]
Для вычисления результата (после полного вращения) используется уравнение:
p '= qpq'
где:
p = [0, x, y,z] -вектор направления
q = [w, x, y, z] -вращение
q '= [w, -x, -y, -z]
Алгоритм
- Создать кватернион:
Используя Википедия Я создаю кватернион с помощьювращение вокруг 3 осей (q):
Quaterniond toQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
//Degree to radius:
yaw = yaw * M_PI / 180;
pitch = pitch * M_PI / 180;
roll = roll * M_PI / 180;
// Abbreviations for the various angular functions
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
Quaterniond q;
q.w = cy * cp * cr + sy * sp * sr;
q.x = cy * cp * sr - sy * sp * cr;
q.y = sy * cp * sr + cy * sp * cr;
q.z = sy * cp * cr - cy * sp * sr;
return q;
}
Определить вектор направления (направления) плоскости:
p = [0,1,0,0]
Рассчитать Произведение Гамильтона :
p '= qpq'
q '= [w, -qx, -qy, -qz]
p '= (H (H (q, p), q')
Quaterniond HamiltonProduct(Quaterniond u, Quaterniond v)
{
Quaterniond result;
result.w = u.w*v.w - u.x*v.x - u.y*v.y - u.z*v.z;
result.x = u.w*v.x + u.x*v.w + u.y*v.z - u.z*v.y;
result.y = u.w*v.y - u.x*v.z + u.y*v.w + u.z*v.x;
result.z = u.w*v.z + u.x*v.y - u.y*v.x + u.z*v.w;
return result;
}
Результат
Мой результат будетбыть вектором:
v = [p'x, p'y, p'z]
Работает нормально, но так же, как угол поворота Эйлера (блокировка карданного подвеса)Это потому, что я здесь использую также углы Эйлера? Я не понимаю, как это должно работать без вращения вокруг 3 осей. Должен ли я вращаться вокруг каждой оси отдельно?
Буду благодарен за любые советы ипомогите разобраться в этой проблеме.
РЕДАКТИРОВАТЬ (как работает приложение)
1. Мое приложение, основанное на потоковой передаче данных, означает, что через каждые 1 мс оно проверяет наличие новых данных (новая ориентация плоскости).
Пример:
В начале тангаж / крен / рыскание = 0 , после 1 мс рыскание изменяется на 10 градусов, поэтому приложение читает pitch = 0, крен = 0, рыскание = 10 .После следующих 1 мс рыскание снова изменяется на 20 градусов.Таким образом, входные данные будут выглядеть так: pitch = 0, roll = 0, yaw = 30 .
2.Создать направление кватерниона - p
В начале я определяю, что направление (голова) моей плоскости находится на оси X.Таким образом, мое местное направление - v = [1,0,0] в кватернионе (мой p ): p = [0,1,0,0]
Vector3 LocalDirection, GlobalDirection; //head
Quaterniond p,P,q, Q, pq; //P = p', Q=q'
LocalDirection.x = 1;
LocalDirection.y = 0;
LocalDirection.z = 0;
p.w = 0;
p.x = direction.x;
p.y = direction.y;
p.z = direction.z;
3.Создать вращение
После каждых 1 мс Я проверяю углы поворота (Эйлера) из потока данных и вычисляю q , используя toQuaternion
q = toQuaternion(yaw, pitch, roll); // create quaternion after rotation
Q.w = q.w;
Q.x = -q.x;
Q.y = -q.y;
Q.z = -q.z;
4.Вычислить «направление мира»
Используя Произведение Гамильтона Я вычисляю кватернион после вращения, который является моим глобальным направлением:
pq = HamiltonProduct(q, p);
P = HamiltonProduct(pq, Q);
GlobalDirection.x = P.x;
GlobalDirection.y = P.y;
GlobalDirection.z = P.z;
5.Повторите 3-4 каждые 1 мс