クォータニオンで回転する

回転軸,回転角→クォータニオン

四元数の成分は、回転軸,回転角を単純に格納してものではない!
回転軸、回転角が与えれたら次式のように格納せよ。

$n$ 回転軸
$\theta$ 回転角
(1)
\begin{align} \left( \sin\frac{\theta}{2}n_x,\sin\frac{\theta}{2}n_y,\sin\frac{\theta}{2}n_z,\cos\frac{\theta}{2} \right) \end{align}

結論

ベクトルPを回転軸Rの周りをθ度回転させたいときはこうする。

(2)
\begin{align} P'=\bar{U}PU \end{align}

P'は回転後のベクトル
Pは回転前のベクトル
Uは次式のようなクォータニオン

(3)
\begin{align} U=\cos\frac{\theta}{2}+R\sin\frac{\theta}{2}; R=s_ur_u \end{align}

として展開してみると、任意の軸の周りで回転すると同じカタチになるのだ。
このクォータニオンUに対応する回転行列Tは次のようになる。

(4)
\begin{align} U=U_w+s_xU_x+s_yU_y+s_zU_z=\begin{pmatrix}U_w+iU_z&U_y+iU_x\\ -U_y+iU_x&U_w-iU_z\end{pmatrix} \end{align}

としたとき

(5)
\begin{align} T=\begin{pmatrix}1-2U_y^2-2U_z^2&2U_xU_y+2U_zU_w&2U_zU_x-2U_wU_y\\ 2U_xU_y-2U_zU_w&1-2U_z^2-2U_x^2&2U_yU_z+2U_zU_w\\ 2U_zU_x-2U_wU_y&2U_yU_z&1-2U_x^2-2U_y^2\end{pmatrix} \end{align}

$U_w,U_x,U_y,U_z$の中身は

(6)
\begin{align} U_w=\cos\frac{\theta}{2}\\ U_\mu=r_{\mu}\sin\frac{\theta}{2}; \mu\in\{x,y,z\} \end{align}

ベクトルの回転

外積によるベクトルの回転は次式である。
ベクトルpと回転軸rが直行している場合

(7)
\begin{align} p'=p\cos\theta+r\times p \sin\theta \end{align}

ベクトルpと回転軸rが直行してない場合

(8)
\begin{align} p'=p\cos\theta+r\times p \sin\theta + r\left<r,p\right>(1-\cos\theta) \end{align}

2次元の回転が複素数を使うことによって美しくなったように(結局$\cos\theta+\sin\theta$になった
(1)より)3次元の回転もクォータニオンを使えば美しくまとめることができる。
次のようなクォータニオンUを定義する。

(9)
\begin{align} U=1\cos\theta+R\sin\theta\\ =1\cos\theta+s_ur_u\sin\theta \end{align}

$s_u$がどんなかはquaternion参照。

(10)
\begin{align} =\begin{pmatrix}\cos\theta&0\\ 0&\cos\theta\end{pmatrix}+ \begin{pmatrix}ir_z\sin\theta&r_y\sin\theta+ir_x\sin\theta\\ -r_y\sin\theta+ir_x\sin\theta&-ir_z\sin\theta\end{pmatrix}\\ =\begin{pmatrix} \cos\theta+ir_z\sin\theta&r_y\sin\theta+ir_x\sin\theta\\ -r_y\sin\theta+ir_x\sin\theta&\cos\theta-ir_z\sin\theta\end{pmatrix} \end{align}

今度は単位行列1を省略する。

サンプルソースコード

    public Quaternion rotateVector(float theta, float axisX, float axisY, float axisZ, Quaternion dst) {
        //v=W*v*Wをする。
        Quaternion workquat;
        workquat.x =(-axisX*sin(theta/2));
        workquat.y = (-axisY*sin(theta/2));
        workquat.z = (-axisZ*sin(theta/2));
        workquat.w = (cos(theta/2));
        dst=dst*workquat;
        workquat.x = (axisX*sin(theta/2));
        workquat.y = (axisY*sin(theta/2));
        workquat.z = (axisZ*sin(theta/2));
        workquat.w = (cos(theta/2));
        dst=workquat*dst;
        return dst;
    }
サポートサイト Wikidot.com