matlab

３次元ベクトルの移動を行なう

function [ result ] = rot( mat , deg , axis )
% rot( mat , deg )
% rot( mat , deg , axis )

 if nargin <= 1;    error('引数が足りません');end;
 if nargin == 2;    
     axis = 3;
 end;

 [ num , dim ] = size( mat );
 theta = deg / 180 * pi;

 rotNibble = [ cos( theta )                 , sin( theta )*(-1)^axis,0;
              sin( theta )*(-1)^(axis+1)   , cos( theta ),          0;
              0                             ,0                       ,1];

 rotMat = insertRows( rotNibble , zeros , axis ); 
 rotMat = insertCols( rotMat    , zeros', axis );
 rotMat( axis , axis ) = 1;
 rotMat( 4 ,4 ) = 1;
  
 for i = 1 : num
    matArea = zeros( 4 , 1 );
    matArea( 1:3  ) = mat( i , 1:3 )';
    result( i , : ) = rotMat * matArea;
 end;
end