21/09/11 9:11 PM

O:\1_PHDSTUFF\MAHMAD\MOD_RESEARCH\NAVIGATION\myfun...\TRN.m

1 of 2

function Xout=TRN(varargin) %By: Mohd Yazed Ahmad %this function transforms a given input based on the given transformation mode; %Input1: Xin=[x1 y1 z1; x2 y2 z3; ... ; xn yn zn].'; %Input2: Rotation angle % At this stage this function only accepts rotation mode and x,y values %Input3: mode, 1=Translation, 2= Scaling; 3=Rotation about z axis %Plot mode; Xin=varargin{1}; %theta=varargin{2}; % mode=varargin{3}; plotmode=varargin{4};

if length(Xin(1,:))<3 Xin(:,3)=0; % assign zeros end

%% Rotation mode, about z axis if mode==1; %assign variables txyz=varargin{2}; % if length(txyz)<3 txyz(3)=0; % assign zeros end TrX=[ 1 0 0 txyz(1);... %tramnslation matrix 0 1 0 txyz(2);... 0 0 1 txyz(3);... 0 0 0 1]; for i=1:length(Xin(:,1)) Xtmp(i,:)=(TrX*[Xin(i,1:3), 1].').'; % Translate row by row, we included 1 for rotation end %% assign output Xr=Xtmp(:,1:3); end %% End mode 1

%% Rotation mode, about z axis if mode==3; %assign variables theta=varargin{2}; %theta=-pi/2; RoZ=[cos(theta) -sin(theta) 0 0;... %rotational matrix sin(theta) cos(theta) 0 0;... 0 0 1 0;... 0 0 0 1]; for i=1:length(Xin(:,1)) Xtmp(i,:)=(RoZ*[Xin(i,1:3), 1].').'; % rotate row by row, we included 1 for rotation

21/09/11 9:11 PM end

O:\1_PHDSTUFF\MAHMAD\MOD_RESEARCH\NAVIGATION\myfun...\TRN.m

2 of 2

%% assign output Xr=Xtmp(:,1:3); %{ if length(Xin(1,:))==3 Xr=Xtmp(:,1:3); end if length(Xin(1,:))==2 Xr=Xtmp(:,1:2); end %} end %% End mode 5 %% Rotation mode, about y axis if mode==5; %assign variables theta=varargin{2}; %theta=-pi/2; RoZ=[cos(theta) 0 sin(theta) 0 1 0 -sin(theta) 0 cos(theta) 0 0 0

0;... %rotational matrix 0;... 0;... 1];

for i=1:length(Xin(:,1)) Xtmp(i,:)=(RoZ*[Xin(i,1:3), 1].').'; % rotate row by row, we included 1 for rotation end %% assign output Xr=Xtmp(:,1:3); %{ if length(Xin(1,:))==3 Xr=Xtmp(:,1:3); end if length(Xin(1,:))==2 Xr=Xtmp(:,1:2); end %} end %% End mode 5

%% Plot before and after trasformation if requested if plotmode==1 figure; plot(Xin(:,1),Xin(:,2),':*r'); %before rotation; axis([-20 20 -20 20]); grid on; hold on; Xr=Xtmp(:,1:2); plot(Xr(:,1),Xr(:,2),':*b') %after rotation; end Xout=Xr; end