java - 从 Euler ZXZ 旋转转换为固定轴 XYZ 旋转-6ren

java - 从 Euler ZXZ 旋转转换为固定轴 XYZ 旋转

转载作者：搜寻专家更新时间：2023-10-30 23:00:38

我遇到的问题是，我需要将 XYZ 固定轴旋转转换为关于 Z 的欧拉旋转，然后是 X'，然后是 Z''。

这里是相关的矩阵:

是:

结合起来，作为 Rz(psi) Ry(phi) Rx(theta) = Rxyz(theta,phi,psi);他们给予:

Rxyz: Rxyz

以及我想要的特定欧拉角约定的旋转矩阵；这是:

欧拉: Euler

所以我最初的计划是比较矩阵元素，然后提取我想要的角度；我想出了这个(最后的实际当前代码):

Code

但这在某些情况下不起作用。最明显的是当 Cos(theta)Cos(phi) == 1 时；从那时起 Cos(beta) = 1，因此 Sin[beta] = 0。代码中 Sin(beta) 是 s2。这仅在 Cos(theta) 和 cos(phi) = +/- 1 时发生。

所以我可以马上排除可能的情况；

当 theta 或 phi = 0, 180, 360, 540, ... 时，则 Cos(theta) 和 Cos(phi) 为 +/- 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 的情况。

但是现在就是问题所在了；

我上面所做的一切对我来说都很有意义，但它没有给出正确的角度；

这是一些输出，在每一对中，第一对是 theta phi psi 角，每对的第二对是相应的 alpha beta gamma 线。忽略舍入误差，它似乎让一些角度偏离了大约

[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]