ロドリゲスの公式による回転とクォータニオンによる回転

ロドリゲスの公式とクォータニオンによる回転の両方を実装する
クォータニオンには主にハミルトン方式とJPL方式の２つの定義があるが、ハミルトンを採用する。

最初に間違えてクォータニオンによる回転をq⊗rとしていた。qは回転を表すクォータニオン、rは位置ベクトルを表すクォータニオン。正しくはq⊗r⊗q*とすべき（q*は共役クォータニオン）。

ロドリゲスの公式による回転: 回転軸nは単位ベクトル

クォータニオンによる回転: 回転軸nは単位ベクトル


クォータニオンの積

clear; close all;

x=1;
y=1;
z=1;
P = [x;y;z];
O = [0;0;0];
vec = [O,P];
plot3(vec(1,:),vec(2,:),vec(3,:),'magenta','LineWidth',3);
title('ロドリゲスの公式による回転');
hold on;

Xaxis = [1,0,0]';
Yaxis = [0,1,0]';
Zaxis = [0,0,1]';
OX = [O,Xaxis];
OY = [O,Yaxis];
OZ = [O,Zaxis];
plot3(OX(1,:),OX(2,:),OX(3,:),'r');
plot3(OY(1,:),OY(2,:),OY(3,:),'g');
plot3(OZ(1,:),OZ(2,:),OZ(3,:),'b');
xlim([-2,2]);
ylim([-2,2]);
zlim([-2,2]);

view(180,0);

for theta=linspace(0,2*pi,10)
    if (theta==0) || (theta==2*pi)
        continue
    end
    a = [0,1,0]';
    n = a/norm(a);
    nx = n(1);
    ny = n(2);
    nz = n(3);
    R = rodriguez(nx,ny,nz,theta);
    P2 = R*P;
    vec2 = [O,P2];
    plot3(vec2(1,:),vec2(2,:),vec2(3,:),'k--');
    big;
    drawnow;

    pause(0.3);
end


figure;
plot3(vec(1,:),vec(2,:),vec(3,:),'magenta','LineWidth',3);
title('クォータニオンによる回転');
hold on;

Xaxis = [1,0,0]';
Yaxis = [0,1,0]';
Zaxis = [0,0,1]';
OX = [O,Xaxis];
OY = [O,Yaxis];
OZ = [O,Zaxis];
plot3(OX(1,:),OX(2,:),OX(3,:),'r');
plot3(OY(1,:),OY(2,:),OY(3,:),'g');
plot3(OZ(1,:),OZ(2,:),OZ(3,:),'b');
xlim([-2,2]);
ylim([-2,2]);
zlim([-2,2]);

view(180,0);
for theta=linspace(0,2*pi,10)
    if (theta==0) || (theta==2*pi)
        continue
    end
    a = [0,1,0]';
    n = a/norm(a);
    q = rotate(P,n(1),n(2),n(3),theta);
    P2 = q(2:4);
    vec2 = [O,P2];
    plot3(vec2(1,:),vec2(2,:),vec2(3,:),'k--');
    big;
    drawnow;

    pause(0.3);
end


function R = rodriguez(nx,ny,nz,theta)
C = cos(theta);
S = sin(theta);
R = [nx^2*(1-C)+C     nx*ny*(1-C)-nz*S nz*nx*(1-C)+ny*S;
    nx*ny*(1-C)-nz*S ny^2*(1-C)+C     ny*nz*(1-C)-nx*S;
    nz*nx*(1-C)-ny*S ny*nz*(1-C)+nx*S nz^2*(1-C)+C     ];
end


function q = rotate(P,nx,ny,nz,theta)  
  x = P(1);
  y = P(2);
  z = P(3);
  q1 = [0,x,y,z]';
  q2 = [cos(theta/2),nx*sin(theta/2),ny*sin(theta/2),nz*sin(theta/2)]';
  q3 = [cos(theta/2),-nx*sin(theta/2),-ny*sin(theta/2),-nz*sin(theta/2)]';
  qdash =  MultQuat(q2,q1);  
  q = MultQuat(qdash,q3);
end

function q = MultQuat(q1,q2)
  pw = q1(1);
  px = q1(2);
  py = q1(3);
  pz = q1(4);
  qw = q2(1);
  qx = q2(2);
  qy = q2(3);
  qz = q2(4);  
  q = [pw*qw-px*qx-py*qy-pz*qz;
       pw*qx+px*qw+py*qz-pz*qy;
       pw*qy-px*qz+py*qw+pz*qx;
       pw*qz+px*qy-py*qx+pz*qw]; 
end