piao_rodrigues_2.m

根据rodrigues公式求3×3 旋转矩阵 the rodrigues formula: http://en.wikipedia.org/wiki/Rodrigues _rotation_fo

function R = piao_rodrigues_2 (rotation_axis, rotation_angle)

%   Description:  Given the rotation axis and rotation angle
%                 output the rotation matrix  according to the rodrigues formula
%
%   Input:  rotation_axis: ( 3*1 vector  )
%           rotation_angle: (uint: degree , not the radian )  
%
%   Output: R: Rotation Matrix (3x3 matrix)
%        
%         
%   Note:  the rodrigues formula: http://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
%           this piao_rodrigues_2 function is not the same as the piao_rodrigues function
%
%   Author:  piao 
%   Email:   [e-mail]
%   Date:    2008-10-28

%   added:  part of input error reminder (2008-10-31)



%% input error reminder

[size_row,size_column] = size(rotation_axis) ;

if (size_row ~= 3) || (size_column ~= 1)
    
    error('the first input variable must be 3*1 column vector');    
    
end


[size_row,size_column] = size(rotation_angle) ;

if (size_row ~= 1) || (size_column ~= 1)
    
    error('the second input variable must be scalar');    
    
end




%% calculate the Rotation Matrix R (3*3 matrix)


    a=rotation_axis(1);
    b=rotation_axis(2);
    c=rotation_axis(3);
    
     %  antisymmetric matrix 
    v_x = [  0  -c   b;  
             c   0  -a;  
            -b   a   0] ;
                    
          
    
    theta = rotation_angle / 180 * pi ;    % convert the unit from degree to radian  
    
    
    R = cos(theta)*eye(3) + sin(theta) * v_x + (1-cos(theta))*rotation_axis*rotation_axis';
    
    
   %R = cos(theta)*eye(3) + sin(theta)*piao_antisym(rotation_axis ) + (1-cos(theta))*rotation_axis*rotation_axis' ;