 % file rot3d_arbline_pre2.m
% version in file "transformations.pdf"
%
% ENGR 150 exercise
% rotation about arbitrary axis
% 3-D
% newpoint = composite transformation T * givenpoint 

% =======================================
% MATLAB HELP
% to learn more about Matlab, type 'intro'
% (without quotes) at the command prompt
% also type 'demo' for more complete intro
% for help on any function, type 'helpwin functionname'
% for help on syntax:
% type 'helpwin punct' 'helpwin ops'  'helpwin arith'
% 'helpwin colon'
% for web-based help, type 'helpdesk'
% keyword search, type 'lookfor keyword'
% type variable names to get their values
% =========================================

% following lecture notes pp 73-75
% wish to rotate R around line PQ by theta CCW
% inputs: givenpoint R(x,y,z), angle theta, and
% line defined by 2 endpoints 
% endpt1 = P(px,py,pz)
% endpt2 = Q(qx,qy,qz)
% output: reflected point R'(x,y,z)

% enter inputs

close all; %close all figures from previous matlab runs 
clear all; % clear all variables from previous matlab runs

%P = endpt1 = [px py pz]; 
%Q = endpt2 = [qx qy qz];
endpt1 = [20 30 0]';
endpt2 = [50 60 20]';
px=endpt1(1);
py=endpt1(2);
pz=endpt1(3);
qx=endpt2(1);
qy=endpt2(2);
qz=endpt2(3);

vline=endpt2-endpt1; %given line
A=vline(1);B=vline(2);C=vline(3);
givenpoint=vline/2+endpt1+[vline(2) -vline(1) 0]'/2;
% for a clear example, we choose givenpoint so that
% a vector from givenpoint to the middle of vline will be
% perpendicular to vline; thus
% dot product of vector and line will be zero


%F=moviein(36);
for rotindex=1:24
theta=rotindex*2*pi/36;
%theta=input('rotation angle  ');
% start transformation
% add extra dimension
%gpoint = [givenpoint ones(15,1)];
gpoint = [givenpoint; ones(1,1)]; %homogeneous coordinates

%use endpt1 endpt2 to define line as a matrix
%example: gline = [2 3;
%                  5 6]; 
gline = [endpt1 endpt2];
ghline = [gline; ones(1,2)]; %homogeneous coordinates

% note that we can write
% endpt1 = gline(:,1)
% is column 1 of matrix gline, 
% for practice, type these variable names
% at the command prompt to see their values

L = norm((endpt2-endpt1)); % length of line

% T1: translation of endpt1 to origin
T1=[ 1  0   0  -px;
     0  1   0  -py;
     0  0   1  -pz;
     0  0   0  1];
  
 % T2: rotate line CCW about x axis into x-z plane
 % T2 = R(x,theta1)
 
 L1 = sqrt((qx-px)^2+(qz-pz)^2);
 theta1=asin((qx-px)/L1);
 V=sqrt(B*B+C*C);
  % sin(theta1)=B/V;
  % cos(theta1)=C/V;
  
 T2 = [1   0    0    0;
       0  C/V  -B/V  0;
       0  B/V   C/V  0;
       0   0    0    1];
       
 % T3: rotate CCW about y-axis CW through theta2 to get it in z direction
 % T3 = R(x,-theta2)
 %  angle is negative since CW rotation
%  sin(theta2)=-A/L
%  cos(theta2)=V/L

T3 = [ V/L 0 -A/L 0;
        0  1   0  0;
       A/L 0  V/L 0;
        0  0   0  1];

 
 % now line is coincident with z axis    
     
 % T4: CCW rotation by theta about line
 T4=[ cos(theta) -sin(theta) 0  0;
      sin(theta)  cos(theta) 0  0;
          0        0        1  0 
          0        0        0  1]; 
       
 % now reverse T3, T2, T1
       
 % T5: R(y,theta2)
 T5 = [ V/L 0 A/L 0;
        0  1   0  0;
       -A/L 0  V/L 0;
        0  0   0  1];

 
% T6: R(x,-theta1)
 
T6 = [1     0    0    0;
       0   C/V   B/V  0;
       0  -B/V   C/V  0;
       0    0    0    1];

    
 % T7: translate back by (px,py,pz)
     
  T7=[ 1  0   0  px;
       0  1   0  py;
       0  0   1  pz;
       0  0  0  1];
   
 % composite transformation
  Tcomp=T7*T6*T5*T4*T3*T2*T1; 
 
 % compute new point
 npoint=Tcomp*gpoint;
 %intermediate points and lines
 npoint1=T1*gpoint;
 npoint2=T2*T1*gpoint;
 npoint3=T3*T2*T1*gpoint;
 npoint4=T4*T3*T2*T1*gpoint;
 ghline1=T1*ghline;
 ghline2=T2*ghline1;
 ghline3=T3*ghline2;
  % eliminate 4th component
  % using []=empty set
 npoint(4,:)=[]; 
 npoint1(4,:)=[];
 npoint2(4,:)=[];
 npoint3(4,:)=[];
 npoint4(4,:)=[];
 ghline1(4,:)=[];
 ghline2(4,:)=[];
 ghline3(4,:)=[];
 
 newpoint=npoint;
 
 % 3-D plot of givenpoint R, line PQ, rotated point R'              
 % gline(:,2) is column 2 of gline with two elements
 % thus plot3 plots two points for each coordinate x,y,z
 % colors r b g y c m w k
 grid on;hold on;   
 
 plot3(gline(1,:),gline(2,:),gline(3,:),'b-');
 plot3(ghline1(1,:),ghline1(2,:),ghline1(3,:),'m-');
 plot3(ghline2(1,:),ghline2(2,:),ghline2(3,:),'y-');
 plot3(ghline3(1,:),ghline3(2,:),ghline3(3,:),'c-');
          
 plot3(givenpoint(1,:),givenpoint(2,:),givenpoint(3,:),'ro');              
 
 plot3(npoint(1,:),npoint(2,:),npoint(3,:),'go');
 plot3(npoint1(1,:),npoint1(2,:),npoint1(3,:),'mo');
 plot3(npoint2(1,:),npoint2(2,:),npoint2(3,:),'yo');
 plot3(npoint3(1,:),npoint3(2,:),npoint3(3,:),'ro');
 plot3(npoint4(1,:),npoint4(2,:),npoint4(3,:),'co');
 
 xlabel('x');ylabel('y');zlabel('z');
 axis([0 60 0 60 -30 30]);
 axis square;
 %axis equal; % 'help axis'
 view(-27,20); % shows viewing angle 'help view'
 %view([60 60 38]);
 %hold off;
 
%F(:,rotindex)=getframe;
end

%movie(F,1)