クォータニオンの個人的整理

#概要
クォータニオンを使うとき、

* 回転行列はどうなったけ？
* 掛け算どうやったけ？
* これは座標系(観察者)を回す？ベクトルを回す？　　->　結論：ベクトルを回す（座標系のみなら、回転方向を逆にすればよい）
* 回転行列への変換と、座標系は回転後・回転前どっちに変換する？

等で悩むので、一度まとめます。検証のために、数値実験も合わせます

###補足：座標系のみなら、回転方向を逆にすればよい
x軸で90度回転、ベクトルがy方向の時、
座標系が回転する場合、ベクトルは回転後の座標系では-z方向を向いている（ベクトルが動かないとき）。
ベクトルが回転する場合、ベクトルはz方向を向いている。

任意の方向に対して、同様の考察をすると、座標系か、ベクトル自体かは、回転方向が逆になるだけである。

ただ、正しく認識していないと、結果が逆方向になってしまう可能性があるということである。

###さらに補足
一般的な力学モデリングでは、本体の座標系と絶対座標系があり、本体の座標系が回転します。
ただ、上の補足と違い、座標系につられてベクトルも回転するため、ベクトルが回転するといえます。
本体の座標系から、ベクトルを見ると定ベクトルになるぐらいで、絶対座標系から見れば回転しているからです。

どの座標系で表現されるかも、クォータニオンでは重要です


#クォータニオン
一般に、
$(l,m,n)^T$軸周りに、$\phi$回転させるクォータニオンは

```math
q = \begin{pmatrix}
cos(\frac{\phi}{2}  )\\
l*sin(\frac{\phi}{2}) \\
n*sin(\frac{\phi}{2}) \\
m*sin(\frac{\phi}{2}) \\
\end{pmatrix}
```

で計算されます。

#クォータニオンの掛け算

```math
q=\begin{pmatrix}
q0\\
q1\\
q2\\
q3\\
\end{pmatrix}\\

p=\begin{pmatrix}
p0\\
p1\\
p2\\
p3\\
\end{pmatrix}\\

```
と二つのクォータニオンを定義したとき、二つの積は


```math
q*p = 
\begin{pmatrix}
q0 &-q1 &-q2&-q3\\
q1&q0&-q3&q2\\
q2&q3&q0&-q1\\
q3&-q2&q1&q0\\
\end{pmatrix}
\begin{pmatrix}
p0\\
p1\\
p2\\
p3\\
\end{pmatrix}\\
```
の行列積に変形できます。

```math
q*p = 
\begin{pmatrix}
p0&-p1&-p2&-p3\\
p1&p0&p3&-p2\\
p2&-p3&p0&p1\\
p3&p2&-p1&p0\\
\end{pmatrix}
\begin{pmatrix}
q0\\
q1\\
q2\\
q3\\
\end{pmatrix}\\
```
にもできます。
#ベクトルの回転
ベクトル$v$がクォータニオン$q$によって回転するとき、

```math
q*v* \bar{q}
```
で計算できます。
ただし、

```math
v= \begin{pmatrix}
0\\
v_x\\
v_y\\
v_z\\
\end{pmatrix}\\
```
とし、

```math
\bar{q} = \begin{pmatrix}
cos(-\frac{\phi}{2}  )\\
l*sin(-\frac{\phi}{2}) \\
n*sin(-\frac{\phi}{2}) \\
m*sin(-\frac{\phi}{2}) \\
\end{pmatrix}\\
=
\begin{pmatrix}
cos(\frac{\phi}{2}  )\\
-l*sin(\frac{\phi}{2}) \\
-n*sin(\frac{\phi}{2}) \\
-m*sin(\frac{\phi}{2}) \\
\end{pmatrix}\\
```
で第2,3,4要素の正負が反転したクォータニオンを使いました。

#回転行列
クォータニオン$q$によって、回転した後のベクトルを$v_{after}$とすると

```math
v_{after} = Cv
```
と行列積で計算できます。この時、回転行列$C$は

```math
C=\begin{pmatrix}
q_w^2+q_x^2-q_y^2-q_z^2&2(q_x*q_y-q_z*q_w)&2(q_z*q_x+ q_y*q_w) \\
2(q_x*q_y + q_z*q_w) & q_w^2-q_x^2+ q_y^2-q_z^2  & 2(q_y*q_z -  q_x*q_w)\\
2(q_z*q_x-q_y*q_w) & 2(q_y*q_z+ q_x*q_w)  & q_w^2 - q_x^2 - q_y^2 + q_z^2\\

\end{pmatrix}\\
```
となります。



#数値検証
$x$軸で90度回転$q_x$
$y$軸で90度回転$q_y$
$z$軸で90度回転$q_z$
それぞれ、を考えてみます。

