[RO]ZYZ角的旋转矩阵及角度计算 ZYZ Angle

ZYZ角

1.根据旋转角度求旋转矩阵

参数：

• angle1：绕 z 轴转动的角度
• angle2：绕 y’ 轴转动的角度
• angle3：绕 z’’ 轴转动的角度

function ZYZ_matrix = get_ZYZ_matrix(angle1,angle2, angle3)
%UNTITLED2 Calculate ZYZ matrix according three angles
%Arg:
%   angle1: number type, the angle about axis z
%   angle2: number type, the angle about axis y'
%   angle3: number type, the angle about axis z''

r11 = cos(angle1) * cos(angle2) * cos(angle3) - sin(angle1) * sin(angle3); 
r12 = -cos(angle1) * cos(angle2) * sin(angle3) - sin(angle1) * cos(angle3); 
r13 = cos(angle1) * sin(angle2);
r21 = sin(angle1) * cos(angle2) * cos(angle3) + cos(angle1) * sin(angle3); 
r22 = -sin(angle1) * cos(angle2) * sin(angle3) + cos(angle1) * cos(angle3); 
r23 = sin(angle1) * sin(angle2);
r31 = -sin(angle2) * cos(angle3); 
r32 = sin(angle2) * sin(angle3); 
r33 = cos(angle2);

ZYZ_matrix = [r11 r12 r13; r21 r22 r23; r31 r32 r33];

end

2. 根据ZYZ旋转矩阵，计算各旋转角度

参数：

• r11：旋转矩阵(1, 1)的值
• 其他参数同理

function get_parameter_from_ZYZ(r11, r12, r13, r21, r22, r23, r31, r32, r33)
%UNTITLED3 Get three rotation angles from ZYZ matrix
%Arg:
%   r11: number, the value of matrix which position is (1, 1)
%   same meaning for other arguments.
if((r13 ~= 0) && (r23 ~= 0))
    angle1 = atan2(r23, r13);
    angle2 = atan2((r13^2 + r23^2)^0.5, r33);
    angle3 = atan2(r32, -r31);
    fprintf('angle z is %f and ' , angle1);
    fprintf('angle y'' is %f and ' , angle2);
    fprintf('angle z'''' is %f ' , angle3);
else
    print("r13 and r23 should not be 0")
end
end