% Exercise 3.1.a: function R=getRotationMatrixFromVector(w) length_w = norm(w); w_hat = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0]; R = eye(3,3)+w_hat/length_w*sin(length_w)+(w_hat^2)/(length_w^2)*(1-cos(length_w)); end % Exercise 3.1.b: function w=getVectorFromRotationMatrix(R) length_w = acos((trace(R)-1)/2); if (length_w == 0) w = [0; 0; 0]; else w = 1/(2*sin(length_w))*[R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2)]'*length_w; end end % Exercise 3.1.c: % [ exp[w_hat] (I-exp[w_hat])w_hat+w*w^Tv)/|w|)*v ] % exp[xi_hat] = [ ] % [ 0^T 1 ] function V=getTransformMatrixFromTwistCoord(twCoord) v = twCoord(1:3); w = twCoord(4:6); length_w = norm(w); w_hat = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0]; R = getRotationMatrixFromVector(w); T = ((eye(3,3)-R)*w_hat+w*w')*v/length_w; V = [R, T; zeros(1,3), 1]; end % [ v=(I-exp[w_hat])*w_hat+w*w^T)^{-1}*|w|*T ] % xi = [ ] % [ w = hat^{-1}(log(w)) ] function xi=getTwistCoordFromTransformMatrix(g) R = g(1:3,1:3); % extract rotation matrix T = g(1:3,4); % extract translation part w = getVectorFromRotationMatrix(R); % calc corresponding w testR = getRotationMatrixFromVector(w); w_hat = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0]; % we need w_hat as well v = inv((eye(3,3)-R)*w_hat+w*w')*norm(w)*T; % calc the translation xi = [v; w]; % concatenate into one vector end % Exercise 3.2.a: R = [0.1729 -0.1468 0.9739 ; 0.9739 0.1729 -0.1468 ; -0.1468 0.9739 0.1729]; rotationaxis = getVectorFromRotationMatrix(R); % [1.0472 1.0472 1.0472]' angle = norm(rotationaxis); % 1.8137 % Exercise 3.2.b: [V,D] = eig(R); % [-0.2406 + 0.9706i ; -0.2406 - 0.9706i ; 1] % -0.2887 + 0.5000i -0.2887 - 0.5000i 0.5774 % 0.5774 0.5774 0.5774 % -0.2887 - 0.5000i -0.2887 + 0.5000i 0.5774 % Exercise 3.3: function W = rotation(V,a,b,alpha) % Initialize W: nVertices = size(V,1) W = zeros(nVertices,3); % Translation matrix (translates the model to the point (0,0,0)): T = [eye(3,3), -a; zeros(1,3), 1]; % Translation back to the center of V: Tback = [eye(3,3), a; zeros(1,3), 1]; % Rotation matrices in homegeneuous coordinates: R1 = getRotationMatrixFromVector(b) R = [R1, zeros(3,1) ; zeros(1,3), 1] % Overall transformation matrix: G = Tback * R * T; % Homogeneous coordinates of V: Vh = [V,ones(nVertices,1)]; % Rotate and translate the vertices: Wh_t = G * Vh'; Wh = Wh_t'; % Back transform from homogenous to 3D coordinates: W = Wh(:,1:3);