Преобразование эйлера в матрицу и матрицы в эйлера
-
22-09-2019 - |
Вопрос
Я пытаюсь преобразовать 3D-поворот, описанный в терминах углов Эйлера, в матрицу, а затем обратно, используя .NET / C #.Мои условности таковы:
- левосторонняя система (x справа, y сверху, z спереди)
- порядок вращений:курс вокруг y, подача вокруг x, крен вокруг z
- вращения выполняются положительно по правилу левой руки (большой палец указывает на + бесконечность).
Мое испытание - это:
Эйлер к матрице (Я удалил часть перевода x, y, z для упрощения)
Matrix3D matrix = new Matrix3D() {
M11 = cosH * cosB - sinH * sinP * sinB,
M12 = - sinB * cosP,
M13 = sinH * cosB + cosH * sinP * sinB,
M21 = cosH * sinB + sinH * sinP * cosB,
M22 = cosB * cosP,
M23 = sinB * sinH - cosH * sinP * cosB,
M31 = - sinH * cosP,
M32 = - sinP,
M33 = cosH * cosP,
};
Матрица для Эйлера
const double RD_TO_DEG = 180 / Math.PI;
double h, p, b; // angles in degrees
// extract pitch
double sinP = -matrix.M23;
if (sinP >= 1) {
p = 90; } // pole
else if (sinP <= -1) {
p = -90; } // pole
else {
p = Math.Asin(sinP) * RD_TO_DEG; }
// extract heading and bank
if (sinP < -0.9999 || sinP > 0.9999) { // account for small angle errors
h = Math.Atan2(-matrix.M31, matrix.M11) * RD_TO_DEG;
b = 0; }
else {
h = Math.Atan2(matrix.M13, matrix.M33) * RD_TO_DEG;
b = Math.Atan2(matrix.M21, matrix.M22) * RD_TO_DEG; }
Должно быть, это неправильно.Если я беру 3 угла, преобразую их в матрицу и преобразую матрицу обратно в углы, результат будет отличаться от начальных значений.
Я просмотрел несколько сайтов с разными формулами, начиная с euclideanspace.com, но теперь я полностью заблудился и не могу найти правильные вычисления.Я ценю вашу небольшую помощь.Есть ли математик на борту?
Решение
Во-первых, следует:
sinP = -matrix.M32
Редактировать: Полное решение следует
Мое происхождение:
Rx(P)=| 1 0 0 |
| 0 cos P -sin P |
| 0 sin P cos P |
Ry(H)=| cos H 0 sin H |
| 0 1 0 |
| -sin H 0 cos H |
Rz(B)=| cos B -sin B 0 |
| sin B cos B 0 |
| 0 0 1 |
Умножается на ваш заказ:
R = Ry(H)*Rx(P)*Rz(B)
= | cos H*cos B+sin H*sin P*sin B cos B*sin H*sin P-sin B*cos H cos P*sin H |
| cos P*sin B cos B*cos P -sin P |
| sin B*cos H*sin P-sin H*cos B sin H*sin B+cos B*cos H*sin P cos P*cos H |
Который дает обратные выводы:
загар B = M12/M22
sin P = -M32
загар H = M31/M33
Другие советы
Ваша идея неверна:"Должно быть, это неправильно.Если я беру 3 угла, преобразую их в матрицу и преобразую матрицу обратно в углы, результат будет отличаться от начальных значений ". Это было бы красиво, но не обязательно верно.В общем случае более одной тройки углов Эйлера (фиксированное соглашение) приводит к одинаковой ориентации в пространстве.Однако это не означает, что в ваших расчетах нет ошибки.Из Википедии:"Например, предположим, что мы используем приведенное выше соглашение zyz;тогда мы имеем следующие эквивалентные пары:(90°, 45°, -105°) ≡ (-270°, -315°, 255°) кратные 360° (72°, 0°, 0°) ≡ (40°, 0°, 32°) сингулярное выравнивание (45°, 60°, -30°) ≡ (-135°, -60°, 150°) бистабильный переворот "
Существует огромное количество комбинаций этих функций, поскольку ответ меняется в зависимости от ваших условностей.Обычно я использую DirectX и те же соглашения, что и Unity.Плюс мой опыт - это полеты, космос и карты, так что рыскание, затем тангаж, затем крен тоже соответствуют стилю широты.
Неясность в соглашениях или несоответствие функций compose / decompose может привести к очень странным ошибкам.Также стоит иметь в виду, что несколько наборов углов Эйлера могут создавать одну и ту же ориентацию.
Соглашения (как указано выше):
- Углы Эйлера:X = Тангаж, Y = Рыскание, Z = крен.
- Порядок Эйлера:Применен поворот, рыскание, затем тангаж, затем крен.
- Оси:+X Вправо, + Y Вверх, + Z Вперед
- Матрицы:Соглашения DirectX (с использованием SimpleMath.h из MS DirectXTK ( ДиректХТК ))
Чтобы преобразовать в версию OpenGL, взгляните на это.
Я взял Майк Танниклиффя преобразовал его в код на C ++ и добавил в свою библиотеку.Я надеюсь, что другие люди сэкономят некоторое время, используя его.
Стоит отметить, что функция compose очищает 4-й столбец и компонент translation до identity, а функция decompose предполагает, что элемент вращения 3x3 содержит чистое вращение (т. Е. без масштаба и т.д.).
Во-первых, код для генерации матрицы из Эйлеров:
//====================================================================================================
// MatrixFromYawPitchRoll
//
// Create matrix based on provided yaw (heading), pitch and roll (bank).
//
// Assumptions:
// Euler: X = Pitch, Y = Yaw, Z = Roll
// Applied: Yaw then pitch then roll
// Axes: X = Right, Y = Up, Z = Forward
// DirectX: Matrices are row major (http://www.mindcontrol.org/~hplus/graphics/matrix-layout.html)
//
// Code is based on Mike Tunnicliffe's answer to this question:
// https://stackoverflow.com/questions/1996957/conversion-euler-to-matrix-and-matrix-to-euler
inline void MatrixFromYawPitchRoll(
const DirectX::SimpleMath::Vector3& euler,
DirectX::SimpleMath::Matrix& mat)
{
float cosY = cosf(euler.y); // Yaw
float sinY = sinf(euler.y);
float cosP = cosf(euler.x); // Pitch
float sinP = sinf(euler.x);
float cosR = cosf(euler.z); // Roll
float sinR = sinf(euler.z);
mat = DirectX::SimpleMath::Matrix::Identity;
mat._11 = cosY * cosR + sinY * sinP * sinR;
mat._21 = cosR * sinY * sinP - sinR * cosY;
mat._31 = cosP * sinY;
mat._12 = cosP * sinR;
mat._22 = cosR * cosP;
mat._32 = -sinP;
mat._13 = sinR * cosY * sinP - sinY * cosR;
mat._23 = sinY * sinR + cosR * cosY * sinP;
mat._33 = cosP * cosY;
}
Затем закодируйте, чтобы получить обратно углы Эйлера из матрицы:
//====================================================================================================
// MatrixDecomposeYawPitchRoll
//
// Extract the rotation contained in the provided matrix as yaw (heading), pitch and roll (bank) in
// radiuans.
//
// Assumptions:
// Euler: X = Pitch, Y = Yaw, Z = Roll
// Applied: Yaw then pitch then roll
// Axes: X = Right, Y = Up, Z = Forward
// DirectX: Matrices are row major (http://www.mindcontrol.org/~hplus/graphics/matrix-layout.html)
//
// Code is based on Mike Tunnicliffe's answer to this question:
// https://stackoverflow.com/questions/1996957/conversion-euler-to-matrix-and-matrix-to-euler
inline void MatrixDecomposeYawPitchRoll(
const DirectX::SimpleMath::Matrix& mat,
DirectX::SimpleMath::Vector3& euler)
{
euler.x = asinf(-mat._32); // Pitch
if (cosf(euler.x) > 0.0001) // Not at poles
{
euler.y = atan2f(mat._31, mat._33); // Yaw
euler.z = atan2f(mat._12, mat._22); // Roll
}
else
{
euler.y = 0.0f; // Yaw
euler.z = atan2f(-mat._21, mat._11); // Roll
}
}