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

クォータニオンから回転行列を計算することもできる。

clear; close all;
% 回転行列
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
p = [1,2,3]';
o = [0,0,0]';
a_ = [2 -2 3]';
a = a_/norm(a_);
a1=a(1);
a2=a(2);
a3=a(3);
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)];
 
% クォータニオンから回転行列を計算する
C2 = cos(theta/2);
S2 = sin(theta/2);
x = a1*S2;
y = a2*S2;
z = a3*S2;

R2=[1-2*y^2-2*z^2 2*x*y-2*C2*z 2*x*z+2*C2*y;
    2*x*y+2*C2*z 1-2*x^2-2*z^2 2*y*z-2*C2*x;
    2*x*z-2*C2*y 2*y*z+2*C2*x 1-2*x^2-2*y^2];

%Matlabの関数quat2dcmで計算する

R3=quat2dcm([C2 -a1*S2 -a2*S2 -a3*S2]);

R
R2
R3

実行結果

R =
    0.7760   -0.5834   -0.2396
    0.4456    0.7760   -0.4464
    0.4464    0.2396    0.8622
R2 =
    0.7760   -0.5834   -0.2396
    0.4456    0.7760   -0.4464
    0.4464    0.2396    0.8622
R3 =
    0.7760   -0.5834   -0.2396
    0.4456    0.7760   -0.4464
    0.4464    0.2396    0.8622

クォータニオンから回転行列を計算できていることがわかる

クォータニオンの積を公式から計算するよりもわかりやすい
$q$ と $q^{*}$ の順番がこんがらがる（ $q^{*}pq$ なのか $qpq^{*}$ なのか）こともあるので、回転行列にして計算するほうがいいかも。