浮動小数点演算や乗算命令が無かったり非力だったりするプロセッサ向け
データ形式と演算精度
$\left( -1.0 \sim 1.0 \right)$ の範囲を表すデータは固定小数点とします。例えば
| 符号あり整数部 | 小数部 |
|---|---|
| 2ビット | 14ビット（精度：16384分の1） |
| 4ビット | 12ビット（精度： 4096分の1） |
などです。
角度情報は正規化して $\left( 2\pi \to 1.0 \right)$ とすることで、sin の参照表を $2^n$ 個にして、角度情報と $2^n-1$ の論理積で sin の参照表の配列番号を作ります。（環境によっては、1象限分にしてメモリ使用量を減らしたり、メモリに余裕があれば (4+1=5) 象限分を用意して sin と cos で共通の参照表にするといった工夫もありでしょう）
座標系と回転行列
座標系と回転行列 $T$ は
\begin{eqnarray}
座標系 &：& 右手系 \\
X 軸 &：& 左方向 \\
Y 軸 &：& 上方向 \\
Z 軸 &：& 前方向 \\
\\
yow &=& \theta_{yow} \ (ZX 平面回転) \\
pitch &=& \theta_{pitch} \ (YZ 平面回転) \\
roll &=& \theta_{roll} \ (XY 平面回転) \\
\\
T_{yow} &=& \left(
\begin{array}{ccc}
\cos{\theta_{yow}} & 0 & \sin{\theta_{yow}} \\
0 & 1 & 0 \\
-\sin{\theta_{yow}} & 0 & \cos{\theta_{yow}} \\
\end{array}
\right) \\
\\
T_{pitch} &=& \left(
\begin{array}{ccc}
1 & 0 & 0 \\
0 & \cos{\theta_{pitch}} & -\sin{\theta_{pitch}} \\
0 & \sin{\theta_{pitch}} & \cos{\theta_{pitch}} \\
\end{array}
\right) \\
\\
T_{roll} &=& \left(
\begin{array}{ccc}
\cos{\theta_{roll}} & -\sin{\theta_{roll}} & 0 \\
\sin{\theta_{roll}} & \cos{\theta_{roll}} & 0 \\
0 & 0 & 1 \\
\end{array}
\right) \\
\\
T = T_{yow} T_{pitch} T_{roll} &=& \left(
\begin{array}{ccc}
m_{11} & m_{12} & m_{13} \\
m_{21} & m_{22} & m_{23} \\
m_{31} & m_{32} & m_{33} \\
\end{array}
\right) \\
\\
m_{11} &=& + \cos{\theta_{yow}} \cos{\theta_{roll}} + \sin{\theta_{yow}} \sin{ \theta_{pitch}} \sin{\theta_{roll}} \\
m_{12} &=& - \cos{\theta_{yow}} \sin{\theta_{roll}} + \sin{\theta_{yow}} \sin{ \theta_{pitch}} \cos{\theta_{roll}} \\
m_{13} &=& + \sin{\theta_{yow}} \cos{\theta_{pitch}} \\
\\
m_{21} &=& + \cos{\theta_{pitch}} \sin{\theta_{roll}} \\
m_{22} &=& + \cos{\theta_{pitch}} \cos{\theta_{roll}} \\
m_{23} &=& - \sin{\theta_{pitch}} \\
\\
m_{31} &=& - \sin{\theta_{yow}} \cos{\theta_{roll}} + \cos{\theta_{yow}} \sin{ \theta_{pitch}} \sin{\theta_{roll}} \\
m_{32} &=& + \sin{\theta_{yow}} \sin{\theta_{roll}} + \cos{\theta_{yow}} \sin{ \theta_{pitch}} \cos{\theta_{roll}} \\
m_{33} &=& + \cos{\theta_{yow}} \cos{\theta_{pitch}} \\
\end{eqnarray}
とします。加法定理を使って変形すると
\begin{eqnarray}
m_{11} &=&
\frac{1}{2} \cos{ \left( \theta_{yow} - \theta_{roll} \right) } +
\frac{1}{2} \cos{ \left( \theta_{yow} + \theta_{roll} \right) }
\\
& + &
\frac{1}{4} \sin{ \left( \theta_{yow} - \theta_{pitch} + \theta_{roll} \right) } -
\frac{1}{4} \sin{ \left( \theta_{yow} - \theta_{pitch} - \theta_{roll} \right) }
\\
& + &
\frac{1}{4} \sin{ \left( \theta_{yow} + \theta_{pitch} - \theta_{roll} \right) } -
\frac{1}{4} \sin{ \left( \theta_{yow} + \theta_{pitch} + \theta_{roll} \right) }
\\
\\
m_{12} &=&
\frac{1}{2} \sin{ \left( \theta_{yow} - \theta_{roll} \right) } -
\frac{1}{2} \sin{ \left( \theta_{yow} + \theta_{roll} \right) }
\\
& + &
\frac{1}{4} \cos{ \left( \theta_{yow} - \theta_{pitch} + \theta_{roll} \right) } +
\frac{1}{4} \cos{ \left( \theta_{yow} - \theta_{pitch} - \theta_{roll} \right) }
\\
& - &
\frac{1}{4} \cos{ \left( \theta_{yow} + \theta_{pitch} - \theta_{roll} \right) } -
\frac{1}{4} \cos{ \left( \theta_{yow} + \theta_{pitch} + \theta_{roll} \right) }
\\
\\
m_{13} &=&
\frac{1}{2} \sin{ \left( \theta_{yow} - \theta_{pitch} \right) } +
\frac{1}{2} \sin{ \left( \theta_{yow} + \theta_{pitch} \right) } \\
\\
\\
m_{21} &=&
\frac{1}{2} \sin{ \left( \theta_{pitch} + \theta_{roll} \right) } -
\frac{1}{2} \sin{ \left( \theta_{pitch} - \theta_{roll} \right) } \\
\\
m_{22} &=&
\frac{1}{2} \cos{ \left( \theta_{pitch} + \theta_{roll} \right) } +
\frac{1}{2} \cos{ \left( \theta_{pitch} - \theta_{roll} \right) } \\
\\
m_{23} &=& - \sin{\theta_{pitch}}
\\
\\
\\
m_{31} &=& -
\frac{1}{2} \sin{ \left( \theta_{yow} - \theta_{roll} \right) } -
\frac{1}{2} \sin{ \left( \theta_{yow} + \theta_{roll} \right) }
\\
& + &
\frac{1}{4} \cos{ \left( \theta_{yow} - \theta_{pitch} + \theta_{roll} \right) } -
\frac{1}{4} \cos{ \left( \theta_{yow} - \theta_{pitch} - \theta_{roll} \right) }
\\
& + &
\frac{1}{4} \cos{ \left( \theta_{yow} + \theta_{pitch} - \theta_{roll} \right) } -
\frac{1}{4} \cos{ \left( \theta_{yow} + \theta_{pitch} + \theta_{roll} \right) }
\\
\\
m_{32} &=&
\frac{1}{2} \cos{ \left( \theta_{yow} - \theta_{roll} \right) } -
\frac{1}{2} \cos{ \left( \theta_{yow} + \theta_{roll} \right) }
\\
& - &
\frac{1}{4} \sin{ \left( \theta_{yow} - \theta_{pitch} + \theta_{roll} \right) } -
\frac{1}{4} \sin{ \left( \theta_{yow} - \theta_{pitch} - \theta_{roll} \right) }
\\
& + &
\frac{1}{4} \sin{ \left( \theta_{yow} + \theta_{pitch} - \theta_{roll} \right) } +
\frac{1}{4} \sin{ \left( \theta_{yow} + \theta_{pitch} + \theta_{roll} \right) }
\\
\\
m_{33} &=&
\frac{1}{2} \cos{ \left( \theta_{yow} - \theta_{pitch} \right) } +
\frac{1}{2} \cos{ \left( \theta_{yow} + \theta_{pitch} \right) } \\
\\
\end{eqnarray}
となるので
- 角度は加減算
- $\sin$ と $\cos$ はテーブル参照
- $m_{nn}$ は加減算とシフト
で処理可能になります。
注意：項が多いので誤記があるかもしれません