
%
% Quaternion rotation example : 3D rotation
% 
% Chad Jenkins (cjenkins@cs.brown.edu)
% Modified: October 10, 2010
%
% based on pseudocode example from Wikipedia Quaternion entry 
%

 
%inputs
% point to rotate
point = [1 1 0];

% rotation axis and angle
rotation_vector = [0 sin(pi/4) cos(pi/4)];
rotation_angle_final = 3*pi/4;

figure;
plot3(point(1,1),point(1,2),point(1,3),'b.','MarkerSize',25);
hold on;
plot3([0 rotation_vector(1,1)],[0 rotation_vector(1,2)],[0 rotation_vector(1,3)],'r-','LineWidth',10);

%-----

for rotation_angle = 0:rotation_angle_final/100:rotation_angle_final

sin_coeff = sin(rotation_angle*0.5);

% define rotation quaternion as cos(angle/2) + rotation_axis*sin(angle/2)
rotation_quaternion = [cos(rotation_angle*0.5) sin_coeff*rotation_vector];

% make sure rotation quaternion is normalized
rotation_quaternion = rotation_quaternion/norm(rotation_quaternion);

% redefine point to rotate as quaternion 0 + point_vector
point_quaternion = [0 point];

% define the conjugate of the rotation quaternion 
% for a quaternion a + bi, its conjugate is a - bi
rotation_quaternion_conjugate = [rotation_quaternion(1,1) -rotation_quaternion(2:end)];

% perform rotation by conjugation of point_quaternion with rotation_quaternion
% conjugation of a by b is b*a*b^-1, where b^-1 is the conjugate of b

 % first multiply (b*a)
 
temp_point_rotated(1,1) = ...
rotation_quaternion(1,1)*point_quaternion(1,1) ...
-rotation_quaternion(1,2)*point_quaternion(1,2) ...
-rotation_quaternion(1,3)*point_quaternion(1,3) ...
+rotation_quaternion(1,4)*point_quaternion(1,4);
temp_point_rotated(1,2) = ...
rotation_quaternion(1,1)*point_quaternion(1,2) ...
+rotation_quaternion(1,2)*point_quaternion(1,1) ...
+rotation_quaternion(1,3)*point_quaternion(1,4) ...
-rotation_quaternion(1,4)*point_quaternion(1,3);
temp_point_rotated(1,3) = ...
rotation_quaternion(1,1)*point_quaternion(1,3) ...
-rotation_quaternion(1,2)*point_quaternion(1,4) ...
+rotation_quaternion(1,3)*point_quaternion(1,1) ...
+rotation_quaternion(1,4)*point_quaternion(1,2);
temp_point_rotated(1,4) = ...
rotation_quaternion(1,1)*point_quaternion(1,4) ...
+rotation_quaternion(1,2)*point_quaternion(1,3) ...
-rotation_quaternion(1,3)*point_quaternion(1,2) ...
+rotation_quaternion(1,4)*point_quaternion(1,1);


% next multiply by conjugate (b*a)*b^-1
point_rotated_quaternion(1,1) = ...
temp_point_rotated(1,1)*rotation_quaternion_conjugate(1,1) ...
-temp_point_rotated(1,2)*rotation_quaternion_conjugate(1,2) ...
-temp_point_rotated(1,3)*rotation_quaternion_conjugate(1,3) ...
+temp_point_rotated(1,4)*rotation_quaternion_conjugate(1,4);
point_rotated_quaternion(1,2) = ...
temp_point_rotated(1,1)*rotation_quaternion_conjugate(1,2) ...
+temp_point_rotated(1,2)*rotation_quaternion_conjugate(1,1) ...
+temp_point_rotated(1,3)*rotation_quaternion_conjugate(1,4) ...
-temp_point_rotated(1,4)*rotation_quaternion_conjugate(1,3);
point_rotated_quaternion(1,3) = ...
temp_point_rotated(1,1)*rotation_quaternion_conjugate(1,3) ...
-temp_point_rotated(1,2)*rotation_quaternion_conjugate(1,4) ...
+temp_point_rotated(1,3)*rotation_quaternion_conjugate(1,1) ...
+temp_point_rotated(1,4)*rotation_quaternion_conjugate(1,2);
point_rotated_quaternion(1,4) = ...
temp_point_rotated(1,1)*rotation_quaternion_conjugate(1,4) ...
+temp_point_rotated(1,2)*rotation_quaternion_conjugate(1,3) ...
-temp_point_rotated(1,3)*rotation_quaternion_conjugate(1,2) ...
+temp_point_rotated(1,4)*rotation_quaternion_conjugate(1,1);

point_rotated = point_rotated_quaternion(2:4);
plot3(point_rotated(1,1),point_rotated(1,2),point_rotated(1,3),'r.');

end

%----

%plot result
plot3(point_rotated(1,1),point_rotated(1,2),point_rotated(1,3),'r.','MarkerSize',25);
plot3([0 point(1,1)],[0 point(1,2)],[0 point(1,3)],'b-');
plot3([0 point_rotated(1,1)],[0 point_rotated(1,2)],[0 point_rotated(1,3)],'r-');
axis('equal');


