%% Testing your own code
% rotation matrix - what is wrong, how to check it?

points = [3, 4, 4, 3; 0, 0, 0, 0; 1, 1, 3, 3];
print_points(points);

% first rotation - yaw
% https://en.wikipedia.org/wiki/Rotation_matrix

roll = 0 * pi /180;
pitch = 0 * pi / 180;
yaw = 60 * pi / 180;

x1 = cos(yaw);
x2 = sin(yaw);
x3 = cos(pitch);
x4 = sin(pitch);
x5 = cos(roll);
x6 = sin(roll);

% Rz = [x1,-x2,0;x2,x1,0;0,0,1];
% Ry = [x3,0,x4;0,1,0;-x3,0,x4];
% Rx = [1,0,0;0,x5,-x6;0,x6,x5];
% 
% R = Rz*Ry*Rx;
R = [ x1*x3, x1*x4*x6 - x2*x5, x2*x6 + x1*x4*x5; x2*x3, x1*x5 + ...
x2*x4*x6, x2*x4*x5 - x1*x6; -x3, x3*x6, x3*x5];

% Rotate the points 
points = R * points;

% second rotation - some pitch 
roll = 0 * pi /180;
pitch = 60 * pi / 180;
yaw = 0 * pi / 180;

x1 = cos(yaw);
x2 = sin(yaw);
x3 = cos(pitch);
x4 = sin(pitch);
x5 = cos(roll);
x6 = sin(roll);

R = [ x1*x3, x1*x4*x6 - x2*x5, x2*x6 + x1*x4*x5; ...
x2*x3, x1*x5 + x2*x4*x6, x2*x4*x5 - x1*x6; ...
-x5, x3*x6, x3*x5];

points = R * points;