% These scripts require a bit of preliminary work by the instructor about rigid body motion in three dimensions, a little beyond what people normally see in Linear Algebra. The choice of topics is guided by Chaper 2 of the text by Ma, Soatto, Kosecka and Sastry ``An Invitation to 3-D Vision'' (2004), used by John Swenson in his ME-579 Machine Vision class, Fall 2019, at Washington State University.

% Rotations in space can be implemented by left multiplication by a rotation matrix R. These rotation matrices satisfy R*R'=identity and det(R)=1, and any matrix satisfying these two properties is a rotation matrix.

a=[1;3;-6]
norm(a)

% expm(Skew3(a)) is the matrix that implements rotation around axis a by angle norm(a).

B=expm(Skew3(a))
B*a

c=[a(2);-a(1);0];
c=c/norm(c)

% c is now a unit vector perpendicular to the axis of rotation. B should rotate it by angle norm(a).

ang=acos(c'*(B*c))

norm(a)-2*pi

% The above commands illustrate that B really is the rotation matrix described.

% The matrix B can also be found by ordinary matrix operations without the matrix exponential map, by using versions of the important Rodrigues' formula.

R=RotUsingRodriguesFormula(a)
R-B

% Euler angles (roll, pitch, yaw) can be used to describe a rotation and Euler angles can be calculated from rotations.

% You can read about this in the comments inside the functions EulerAngsFromRot.m and RotFromEulerZYX.m.

[W,K]=EulerAngsFromRot(R)
RotFromEulerZYX(W)
RotFromEulerZYX(K)

% Rigid body transformations correspond to a rotation combined with a translation. These can be represented by a 4x4 matrix with the rotation matrix in the top left block, the translation vector in the top right three spots and the bottom row given by 0 0 0 1. There are lots of reasons why this form is useful, which we do not dwell on here (see the book, Ch.2 above.) 

% The product of any two matrices of this form is another. We note that you can compose these rigid body transformations (one followed by another) by matrix multiplication of these 4x4 matrices.

% Here we are identifying R^3 with the hyperplane in R^4 at height 1 in the 4th coordinate. The 4x4 matrix is a linear transformation on R^4 that acts on this copy of R^3 as a rigid motion there. Note: a rigid motion that involves translation is NOT a linear map on R^3 because it does not send the origin to the origin. We get around that by constructing a linear map on R^4 that agrees with the rigid motion when restricted to the hyperplane at height 1.


S=RigidTransform([1;2;-1],R)
T=RigidTransform(-R'*[1;2;-1],R')

T*S

% T is the inverse rigid transform for S.


% Finally, we discuss visualization of moved and rotated axes.  See the following.

PlotMovingFrame(S)


% Here is an example that creates a movie of a moving frame.

clear('M');
clear('v')

H=@(t) [1 0 0 t;0 1 0 2*t;0 0 1 2*t;0 0 0 1]

v = VideoWriter('MovingFrame.mp4','MPEG-4');
% MPEG-4 is best on Mac

figure('Renderer', 'painters', 'Position', [200 200 900 900])
% distance of screen lower left to figure lower left, followed by pixel size of figure.

t=0:.01:1;
for i=1:length(t)
PlotMovingFrame(H(t(i)))
M(i)=getframe;
end

open(v)
writeVideo(v,M)
close(v)

% ***************************************************

% Now copy and paste the following more interesting example.


clear('M');
clear('v')

G=@(t) [[1;t;2*t;0]/sqrt(1+t^2+4*t^2) [-t;1;0;0]/sqrt(1+t^2) [cross([1;t;2*t]/sqrt(1+t^2+4*t^2) , [-t;1;0]/sqrt(1+t^2));0] [t;cos(2*t);sin(2*t);1]]

v = VideoWriter('MovingFrame2.mp4','MPEG-4');
% MPEG-4 is best on Mac

figure('Renderer', 'painters', 'Position', [200 200 901 901])
% distance of screen lower left to figure lower left, followed by pixel size of figure.

t=0:.01:1;
for i=1:length(t)
PlotMovingFrame(G(t(i)))
M(i)=getframe;
end

open(v)
writeVideo(v,M)
close(v)