rot2.m

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

function R=Rot2(K,theta);
% The function  R=Rot2(k,theta)is to compute the rotation matrix R.
%
% The inputs of the function are the angle theta and general axis matrix k.
% In which, theta is a radian angle and k is a 3*1 matrix. 
%
% The output is the rotation matrix R, which is a 3x3 matrix.
%        
%The function can also check error on wrong size of the matrixes.
 
theta = input('Please input the angle theta, which should not be 0 or pi, or in the range of (0,2pi). theta = '); %input theta
 K = input('Please input the general axis matirx, which should be a 3x1 matrix. K = ');   %input K
    
%Error checking
 [a b]=size(K);                           
if a~=3, 'Error--K should be a 3x1 matrix', return, end;         
    if b~=1, 'Error--K should be a 3x1 matrix', return, end;                     % make sure K is 3x1 matrix

if  theta==0, 'Error--theta can not be 0', return, end;                           %make sure thera is not 0.
     if  theta==pi, 'Error--theta can not be pi', return, end;                   %make sure thera is not pi.
        if  theta>=2*pi,  'Error--theta can not be more than 2pi', return, end;   %make sure thera is (0,2pi).

%Calculate the rotation matrix
s=sin(theta);
c=cos(theta);
v=1-cos(theta);

R(1,1)= (K(1)*K(1)*v)+c;
R(1,2)= (K(1)*K(2)*v)-K(3)*s;      
R(1,3)= (K(1)*K(3)*v)+K(2)*s;

R(2,1)= (K(1)*K(2)*v)+K(3)*s;
R(2,2)= (K(2)*K(2)*v)+c;       
R(2,3)= (K(2)*K(3)*v)-K(1)*s;

R(3,1)= (K(1)*K(3)*v)-K(2)*s;
R(3,2)= (K(2)*K(3)*v)+K(1)*s;      
R(3,3)= (K(3)*K(3)*v)+c;

R=[R(1,1) R(1,2) R(1,3);R(2,1) R(2,2) R(2,3);R(3,1) R(3,2) R(3,3)]