%%%%%%%%%%%%%%%% %%% SLERP %%%%%% %%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%% 0002 %%%% SLERP %%%%%% 0003 %%%%%%%%%%%%%%%%% 0004 0005 % Sagi Dalyot % 0006 0007 % This routine aims for calculating a unit quaternion, describing a rotation matrix, 0008 % which lies between two known unit quaternions - q1 and q2, 0009 % using a spherical linear interpolation - Slerp. 0010 % Slerp follow the shortest great arc on a unit sphere, 0011 % hence, the shortest possible interpolation path. 0012 % Consequently, Slerp has constant angular velocity, 0013 % so it is the optimal interpolation curve between two rotations. 0014 % (first published by Sheomake K., 1985 - Animating Rotation with Quaternion Curves) 0015 0016 % end of file -> explnation of rotation matrix and quaternions 0017 0018 % in general: 0019 % slerp(q1, q2, t) = q1*(sin(1-t)*teta)/sin(t) + q2*(sin(t*teta))/sin(teta) 0020 % where teta is the angle between the two unit quaternions, 0021 % and t is between [0,1] 0022 0023 % two border cases will be delt: 0024 % 1: where q1 = q2 (or close by eps) 0025 % 2: where q1 = -q2 (angle between unit quaternions is 180 degrees). 0026 % in general, if q1=q2 then Slerp(q; q; t) == q 0027 0028 function [qm] = slerp (qi, qn, t, eps) 0029 0030 % where qi=[w1 x1 y1 z1] - start unit quaternions 0031 % qn=[w2 x2 y2 z2] - end unit quaternions 0032 % t=[0 to 1] 0033 % eps=threshold value 0034 0035 if t==0 % saving calculation time -> where qm=qi 0036 qm=qi; 0037 elseif t==1 % saving calculation time -> where qm=qn 0038 qm=qn; 0039 else 0040 qi = qi(:)'; 0041 qn = qn(:)'; 0042 0043 C=dot(qi, qn); % Calculating the angle beteen the unit quaternions by dot product 0044 0045 teta=acos(C); 0046 0047 if (1 - C) <= eps % if angle teta is close by epsilon to 0 degrees -> calculate by linear interpolation 0048 qm=qi*(1-t) + qn*t; % avoiding divisions by number close to 0 0049 0050 elseif (1 + C) <= eps % when teta is close by epsilon to 180 degrees the result is undefined -> no shortest direction to rotate 0051 q2(1) = qi(4); q2(2) = -qi(3); q2(3)= qi(2); q2(4) = -qi(1); % rotating one of the unit quaternions by 90 degrees -> q2 0052 qm=qi*(sin((1-t)*(pi/2)))+q2*sin(t*(pi/2)); 0053 0054 else 0055 qm=qi*(sin((1-t)*teta))/sin(teta)+qn*sin(t*teta)/sin(teta); 0056 end 0057 end 0058 0059 0060 % eof 0061 % q = [w, (x, y, z)] quaternion definition 0062 % 0063 % where 0064 % R = [1-2*y^2-2*z^2 2*x*y-2*w*z 2*x*z+2*w*y R is function of 3 euler rotation angles 0065 % 2*x*y+2*w*z 1-2*x^2-2*z^2 2*y*z-2*w*x 0066 % 2*x*z-2*w*y 2*y*z+2*w*x 1-2*x^2-2*y^2] 0067 % => R = [M00 M01 M02 0068 % M10 M11 M12 0069 % M20 M21 M22] 0070 % => trace(R) = 4 - 4*(x^2+y^2+z^2), and x^2+y^2+z^2+w^2==1 0071 % => w=(trace)^.5 0072 % => x=(M21-M12)/4*w 0073 % => y=(M02-M21)/4*w 0074 % => x=(M10-M01)/4*w 0075 % => q = [w, (x, y, z)]