Преобразование Эйлера в матрицу и Матрица в Эйлер
Я пытаюсь преобразовать 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, но теперь я полностью потерян и не могу найти правильные вычисления. Я ценю небольшую помощь. На борту есть математик?
3 ответов
во-первых, следует:
sinP = -matrix.M32
EDIT: полное решение следующим образом
мой вывод:
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 |
который дает обратные производные:
tan B = M12 / M22
sin P = - M32
tan 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. Плюс мой фон-flightsims, пространство и карты, поэтому рыскать, а затем бросать матчи LAT/lon стиль тоже.
неясность в соглашениях или несоответствие функций compose/decompose может привести к очень странным ошибкам. Также стоит иметь в виду, что несколько наборов углов Эйлера могут производить то же самое ориентация.
соглашения (как указано выше):
- углы Эйлера: X = тангаж, Y = рыскание, Z = крен
- порядок Эйлера: вращение применяется, рыскание, затем шаг, затем крен
- Оси: + X Вправо, +Y Вверх, +Z Вперед
- матрицы: соглашения DirectX (с использованием SimpleMath.h от MS DirectXTK)
чтобы преобразовать в версию OpenGL, взгляните на этой.
Я Майк Tunnicliffe's ответ и преобразовал его в код C++ и добавил его в мою библиотеку. Я надеюсь, что другие люди сэкономят время, используя его.
стоит отметить, что функция compose очищает 4-й столбец и компонент перевода для идентификации, а функция decompose предполагает, что элемент вращения 3x3 содержит чистое вращение (т. е. без масштаба и т. д.).
во-первых, код для создания матрицы из Eulers:
//====================================================================================================
// 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
}
}