% Chapter 3 Section 2 Demos
close all; clear s sd sdd;  

% %%%%%%%%%%%% Section 3.2 Time Varying Coordinate Frames
% Make small rotations about x, y, and z from the identity orientation.
thx = 1; thy = 2; thz = 3;
scale = 1e-3;
Rdelta1 = rotx(thx*scale) * roty(thy*scale) * rotz(thz*scale)
Rdelta2 = roty(thy*scale) * rotx(thx*scale) * rotz(thz*scale)
Rdelta3 = roty(thy*scale) * rotz(thz*scale) * rotx(thx*scale)
delta1 = vex(Rdelta1 - eye(3))
delta2 = vex(Rdelta2 - eye(3))
delta3 = vex(Rdelta3 - eye(3))
fprintf('What about more digits of accuracy?\n\n');

digits(6)
scale = 1e-3;
Rdelta1 = vpa(rotx(thx*scale) * roty(thy*scale) * rotz(thz*scale))
Rdelta2 = vpa(roty(thy*scale) * rotx(thx*scale) * rotz(thz*scale))
Rdelta3 = vpa(roty(thy*scale) * rotz(thz*scale) * rotx(thx*scale))
th = [thx; thy; thz];
delta1 = vpa(vex(Rdelta1 - eye(3)))
delta2 = vpa(vex(Rdelta2 - eye(3)))
delta3 = vpa(vex(Rdelta3 - eye(3)))
diff1 = vpa(vex(Rdelta1 - eye(3)) - th*scale)
diff2 = vpa(vex(Rdelta2 - eye(3)) - th*scale)
diff3 = vpa(vex(Rdelta3 - eye(3)) - th*scale)

% Increasing the sizes of the rotations violates the infinitesimal
% motion assumption.
scale = 1e-2;
Rdelta1 = rotx(thx*scale) * roty(thy*scale) * rotz(thz*scale);
Rdelta2 = roty(thy*scale) * rotx(thx*scale) * rotz(thz*scale);
Rdelta3 = roty(thy*scale) * rotz(thz*scale) * rotx(thx*scale);
delta1 = vpa(vex(Rdelta1 - eye(3)))
delta2 = vpa(vex(Rdelta2 - eye(3)))
delta3 = vpa(vex(Rdelta3 - eye(3)))

scale = 1e-1;
Rdelta1 = rotx(thx*scale) * roty(thy*scale) * rotz(thz*scale)
Rdelta2 = roty(thy*scale) * rotx(thx*scale) * rotz(thz*scale)
Rdelta3 = roty(thy*scale) * rotz(thz*scale) * rotx(thx*scale)
delta1 = vpa(vex(Rdelta1 - eye(3)))
delta2 = vpa(vex(Rdelta2 - eye(3)))
delta3 = vpa(vex(Rdelta3 - eye(3)))

% The same stuff works from arbitrary orientations
fprintf('\nThis still works from an arbitrary orientation!\n\n');
theta = rand(3,1);
R0 = rotz(theta(1)) * rotx(theta(2)) * rotz(theta(3));
scale = 1e-3;
R1 = R0 * rotx(thx*scale) * roty(thy*scale) * rotz(thz*scale)
% The followig delta theta is not the same, since the frames of
% reference are different
del_th = tr2delta(r2t(R0), r2t(R1))
R1_new = t2r( delta2tr(del_th) * r2t(R0))
% but the R1_new obtained is the same as the original R1.
norm(R1 - R1_new)
% Rotate del_th into the original frame to compare
th_new = vpa(R0' * del_th(4:6))
th_new - th * scale

R2 = R0 * roty(thy*scale) * rotx(thx*scale) * rotz(thz*scale);
del_th = tr2delta(r2t(R0), r2t(R2));
R2_new = t2r( delta2tr(del_th) * r2t(R0));
norm(R2 - R2_new)
th_new = vpa(R0' * del_th(4:6))

R3 = R0 * roty(thy*scale) * rotz(thz*scale) * rotx(thx*scale);
del_th = tr2delta(r2t(R0), r2t(R3));
R3_new = t2r( delta2tr(del_th) * r2t(R0));
norm(R3 - R3_new)
th_new = vpa(R0' * del_th(4:6))
