基本的な回転の表現や変換式をまとめます． *サンプルコード (C++) ライブラリ：[link:simplesp] サンプルコード：simplesp/sample/sp/rot 基本的な回転表現を相互に変換します．{{small:オイラー角はz-y-x系を利用しています．}} *２次元の回転 　2次元のベクトル{$(x,y)$}を角度{$\theta$}だけ回転させたいとします．回転後のベクトル{$(x',y')$}は，次の２ｘ２の回転行列{$R$}に基づいて計算します． {$$\begin{align*} \begin{bmatrix} x'\\y' \end{bmatrix} &= R \begin{bmatrix} x\\y \end{bmatrix} =\begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x\\y \end{bmatrix} \end{align*}$$} 　回転は図１の左のように座標系を固定した見方と，図１の右のように座標軸を逆に回転させる見方があります． [img:3sb7] {{small:図１ ２次元の回転}} 　また，逆行列を掛けると逆回転の変換になります．回転行列は必ず直行行列になるため，転置行列と逆行列は同じ意味になります．{{small:（これは３次元の時も同様です）}} {$$ R^{\mathrm{T}} = R^{-1} $$} *３次元の回転 **3次元の回転行列 ３次元のベクトルを回転させる場合，３ｘ３の回転行列を掛けて回転後のベクトルを計算します． {$$ \begin{bmatrix} x'\\y'\\z' \end{bmatrix} =R \begin{bmatrix} x\\y\\z \end{bmatrix} $$} **各軸周りの回転 １つの座標軸を基準にその周りに回転を加える方法です． ***X軸周りの回転 {$$ \begin{bmatrix} x'\\y'\\z' \end{bmatrix} =\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x\\y\\z \end{bmatrix} =R_X(\theta) \begin{bmatrix} x\\y\\z \end{bmatrix} $$} ***Y軸周りの回転 {$$ \begin{bmatrix} x'\\y'\\z' \end{bmatrix} =\begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & ０ \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} \begin{bmatrix} x\\y\\z \end{bmatrix} =R_Y(\theta) \begin{bmatrix} x\\y\\z \end{bmatrix} $$} ***Z軸周りの回転 {$$ \begin{bmatrix} x'\\y'\\z' \end{bmatrix} =\begin{bmatrix}\cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x\\y\\z \end{bmatrix} =R_Z(\theta) \begin{bmatrix} x\\y\\z \end{bmatrix} $$} **オイラー角 (Euler angles) 各軸周りの回転を順番に掛けることで３次元の回転を表現します． {$$ \begin{bmatrix} x'\\y'\\z' \end{bmatrix} = R_Z(\gamma)R_Y(\beta)R_X(\alpha)\begin{bmatrix} x\\y\\z \end{bmatrix} $$} 上記の式は，掛ける順番からz-y-x系と呼びます．また，{$\alpha, \beta, \gamma$}をオイラー角と呼びます． {{small:z-y-x系以外にz-y-z系なども良く利用されます．利用する系によって式は変わるので注意してください}} {{small,red:2019/10追記：z-y-xの表記の順番については誤りがある可能性があります。z-y-x or x-y-z ?}} ***z-y-x系? サンプルコードで利用している系です． {$$ R_Z(\gamma)R_Y(\beta)R_X(\alpha) = \begin{bmatrix} \cos\beta \cos\gamma & \sin\alpha \sin\beta \cos \gamma - \cos \alpha \sin\gamma & \cos\alpha \sin\beta \cos\gamma + \sin\alpha \sin\gamma \\ \cos\beta \sin\gamma & \sin\alpha \sin\beta \sin\gamma + \cos\alpha \cos\gamma & \cos\alpha \sin\beta \sin\gamma - \sin\alpha \cos\gamma \\ -\sin\beta & \sin\alpha \cos\beta & \cos\alpha \cos\beta \end{bmatrix} $$} ***z-y-z系 wikipedeaで解説されている系です．こちらも良く利用されます． {{small:[link:https://ja.wikipedia.org/wiki/オイラー角] }} {$$ R_Z(\gamma)R_Y(\beta)R_Z(\alpha) = \begin{bmatrix} \cos\alpha \cos\beta \cos\gamma - \sin\alpha \sin\gamma & -\cos\alpha \cos\beta \sin\gamma - \sin\alpha \cos\gamma & \cos\alpha \sin\beta \\ \sin\alpha \cos\beta \cos\gamma + \cos\alpha \sin\gamma & -\sin\alpha \cos\beta \sin\gamma +\cos\alpha \cos\gamma & \sin\alpha \sin\beta \\ -\sin\beta \cos\gamma & \sin\beta \sin\gamma & \cos\beta \end{bmatrix} $$} **クォータニオン 四元数 (Quaternion) ジンバルロックを避けるために利用する４次元で回転を表現する方法です． {$$q = [q_x, q_y, q_z, q_w]$$} {{small:ただし，{$|q| = 1$}を満たすように正規化して数値を保持します．}} 単位ベクトル{$[n_x, n_y, n_z]$} を回転軸として角度{$\theta$}で回転する場合，クォータニオンは次の式で設定します． {$$ \begin{bmatrix} q_x \\ q_y \\ q_z \\ q_w \end{bmatrix} =\begin{bmatrix}\sin(\theta/2) n_x \\ \sin(\theta/2) n_y \\ \sin(\theta/2) n_z \\ \cos(\theta/2) \end{bmatrix} $$} また，クォータニオンを回転行列に変換する場合は次の式に基づいて計算します． {$$ \begin{bmatrix} q_w^2 + q_x^2 - q_y^2 - q_z^2 & 2(q_x q_y - q_w q_z) & 2(q_x q_z + q_w q_y) \\ 2(q_x q_y + q_w q_z) & q_w^2 - q_x^2 + q_y^2 - q_z^2 & 2(q_y q_z - q_w q_x) \\ 2(q_x q_z - q_w q_y) & 2(q_y q_z + q_w q_x) & q_w^2 - q_x^2 - q_y^2 + q_z^2 \end{bmatrix} $$} ***クォータニオンの計算 クォータニオンで保持するパラメータを逆回転にする場合，{$[qx, qy, qz]$}か{$qw$}にー１を掛けます． {$$[-q_x, -q_y, -q_z, q_w], [q_x, q_y, q_z, -q_w]$$} ２つのクォータニオン{$q_0$}，{$q_1$}をかけ合わせる場合，次の計算式を使います． {$$ \begin{bmatrix}q_x \\ q_y \\ q_z \\ q_w \end{bmatrix} = \begin{bmatrix} q_{0w} q_{1x} + q_{0x} q_{1w} + q_{0y} q_{1z} - q_{0z} q_{1y} \\ q_{0w} q_{1y} + q_{0y} q_{1w} + q_{0z} q_{1x} - q_{0x} q_{1z} \\ q_{0w} q_{1z} + q_{0z} q_{1w} + q_{0x} q_{1y} - q_{0y} q_{1x} \\ q_{0w} q_{1w} - q_{0x} q_{1x} - q_{0y} q_{1y} - q_{0z} q_{1z} \end{bmatrix} $$} **ロドリゲスの回転公式 (Rodrigues' rotation formula) ３次元の回転軸を表す単位ベクトル{$[n_x, n_y, n_z]$}と回転量{$\theta$}から，回転行列を作るための公式です． 上記，クォータニオン経由でも同じ数値を計算できます． {$$ \begin{bmatrix} \cos\theta + n_x^2(1 - \cos\theta) & n_x n_y (1 - \cos\theta) - n_z \sin\theta & n_x n_z (1 - \cos\theta) + n_y \sin\theta\\ n_y n_x (1 - \cos\theta) + n_z \sin\theta & \cos\theta + n_y^2(1 - \cos\theta) & n_y n_z (1 - \cos\theta) - n_x \sin\theta\\ n_z n_x (1 - \cos\theta) - n_y \sin\theta & n_z n_y (1 - \cos\theta) + n_x \sin\theta & \cos\theta + n_z^2(1 - \cos\theta) \end{bmatrix} $$}