SLERP（球面線形補間：クォータニオンによる大円補間）による補間

swkagami.hatenablog.com

(23)式を使えば、クォータニオンq1からクォータニオンq2への補間位置が逐次的に求められる。

このテクニックは「実践ロボット制御」でも単位クォータニオンを用いた大円補間として紹介されていた。

clear; close all;

origin = [1,0,0]';
destination = [1,1,1]';
q1 = [0,origin(1),origin(2),origin(3)]';
q2 = [0,destination(1),destination(2),destination(3)]';

phi = acos(dot(q1,q2)/(norm(q1)*norm(q2)));

plot3([0,q1(2)],[0,q1(3)],[0,q1(4)]);
hold on;
plot3([0,q2(2)],[0,q2(3)],[0,q2(4)]);

xlim([-2,2]);
ylim([-2,2]);
zlim([-2,2]);

for t=linspace(0,1,10)
    if(t==0)||(t==1)
        continue
    end
    tmp = sin((1-t)*phi)/sin(phi)*q1+sin(t*phi)/sin(phi)*q2;
    px = tmp(2);
    py = tmp(3);
    pz = tmp(4);
    plot3([0,px],[0,py],[0,pz],'k--');
    big;
    drawnow;
    pause(0.3);
end