クォータニオンと回転行列

ベクトルpを $a=(a_1, a_2, a_3)^T$ の周りに $\theta$ 回転する回転行列は、
$R(\theta) = \begin{pmatrix} C+a_1^2(1-C) & -a_3S + a_1 a_2 (1-C)& a_2 S+ a_1a_3 (1-C)\\ a_3 S + a_1 a_2 (1-C) & C+a_2^2(1-C) & -a_1 S + a_2 a_3 (1-C) \\ -a_2 S+a_1 a_3 (1-C) & a_1 S + a_2 a_3 (1-C) & C+a_3^2(1-C) \end{pmatrix}$
ただし、 $C=\cos(\theta), S=\sin(\theta)$ で、 $|a|=1$ とする。

クォータニオンを用いてpをθ回転することを考える。
クオータニオンqを以下で定義すると、

$qpq^{*}$ でθ回転後のベクトルが求まる。

以下では回転行列による回転とクォータニオンによる回転が一致することを見る。
p=(1,2,3)をa=(2 -2 3)の周りにθ=45°回転する。

close all; clear;

% 回転行列
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(121);
p = [1,2,3]';
o = [0,0,0]';
plot3(0,0,0,'bo');
hold on;
mArrow3(o,p,'color','red');
a_ = [2 -2 3]';
a = a_/norm(a_);
a1=a(1);
a2=a(2);
a3=a(3);
mArrow3(o,a);
theta = pi/4;

C = cos(theta);
S = sin(theta);
R = [C+a1^2*(1-C) -a3*S+a1*a2*(1-C) a2*S+a1*a3*(1-C);
     a3*S+a1*a2*(1-C) C+a2^2*(1-C) -a1*S+a2*a3*(1-C);
     -a2*S+a1*a3*(1-C)  a1*S+a2*a3*(1-C) C+a3^2*(1-C)];
 
p2 = R*p; 
mArrow3(o,p2,'color','blue');
title('Rotation matrix R');

% クォータニオン
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(122);
C2 = cos(theta/2);
S2 = sin(theta/2);
px = p(1);
py = p(2);
pz = p(3);
Q1 = [C2 -a1*S2 -a2*S2 -a3*S2;
      a1*S2 C2 -a3*S2 a2*S2;
      a2*S2 a3*S2 C2 -a1*S2;
      a3*S2 -a2*S2 a1*S2 C2];
P = [0 -px -py -pz;
     px 0 -pz py;
     py pz 0 -px;
     pz -py px 0];
Q2 = [C2 -a1*S2 -a2*S2 -a3*S2]'; 
p3 = Q1*P*Q2;
p3 = p3(2:end);
plot3(0,0,0,'bo');
hold on;
mArrow3(o,p,'color','red');
mArrow3(o,a);
mArrow3(o,p3,'color','blue');
title('quaternion');

赤：回転前のベクトルp
黒：回転軸a
青：回転後のベクトル
f:id:seinzumtode:20200510155831p:plain
ベクトルの描画にはmArrow.mを用いた。
www.mathworks.com