姿勢制御

オイラー角 X軸=30度、Y軸=40度、Z軸=45度
物体座標系でXYZ型で回転する例
この動画は計算の手順を説明するもので実際の計算は最初の位置から最後の位置（赤）を直接算出します

XYZの順番で回転するイメージ

オイラー角

紙飛行機の先端方向をX軸として翼方向をY軸、垂直翼方向をZ軸として考えます
姿勢を変えるにあたり、XYZそれぞれの軸を回転して姿勢を変えるとして、X,Y,Zの回転角度をまとめてオイラー角と呼びます

オイラー角の型

オイラー角は回転する順番によって12種類の型があり、回転する順番が変わると算出結果も変わります

X Y Z	X Y X
X Z Y	X Z X
Y X Z	Y X Y
Y Z X	Y Z Y
Z X Y	Z X Z
Z Y X	Z Y Z

物体座標系と固定座標系

物体座標系は紙飛行機の先端が常にX軸方向というように物体の座標系で計算し、固定座標系は座標軸周りで計算します
このライブラリでは物体座標系はEulerToMatrix, matRotate3Dを使って、固定座標系ではmatRotateXYZで計算できると思います

注意する点

オイラー角での変換はシンバルロックが発生することがあります
シンバルロックとは座標変換の際に２つの回転軸が同じ向きになってしまい３軸が２軸になってしまい無効になった１軸が回転できなくなる現象のことです
これを回避するために回転する順場を変える事で回避することになります

紙飛行機について

紙飛行機は５点の回転の計算をして図形を回転しています

ちなみに

この動画は説明用にPythonで作成したグラフを撮影したものです

クォータニオン

回転軸（３次元ベクトル）と回転角からなる４つの成分で回転を表現します
オイラー角とは違いシンバルロックが発生することはありません
また、クォータニオンは計算がシンプルなので計算コストが削減され高速に計算することができます

考え方

現在の姿勢から目的の姿勢にするためには回転する軸と回転角度を決定すれば目的の姿勢に回転することができるというものです
目的の姿勢がどんな向きでも可能です
１点の回転を考えると次の図のようになります

紙飛行機の５点を回転軸を中心にそれぞれ回転すると以下のようになります

回転軸と回転角度の求め方

初期位置のクォータニオン \[ Q1= \begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \\ \end{bmatrix} = \begin{bmatrix} 1 \\ point.x \\ point.y \\ point.z \\ \end{bmatrix} \] 目標位置のオイラー角をクォータニオンに変換 \[ Q2=EulerToQuaternion(th, order) \] 回転軸（単位ベクトル） \[ \begin{align} V=&arcsin(Q2.img)\times2 \\ T=&\frac{V}{norm(V)} \end{align} \] 回転を表すクォータニオン \[ \begin{align} R.real=&cos\frac{Q2.real}{2} \tag1 \\ R.imag=&T\times sin\frac{Q2.real}{2}\tag2 \\ \end{align} \] 回転の計算
\[ out = R \times Q1 \times \bar{R} \] (1)・・・回転角度
(2)・・・回転軸