%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sample Matlab program for Homework 1.
%
% A Matlab script file that uses the function euler.m 
% and plotVector.m to produce plots of frame vectors
%
% Here's how to get started: 
%        1) Copy myprogram1.m (this file), euler.m and
%           plotVector.m into your working directory.
%        2) Invoke Matlab by typing "matlab" at
%           the unix prompt. 
%        3) Type "myprogram1" in the Matlab window
%           to run this file.
% Note: the program may take a few seconds to run.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

e1 = [1 0 0]' ;    %define e1-coord vector (column) 
e2 = [0 1 0]' ;    %define e2-coord vector (column) 
e3 = [0 0 1]' ;    %define e3-coord vector (column) 

figure(1); clf ;   %create figure window #1 and clear its contents

plot3(0,0,0)   ;   %initialize 3D plot
hold on        ;   %keep adding data to current figure

origin=[0 0 0]' ;             %set origin for plotVector
plotVector(origin,e1,'g') ;   %plot e1 as green line segment
plotVector(origin,e2,'r') ;   %plot e2 as red   line segment
plotVector(origin,e3,'b') ;   %plot e3 as blue  line segment

coordsE = [-0.3 -1.6 -pi/4]' ;  %set Euler coords 

N = 20 ;
for i=0:N
   t = i/N ;                        %current time
   theta = t*coordsE ;              %compute theta(t)
   D = euler(theta)  ;              %evaluate Euler map
   d1=D*e1 ; d2=D*e2 ; d3=D*e3 ;    %compute d-vectors 
   plot3(d1(1),d1(2),d1(3),'gs');   %plot tip of d1 as green square
   plot3(d2(1),d2(2),d2(3),'r+');   %plot tip of d2 as red   cross
   plot3(d3(1),d3(2),d3(3),'bo');   %plot tip of d3 as blue  circle
end

view(-80,40) ;                     %set final view of 3D plot
axis image   ;                     %make plot look nicer
grid on  ;                         %make plot look nicer
hold off ;                         %stop adding data to figure

