Rotating points in 3D using affine transformation in MATLAB

affineRotExample3D.m

function affineRotExample3D(thetaX, thetaY, thetaZ)
% Example of how to rotate an array of points in 3 using affine transformation
%
% function affineRotExample3D(thetaX, thetaY, thetaZ)
%
% Purpose
% Rotate a grid of points in three dimensions about its centre using affine transformation
%
% 
% By default:
% thetaX = 0
% thetaY = 0
% thetaZ = 0.7
%
%
% Example
%
% clf 
% subplot(2,2,1), affineRotExample3D(0.4,0,0), title('Rotation in x')
% subplot(2,2,2), affineRotExample3D(0,0.4,0), title('Rotation in y')
% subplot(2,2,3), affineRotExample3D(0,0,0.7), title('Rotation in z')
% subplot(2,2,4), affineRotExample3D(0.3,0.3,0.5), title('Rotation in all three axes')
%
%
% Further exploration:
% Try changing the values of x_c, y_c, or z_c to alter the position around which the 
% rotation is done. 
%
%
% More details:
% https://www.mathworks.com/matlabcentral/answers/93554-how-can-i-rotate-a-set-of-points-in-a-plane-by-a-certain-angle-about-an-arbitrary-point
% https://people.cs.clemson.edu/~dhouse/courses/401/notes/affines-matrices.pdf
% 
% Rob Campbell - Basel 2017



if nargin<1 || isempty(thetaX)
    thetaX=0;
end

if nargin<2 || isempty(thetaY)
    thetaY=0;
end

if nargin<3 || isempty(thetaZ)
    thetaZ=0.7;
end


% define the x- and y-data for the original line we would like to rotate
[x,y]=meshgrid(-8:8,-8:8);
z=zeros(size(y));



% choose a centre point which will be the center of rotation
x_c = 0;
y_c = 0;
z_c = 0;

% Rotation around the X axis
Rx = [1 0            0           0; ...
      0 cos(thetaX) -sin(thetaX) 0; ...
      0 sin(thetaX)  cos(thetaX) 0; ...
      0 0            0           1];

% Rotation around the Y axis
Ry = [cos(thetaY)  0 sin(thetaY) 0; ...
      0            1 0           0;
      -sin(thetaY) 0 cos(thetaY) 0; ...
      0            0 0           1];


% Rotation around the Z axis
Rz = [cos(thetaZ) -sin(thetaZ) 0 0; ...
     sin(thetaZ)   cos(thetaZ) 0 0; ...
     0            0          1 0; ...
     0            0          0 1];





%Define affine transformation for translation
%(a will translate to this position then c will translate back)
a = [1 0 0 x_c; ...
     0 1 0 y_c; ...
     0 0 1 z_c; ...
     0 0 0 1];

c = [1 0 0 -x_c; ...
     0 1 0 -y_c; ...
     0 0 1 -z_c; ...
     0 0 0 1];



M = a*Rx*Ry*Rz*c; % Produce the affine matrix


rot = zeros(4,prod(size(z))); % The rotated values
orig = zeros(3,prod(size(z))); % The original values
n=1;
for ii=1:size(x)
    for jj=1:size(y)
        rot(:,n) = M*[x(ii,jj) y(ii,jj) z(ii,jj) 1]';
        orig(:,n) =[x(ii,jj) y(ii,jj) z(ii,jj)]';
        n=n+1;
    end
end




% Plot
cla
% pick out the vectors of rotated x- and y-data
x_rot = rot(1,:);
y_rot = rot(2,:);
z_rot = rot(3,:);

hold on
for ii=1:length(rot)
    plot3([orig(1,ii),rot(1,ii)], ...
        [orig(2,ii),rot(2,ii)], ...
        [orig(3,ii),rot(3,ii)], ...
        '-', 'Color', [0.73,0.73,1]);
end


plot3(x_rot,y_rot,z_rot,'.r','MarkerSize',15)
plot3(orig(1,:), orig(2,:), orig(3,:),'.k', 'MarkerSize',10)
view(3)

hold off

xlabel('x'), ylabel('y'), zlabel('z')
axis equal
grid on
box on