Задача с углами Эйлера из матрицы вращения YZX - PullRequest
Новая
       44

Задача с углами Эйлера из матрицы вращения YZX

3 голосов
/ 28 июня 2011

Я застрял, получая углы Эйлера из матрицы вращения.

Мои условные обозначения:

  • Левша (x вправо, z назад, y вверх)
  • YZX
  • Левосторонний угол поворота

Моя матрица вращения построена из углов Эйлера, как (из моего кода): 

    var xRotationMatrix = $M([
        [1,  0,   0, 0], 
        [0, cx, -sx, 0], 
        [0, sx,  cx, 0], 
        [0,  0,   0, 1]
    ]);

    var yRotationMatrix = $M([
        [ cy, 0, sy, 0], 
        [  0, 1,  0, 0], 
        [-sy, 0, cy, 0], 
        [  0, 0,  0, 1]
    ]);
    var zRotationMatrix = $M([
        [cz, -sz, 0, 0], 
        [sz,  cz, 0, 0], 
        [ 0,   0, 1, 0], 
        [ 0,   0, 0, 1]
    ]);

В результате чего получается итоговая матрица вращения: 

R(YZX) = | cy.cz, -cy.sz.cx + sy.sx,  cy.sz.sx + sy.cx, 0|
         |    sz,             cz.cx,            -cz.sx, 0|
         |-sy.cz,  sy.sz.cx + cy.sx, -sy.sz.sx + cy.cx, 0|
         |     0,                 0,                 0, 1|

Я вычисляю свои углы Эйлера обратно из этой матрицы, используя этот код: 

this.anglesFromMatrix = function(m) {
    var y = 0, x = 0, z = 0;

    if (m.e(2, 1) > 0.999) {
        y = Math.atan2(m.e(1, 3), m.e(3, 3));
        z = Math.PI / 2;
        x = 0;
    } else if (m.e(2, 1) < -0.999) {
        y = Math.atan2(m.e(1, 3), m.e(3, 3));
        z = -Math.PI / 2;
        x = 0;
    } else {
        y = Math.atan2(-m.e(3, 1), -m.e(1, 1));
        x = Math.atan2(-m.e(2, 3), m.e(2, 2));
        z = Math.asin(m.e(2, 1));
    }
    return {theta: this.deg(x), phi: this.deg(y), psi: this.deg(z)};
};

Я делал математику взад и вперед несколько раз, но я не вижу, что не так. Любая помощь будет с благодарностью.

Ответы [ 3 ]

2 голосов
/ 28 июня 2011

Ваши углы матрицы и Эйлера не совпадают. Похоже, вы должны использовать 

y = Math.atan2(-m.e(3, 1), m.e(1, 1));

вместо 

y = Math.atan2(-m.e(3, 1), -m.e(1, 1));

для общего случая (ветвь else).

Я сказал "похоже", потому что - что это за язык? Я предполагаю, что у вас есть правильное индексирование для этого языка. Ты уверен насчет atan2? Для atan2 не существует единого соглашения. В некоторых языках программирования термин синус является первым аргументом, в других термин косинус является первым аргументом.

1 голос
/ 28 июня 2011

Последняя и самая важная ветвь функции anglesFromMatrix имеет небольшую ошибку знака, но в остальном работает правильно. Используйте 

y = Math.atan2(-m.e(3, 1), m.e(1, 1))

, поскольку только m.e(3, 1) из m.e(1, 1) = cy.cz и m.e(3, 1) = -sy.cz должны быть инвертированы. Я не проверял другие ветки на ошибки.

Помните, что, поскольку sz = m.e(2, 1) имеет два решения, углы (x, y, z), используемые для построения матрицы m, могут не совпадать с углами (rx, ry, rz), возвращаемыми anglesFromMatrix(m). Вместо этого мы можем проверить, что матрица rm, построенная из (rx, ry, rz), действительно равна m.

0 голосов
/ 09 сентября 2014

Я много работал над этой проблемой, чтобы придумать правильные углы для данной матрицы.Проблема в математике возникает из-за невозможности определить точное значение для SIN, так как -SIN (x) = SIN (-x), и это повлияет на другие значения матрицы.Решение, которое я придумал, предлагает два одинаково достоверных решения из восьми возможных.Я использовал стандарт Z.Y.Форма матрицы X, но она должна быть адаптирована к любой матрице.Начнем с поиска трех углов: X = atan (m32, m33): Y = -asin (m31): Z = atan (m21, m11): Затем создайте углы X '= -sign (X) * PI + XY '= знак (Y) * PI-Y: Z = -знак (Z) * pi + Z.Используя эти углы, создайте восемь групп углов: XYZ: X'YZ: XYZ ': X'YZ': X'Y'Z ': XY'Z': X'Y'Z: XY'Zвосемь соответствующих матриц.Затем сделайте сумму разности между неизвестной матрицей и каждой матрицей.Это сумма каждого элемента неизвестного минус один и тот же элемент тестовой матрицы.После этого две суммы будут равны нулю, и эти матрицы будут представлять углы решения для исходной матрицы.Это работает для всех возможных комбинаций углов, включая 0.По мере того как вводятся 0, больше из восьми тестовых матриц становятся действительными.При 0,0,0 они все становятся матрицами идентичности!Надеюсь, это поможет, это сработало очень хорошо для моего приложения.Брюс обновление После обнаружения проблем с Y = -90 или 90 градусов в решении выше.Я придумал это решение, которое, похоже, воспроизводит матрицу при любых значениях!X = if (или (m31 = 1, m31 = -1), 0, atan (m33 + 1e-24, m32)) Y = -asin (m31) Z = if (или (m31 = 1, m31 = -1), - atan2 (m22, m12), atan2 (m11 + 1e-24, m21)) Я очень долго искал это решение, но оно оказалось очень полезным: o) Надеюсь, это поможет!Bruce

...