備忘録：任意軸の回転行列

回転行列の中身なんて覚えてられないので、ここにもメモ書き。

回転軸の単位ベクトル $\left(x,y,z\right)$ と回転角 $\theta$ では

```math
\left(
\begin{array}{lll}
x\ x\ \left(1 - cos \theta \right) + cos \theta &
x\ y\ \left(1 - cos \theta \right) - z\ sin \theta &
x\ z\ \left(1 - cos \theta \right) + y\ sin \theta \\
y\ x\ \left(1 - cos \theta \right) + z\ sin \theta &
y\ y\ \left(1 - cos \theta \right) + cos \theta &
y\ z\ \left(1 - cos \theta \right) - x\ sin \theta \\
z\ x\ \left(1 - cos \theta \right) - y\ sin \theta &
z\ y\ \left(1 - cos \theta \right) + x\ sin \theta &
z\ z\ \left(1 - cos \theta \right) + cos \theta
\end{array}
\right)
```

クォータニオン $\left(w_q,x_q,y_q,z_q\right) = \left(cos\frac{\theta}{2},\ x\ sin\frac{\theta}{2},\ y\ sin\frac{\theta}{2},\ z\ sin\frac{\theta}{2} \right)$ では

```math
\left(
\begin{array}{lll}
2\ x_q\ x_q + 2\ w_q\ w_q - 1 & 2\ x_q\ y_q - 2\ z_q\ w_q     & 2\ x_q\ z_q + 2\ y_q\ w_q \\
2\ y_q\ x_q + 2\ z_q\ w_q     & 2\ y_q\ y_q + 2\ w_q\ w_q - 1 & 2\ y_q\ z_q - 2\ x_q\ w_q \\
2\ z_q\ x_q - 2\ y_q\ w_q     & 2\ z_q\ y_q + 2\ x_q\ w_q     & 2\ z_q\ z_q + 2\ w_q\ w_q - 1
\end{array}
\right)
```

指数表現( $x^2$ など)は使わず、積( $x\ x$ など)のままにしています。

回転行列から $\theta, \left(x,y,z\right), \left(w_q,x_q,y_q,z_q\right)$ を求めるには、回転行列を

```math
\left(
\begin{array}{ccc}
m11 & m12 & m13 \\
m21 & m22 & m23 \\
m31 & m32 & m33
\end{array}
\right)
```

として

```math
\displaylines {
m11 + m22 + m33 = 1 + 2\ cos \theta =  4 w_q^2 - 1 \\
cos \theta = \frac{m11 + m22 + m33 - 1}{2} \\
\left( sin \theta = \sqrt{1 - cos^2 \theta} \right) \ge 0 \\
\left( w_q = \sqrt{ \frac{m11 + m22 + m33 + 1}{4} } \right) \ge 0
}
```

$cos \theta, sin \theta, w_q$ が分かるので

```math
\displaylines {
\theta = tan^{-1} \frac{sin \theta}{cos \theta} \\

\begin{eqnarray}
sx = \frac{m32-m23}{2} & = & x\ sin \theta = 2\ x_q\ w_q \\
sy = \frac{m13-m31}{2} & = & y\ sin \theta = 2\ y_q\ w_q \\
sz = \frac{m21-m12}{2} & = & z\ sin \theta = 2\ z_q\ w_q \\
\end{eqnarray}

}
```

とすると

```math
\displaylines {

\begin{eqnarray}
v_s & = & \left( sx, sy, sz \right) \\
v_q & = & \left( x_q, y_q, z_q \right) = \frac{v_s}{2\ w_q} \\
\end{eqnarray} \\

\left( x, y, z \right) = \frac{v_s}{\left|v_s\right|} = \frac{v_q}{\left|v_q\right|}

}
```

ですが、$sin\theta=0$ の場合は得られないので

```math
\displaylines {

\begin{eqnarray}
mx = m11-cos\theta & = & x\ x \left( 1-cos\theta \right) \\
my = m22-cos\theta & = & y\ y \left( 1-cos\theta \right) \\
mz = m33-cos\theta & = & z\ z \left( 1-cos\theta \right) \\
ax = \frac{m32+m23}{2} & = & y\ z \left( 1-cos\theta \right) \\
ay = \frac{m13+m31}{2} & = & z\ x \left( 1-cos\theta \right) \\
az = \frac{m21+m12}{2} & = & x\ y \left( 1-cos\theta \right) \\
\end{eqnarray} \\

v_1 = \left( mx, az, ay \right) \\
v_2 = \left( az, my, ax \right) \\
v_3 = \left( ay, ax, mz \right) \\

\left( x, y, z \right) = \frac{v_1}{\left|v_1\right|} = \frac{v_2}{\left|v_2\right|} = \frac{v_3}{\left|v_3\right|} = \frac{v_1+v_2+v_3}{\left|v_1+v_2+v_3\right|}　\\
\left( x_q, y_q, z_q \right) = \left( x, y, z \right)

}
```

としますが、$\left| \left( x, y, z \right) \right|=0$ の場合があります。それは、回転行列が単位行列だから

```math
\displaylines {
\theta = 0 \\
\left( x, y, z \right) = \left( 0, 0, 0 \right) \\
\left( w_q, x_q, y_q, z_q \right) = \left( 1, 0, 0, 0 \right)
}
```

になります。