Преобразование из эйлерова вращения ZXZ в вращение с фиксированной осью XYZ

Проблема, с которой я столкнулся, заключается в том, что мне нужно преобразовать вращения с фиксированной осью XYZ в вращения Эйлера относительно Z, затем X', затем Z''.

Вот соответствующие матрицы:

X:

Y:

Z:

Комбинированные, как Rz(psi) Ry(phi) Rx(theta) = Rxyz(theta,phi,psi ); они дают:

Rxyz:

И матрицу вращения для конкретного соглашения об углах Эйлера, которые я хочу; вот что:

Эйлер:

Итак, мой первоначальный план состоял в том, чтобы сравнить матричные элементы и таким образом извлечь нужные мне углы; Я придумал это (фактический текущий код в конце):

Но это не работает при некоторых обстоятельствах. Наиболее очевидным является случай, когда Cos(theta)Cos(phi) == 1; так как тогда Cos(beta) = 1, и поэтому Sin[beta] = 0. Где Sin(beta) — это s2 в коде. Это происходит только тогда, когда Cos(theta) и cos(phi) = +/- 1.

Итак, я сразу же могу исключить возможные ситуации;

Когда тета или фи = 0, 180, 360, 540, ..., тогда Cos(тета) и Cos(фи) равны +/- 1;

поэтому мне нужно сделать это по-другому только для этих случаев;

В итоге я получил такой код:

public static double[] ZXZtoEuler(double θ, double φ, double ψ){

    θ *= Math.PI/180.0;
    φ *= Math.PI/180.0;
    ψ *= Math.PI/180.0;

    double α = -1;
    double β = -1;
    double γ = -1;

    double c2 = Math.cos(θ) * Math.cos(φ);

    β = Math.acos(r(c2));

    if(eq(c2,1) || eq(c2,-1)){
        if(eq(Math.cos(θ),1)){
            if(eq(Math.cos(φ),1)){
                α = 0.0;
                γ = ψ;
            }else if(eq(Math.cos(φ),-1)){
                α = 0.0;
                γ = Math.PI - ψ;
            }
        }else if(eq(Math.cos(θ),-1)){
            if(eq(Math.cos(φ),1)){
                α = 0.0;
                γ = -ψ;
            }else if(eq(Math.cos(φ),-1)){
                α = 0.0;
                γ = ψ + Math.PI;
            }
        }
    }else{

        //original way

        double s2 = Math.sin(β);

        double c3 = ( Math.sin(θ) * Math.cos(φ) )/ s2;
        double s1 = ( Math.sin(θ) * Math.sin(ψ) + Math.cos(θ) * Math.sin(φ) * Math.cos(ψ) )/s2;

        γ = Math.acos(r(c3));
        α = Math.asin(r(s1));

    }

    α *= 180/Math.PI;
    β *= 180/Math.PI;
    γ *= 180/Math.PI;

    return new double[] {r(α), r(β), r(γ)};
}

Где r и eq — всего лишь две простые функции;

public static double r(double a){
    double prec = 1000000000.0;
    return Math.round(a*prec)/prec;
}

static double thresh = 1E-4;
public static boolean eq(double a, double b){
    return (Math.abs(a-b) < thresh);
}

eq предназначен только для сравнения чисел для тестов, а r — для предотвращения ошибок с плавающей запятой, выталкивающих числа за пределы диапазона Math.acos / Math.asin и дающих мне результаты NaN;

(то есть время от времени я оказывался с Math.acos(1.000000000000000004) или что-то в этом роде.)

Что учитывает 4 случая вращения вокруг x и y, которые оставляют c2==1.

Но теперь возникает проблема;

Все, что я сделал выше, имеет для меня смысл, но не дает правильных углов;

Вот некоторые выходные данные, в каждой паре первые являются углами тета-фи-пси, а вторые в каждой паре — соответствующие альфа-бета-гамма-линии. Не обращая внимания на ошибки округления, кажется, что некоторые углы смещаются примерно на

[0.0, 0.0, 0.0] - correct!
[0.0, 0.0, 0.0]

[0.0, 0.0, 45.0] - correct!
[0.0, 0.0, 45.0]

[0.0, 0.0, 90.0] - correct!
[0.0, 0.0, 90.0]

[0.0, 0.0, 135.0] - correct!
[0.0, 0.0, 135.0]

[0.0, 0.0, 180.0] - correct
[0.0, 0.0, 180.0]

[0.0, 0.0, 225.0] - correct
[0.0, 0.0, 225.0]

[0.0, 0.0, 270.0] - correct
[0.0, 0.0, 270.0]

[0.0, 0.0, 315.0] - correct
[0.0, 0.0, 315.0]

[0.0, 45.0, 0.0] - incorrect: should be [90, 45, -90]
[90.0, 44.999982, 90.0]

[0.0, 45.0, 45.0]
[45.000018, 44.999982, 90.0]

[0.0, 45.0, 90.0]
[0.0, 44.999982, 90.0]

[0.0, 45.0, 135.0]
[-45.000018, 44.999982, 90.0]

[0.0, 45.0, 180.0]
[-90.0, 44.999982, 90.0]

[0.0, 45.0, 225.0]
[-45.000018, 44.999982, 90.0]

[0.0, 45.0, 270.0]
[0.0, 44.999982, 90.0]

[0.0, 45.0, 315.0]
[45.000018, 44.999982, 90.0]

[0.0, 90.0, 0.0]
[90.0, 90.0, 90.0]

[0.0, 90.0, 45.0]
[45.000018, 90.0, 90.0]

[0.0, 90.0, 90.0]
[0.0, 90.0, 90.0]

[0.0, 90.0, 135.0]
[-45.000018, 90.0, 90.0]

[0.0, 90.0, 180.0]
[-90.0, 90.0, 90.0]

[0.0, 90.0, 225.0]
[-45.000018, 90.0, 90.0]

. Я думаю, это связано с тем, как работают Math.acos и Math.asin. Кто-нибудь может придумать решение?

РЕДАКТИРОВАТЬ: math.asin и math.acos возвращают значения между -pi/2 и pi/2 и 0 и pi соответственно. Это не двусмысленно, поэтому я не думаю, что проблема здесь. Кажется, что я где-то ошибся в математике, но я не вижу дыры в своих рассуждениях...

РЕДАКТИРОВАТЬ 2: Для тех, кто не знает, как работают вращения Эйлера, вот так:

Это то есть повернуть вокруг Z, затем вокруг новой оси X( X'), затем вокруг новой оси Z''.