```
px= [[0.70710678]
 [0.70710678]
 [0.        ]
 [0.        ]]
py= [[0.70710678]
 [0.        ]
 [0.70710678]
 [0.        ]]
pz= [[0.70710678]
 [0.        ]
 [0.        ]
 [0.70710678]]
```
になります。

まず、回転行列は

```
qx
 [[ 1.  0.  0.]
 [ 0.  0. -1.]
 [ 0.  1.  0.]]
qy
 [[ 0.  0.  1.]
 [ 0.  1.  0.]
 [-1.  0.  0.]]
qz
 [[ 0. -1.  0.]
 [ 1.  0.  0.]
 [ 0.  0.  1.]]
```
正しく、回転行列でています

次に、$q*e_i* \bar{q},e_{i}:単位ベクトル$で正しく回転するか確認します。

```
qx,ex
 [[0.]
 [1.]
 [0.]
 [0.]]
qx,ey
 [[0.]
 [0.]
 [0.]
 [1.]]
qx,ez
 [[ 0.]
 [ 0.]
 [-1.]
 [ 0.]]
qy,ex
 [[ 0.]
 [ 0.]
 [ 0.]
 [-1.]]
qy,ey
 [[0.]
 [0.]
 [1.]
 [0.]]
qy,ez
 [[0.]
 [1.]
 [0.]
 [0.]]
qz,ex
 [[0.]
 [0.]
 [1.]
 [0.]]
qz,ey
 [[ 0.]
 [-1.]
 [ 0.]
 [ 0.]]
qz,ez
 [[0.]
 [0.]
 [0.]
 [1.]]
```
全て正しく、計算できています。以上より、数値の確認が取れました。

#プログラム

```Python
import numpy as np


def quatanion(w,l,m,n):
    lam = np.array([l,m,n])
    norm = np.linalg.norm(lam)
    lam = lam/norm
    
    quatanion = np.append(np.cos(w/2),lam*np.sin(w/2))
    return quatanion[:,np.newaxis]

def cross(q,p)    :
    
    D = np.array([[q[0][0],-q[1][0],-q[2][0],-q[3][0]],
                  [q[1][0],q[0][0],-q[3][0],q[2][0]],
                  [q[2][0],q[3][0],q[0][0],-q[1][0]],
                  [q[3][0],-q[2][0],q[1][0],q[0][0] ]   ])
    
    
    #print(D.shape)
    #print(D)
    return np.dot(D,p)

def bar(q):
    hoge = np.array([q[0][0],-q[1][0],-q[2][0],-q[3][0]])

    return hoge[:,np.newaxis]    

def COA(q):
    q0 = q[0][0]
    q1 = q[1][0]
    q2 = q[2][0]
    q3 = q[3][0]
    hoge = np.array([[q0**2+q1**2-q2**2-q3**2,2*(q1*q2-q3*q0),2*(q3*q1+q2*q0)],
                     [2*(q1*q2+q3*q0),q2**2-q3**2-q1**2+q0**2,2*(q2*q3-q1*q0)],
                     [2*(q3*q1-q2*q0),2*(q2*q3+q1*q0),q3**2-q1**2-q2**2+q0**2]])
    return hoge
    
"""
#回転行列チェック
print("--------")
x= quatanion(90/180*np.pi,1,0,0)
y= quatanion(90/180*np.pi,0,1,0)
z= quatanion(90/180*np.pi,0,0,1)

C = COA(x)
print("qx\n",C)

C = COA(y)
print("qy\n",C)

C = COA(z)
print("qz\n",C)
"""


#crossチェック
print("--------")
qx= quatanion(90/180*np.pi,1,0,0)
qy= quatanion(90/180*np.pi,0,1,0)
qz= quatanion(90/180*np.pi,0,0,1)

ex= np.array([0,1,0,0])
ex = ex[:,np.newaxis]    

ey= np.array([0,0,1,0])
ey = ey[:,np.newaxis]  

ez= np.array([0,0,0,1])
ez = ez[:,np.newaxis]  

print("qx,ex\n",cross(cross(qx,ex),bar(qx)))
print("qx,ey\n",cross(cross(qx,ey),bar(qx)))
print("qx,ez\n",cross(cross(qx,ez),bar(qx)))


print("qy,ex\n",cross(cross(qy,ex),bar(qy)))
print("qy,ey\n",cross(cross(qy,ey),bar(qy)))
print("qy,ez\n",cross(cross(qy,ez),bar(qy)))

print("qz,ex\n",cross(cross(qz,ex),bar(qz)))
print("qz,ey\n",cross(cross(qz,ey),bar(qz)))
print("qz,ez\n",cross(cross(qz,ez),bar(qz)))


```

#参考

https://www.mss.co.jp/technology/report/pdf/18-07.pdf