Проблема, с которой я столкнулся, заключается в том, что мне нужно преобразовать вращения XYZ с фиксированной осью в вращения Эйлера вокруг Z, затем X ', затем Z' '.
Вот соответствующие матрицы:
X: 
Y: 
Z: 
Объединено, как Rz (фунтов на квадратный дюйм) Ry (phi) Rx (тета) = Rxyz (тета, фи, пси);они дают:
Rxyz: 
И матрицу вращения для конкретного соглашения углов Эйлера, которое я хочу;это:
Эйлер: 
Итак, мой первоначальный план состоял в том, чтобы сравнить матричные элементы и извлечь нужные мне углы;Я придумал это (фактический текущий код в конце):
Но это не работает при нескольких обстоятельствах.Наиболее очевидно, когда Cos (тета) Cos (фи) == 1;с тех пор Cos (бета) = 1, и поэтому Sin [бета] = 0. Где Sin (бета) s2 в коде.Это происходит только тогда, когда Cos (тета) и cos (фи) = +/- 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. Может кто-нибудь придумать решение?
EDIT: math.asin и math.acos возвращают значения от -pi / 2 до pi / 2 и 0 и pi соответственно.Это не двусмысленно, поэтому я не думаю, что проблема здесь.Кажется, у меня где-то может быть неправильная математика, но я не вижу дыру в моих рассуждениях ...
РЕДАКТИРОВАТЬ 2: Для всех, кто не знает, как работает ротация Эйлера, это так:
То есть, вращайтесь вокруг Z , затем вокруг новой оси X ( X '), затемвокруг новой оси Z '' .