rot.m

来自「通过给出的不同坐标之间角度」· M 代码 · 共 24 行

function [k,theta]=Rot(R);
 
%The function [k,theta]=Rot(R)is a function to give the angle and general axis K of a giving matrix R;
%  
%The input of the function is a 3*3 matrix R, and the outputs are angle
%"Theta" and general axis "K" (a 3*1 matrix). 
%                
% The function can also check error on wrong size of the matrixes.

R = input('Please input the Rotition matrix, it should be a 2*2 matrix, R = '); %input the R matrix
%Error checking
[a b]= size(R);
if a~=3, 'Error--R should be a 3x3 matrix', return, end;
    if b~=3, 'Error--R should be a 3x3 matrix', return, end; %make sure R is a 3x3 matrix.

%Calculate theta:
theta=a*cos((R(1,1)+R(2,2)+R(3,3)-1)/2)
% Calculate k: 
k(1)=(1/(2*sin(theta)))*(R(3,2)-R(2,3));
k(2)=(1/(2*sin(theta)))*(R(1,3)-R(3,1));
k(3)=(1/(2*sin(theta)))*(R(2,1)-R(1,2));
 
k=[k(1);k(2);k(3)]