I have a general 3D rotation matrix like this:
|R11 R12 R13|
|R21 R22 R23|
|R31 R32 R33|
From this rotation matrix, I know I can get the yaw Tait-Bryan Euler angle with this:
yaw = atan2(R21, R11)
In my system I have:
R21 = Asin(r) + Bcos(r)
R11 = Ccos(r) + Dsin(r)
with:
r = RZ - 90deg
A = -0.01775
B = 0.9997
C = -0.01714
D = -0.99924
leading to:
yaw = atan2(-0.01775sin(RZ - 90) + 0.9997cos(RZ - 90) , -0.01714cos(RZ - 90) - 0.99924sin(RZ - 90))
Ex.: if RZ = -136.01355deg, I get yaw = -135.0deg which is what I expect given my system.
Now what I really want to do is the opposite: find RZ given a known yaw value. But for some reason this does not always work. I sometimes am +/- 180deg off. Here is how I re-arranged the equation:
atan2(Asin(r) + Bcos(r), Ccos(r) + Dsin(r)) = Y
(Asin(r) + Bcos(r)) / (Ccos(r) + Dsin(r)) = tan(Y)
Asin(r) + Bcos(r) = tan(Y)Ccos(r) + tan(Y)Dsin(r)
(A - tan(Y)D)sin(r) + (B - tan(Y)C)cos(r) = 0
(A - tan(Y)D)tan(r) + (B - tan(Y)C) = 0
(A - tan(Y)D)tan(r) = tan(Y)C - B
tan(r) = (tan(Y)C - B) / (A - tan(Y)D)
r = atan2(tan(Y)C - B, A - tan(Y)D)
With my system's values:
RZ = atan2(-0.01714tan(yaw) - 0.9997 , 0.99924tan(yaw) - 0.01775) + 90deg
So for yaw = -135deg, I get RZ = 43.98645 which is exactly 180 more than the expected value of -136.01355 (ie it looks like I need to subtract 180 from my result).
It seems that for yaw angles between -90 to 90deg, RZ is good; for yaw < 90deg, I need to do RZ - 180deg; and for yaw > 90deg, I need to do RZ + 180deg.
Does this make sense ? Is my math ok ? Is this solution good and what is the explanation for it ? Is it somehow related to tan(yaw) ? I would definitely prefer a solution that always yields the correct result without the extra step !! Thanks for any insights !!
