多面体の慣性テンソル

# 慣性テンソルとは
運動している剛体の原点まわり角運動量は$\boldsymbol{I}\boldsymbol{\omega}$で表せます。$\boldsymbol{\omega}$は角速度です。また$\boldsymbol{I}$は次式で定義されます。
```math
\boldsymbol{I}
=-\int_{\boldsymbol{p}\in V}\rho[\boldsymbol{p}\times]^{2}\mathrm{d}v
=\int_{\boldsymbol{p}\in V}\rho\left[\begin{array}{ccc}
 y^2+z^2 & -xy & -zx \\
 -xy & z^2+x^2 & -yz \\
 -zx & -yz & x^2+y^2
\end{array}\right]\mathrm{d}v
```
ただし$\rho$は剛体の密度、$\mathrm{d}v$は無限小体積要素、$\boldsymbol{p}=[x~y~z]^{\mathrm{T}}$は剛体内部の点、$[\boldsymbol{p}\times]$は$\boldsymbol{p}$の外積行列、$V$は剛体の内部領域です。これは**慣性テンソル**と呼ばれます（なぜ行列ではなくテンソルと呼ぶのか、という説明は割愛します）。

この定義に基づいて、任意の閉じた密度一定の多面体の慣性テンソルの計算方法を導出するのが本記事の趣旨です。

# 原点を頂点に持つ四面体の慣性テンソル
今、四つの頂点のうち一つが原点と一致している四面体を考えます。残り三つの頂点の座標を$\boldsymbol{p}\_{1}$、$\boldsymbol{p}\_{2}$、$\boldsymbol{p}\_{3}$とおくと、この四面体の内部領域は次のように表せます。
```math
V=\left\{\boldsymbol{p}|\boldsymbol{p}=\boldsymbol{p}_{1}t_{1}+\boldsymbol{p}_{2}t_{2}+\boldsymbol{p}_{3}t_{3}, 0\leq t_{1}\leq 1, 0\leq t_{2}\leq 1, 0\leq t_{3}\leq 1, 0\leq t_{1}+t_{2}+t_{3}\leq 1 \right\}
```
また体積要素は
```math
\mathrm{d}v=\mathrm{d}x\mathrm{d}y\mathrm{d}z=[\boldsymbol{p}_{1} \boldsymbol{p}_{2} \boldsymbol{p}_{3}]\mathrm{d}t_{1}\mathrm{d}t_{2}\mathrm{d}t_{3}
```
となります。ただし$[\boldsymbol{p}\_{1} \boldsymbol{p}\_{2} \boldsymbol{p}\_{3}]$は**Grassmannの記号**と呼ばれ、次式で定義されます。
```math
[\boldsymbol{p}_{1} \boldsymbol{p}_{2} \boldsymbol{p}_{3}]
=\boldsymbol{p}_{1}^{\mathrm{T}}(\boldsymbol{p}_{2}\times\boldsymbol{p}_{3})
=\boldsymbol{p}_{2}^{\mathrm{T}}(\boldsymbol{p}_{3}\times\boldsymbol{p}_{1})
=\boldsymbol{p}_{3}^{\mathrm{T}}(\boldsymbol{p}_{1}\times\boldsymbol{p}_{2})
```
念のため、これはスカラーで、原点と$\boldsymbol{p}\_{1}$、$\boldsymbol{p}\_{2}$、$\boldsymbol{p}\_{3}$をそれぞれ結ぶ線分を三辺とする**平行6面体の体積**になります。これ重要です。

これらを慣性テンソルの定義式に当てはめれば、
```math
\boldsymbol{I}
=\int_{0}^{1}\int_{0}^{1-t_{3}}\int_{0}^{1-t_{2}-t_{3}}\rho\,\boldsymbol{J}[\boldsymbol{p}_{1} \boldsymbol{p}_{2} \boldsymbol{p}_{3}]\,\mathrm{d}t_{1}\mathrm{d}t_{2}\mathrm{d}t_{3}
```
ただし
```math
\begin{align}
\boldsymbol{J}
&=
\left[(\boldsymbol{p}_{1}t_{1}+\boldsymbol{p}_{2}t_{2}+\boldsymbol{p}_{3}t_{3})\times\right]^{2}
\\\
&=
-\sum_{i=1,2,3}\left[\boldsymbol{p}_{i}\times\right]^{2}t_{i}^{2}
-\sum_{(i,j)=(1,2),(2,3),(3,1)}\left(\left[\boldsymbol{p}_{i}\times\right]\left[\boldsymbol{p}_{j}\times\right]+\left[\boldsymbol{p}_{j}\times\right]\left[\boldsymbol{p}_{i}\times\right]\right)t_{i}t_{j}
\end{align}
```
となります。ここで、
```math
\begin{align}
\int_{0}^{1}\int_{0}^{1-t_{3}}\int_{0}^{1-t_{2}-t_{3}}t_{i}^{2}\mathrm{d}t_{1}\mathrm{d}t_{2}\mathrm{d}t_{3}
&=\frac{1}{60}\qquad\mbox{for}\quad \forall i=1,2,3
\\
\int_{0}^{1}\int_{0}^{1-t_{3}}\int_{0}^{1-t_{2}-t_{3}}t_{i}t_{j}\mathrm{d}t_{1}\mathrm{d}t_{2}\mathrm{d}t_{3}
&=\frac{1}{120}
\qquad\mbox{for}\quad \forall (i,j)=(1,2),(2,3),(3,1)
\end{align}
```
なので結局、
```math
\boldsymbol{I}
=-\frac{1}{60}\rho\left\{
\sum_{i=1,2,3}\left[\boldsymbol{p}_{i}\times\right]^{2}
+\frac{1}{2}
\sum_{(i,j)=(1,2),(2,3),(3,1)}\left(\left[\boldsymbol{p}_{i}\times\right]\left[\boldsymbol{p}_{j}\times\right]+\left[\boldsymbol{p}_{j}\times\right]\left[\boldsymbol{p}_{i}\times\right]\right)
\right\}[\boldsymbol{p}_{1} \boldsymbol{p}_{2} \boldsymbol{p}_{3}]
```
となります。[Zeo](https://github.com/mi-lib/zeo)での実装は次のようになってます。

```C
/* inertia tensor of a cone. */
zMat3D *zTri3DConeInertia(zTri3D *t, double density, zMat3D *i)
{
  register int j;
  zMat3D m;
  zVec3D *v1, *v2;

  zMat3DZero( i );
  for( j=0; j<3; j++ ){
    v1 = zTri3DVert(t,j);
    v2 = zTri3DVertNext(t,j);
    _zVec3DTripleProd2Mat3D( v1, v1, &m );
    _zMat3DSubDRC( i, &m );
    _zVec3DTripleProd2Mat3D( v1, v2, &m );
    _zMat3DMulDRC( &m, 0.5 );
    _zMat3DSubDRC( i, &m );
    _zVec3DTripleProd2Mat3D( v2, v1, &m );
    _zMat3DMulDRC( &m, 0.5 );
    _zMat3DSubDRC( i, &m );
  }
  zMat3DMulDRC( i, density * zVec3DGrassmannProd( zTri3DVert(t,0), zTri3DVert(t,1), zTri3DVert(t,2) ) / 60 );
  return i;
}
```

# Grassmannの記号の意味についてちょっと注釈
$[\boldsymbol{p}\_{1} \boldsymbol{p}\_{2} \boldsymbol{p}\_{3}]$は原点と$\boldsymbol{p}\_{1}$、$\boldsymbol{p}\_{2}$、$\boldsymbol{p}\_{3}$をそれぞれ結ぶ線分を三辺とする平行6面体の体積になると書きました。しかしベクトルの外積の性質から、$[\boldsymbol{p}\_{1} \boldsymbol{p}\_{2} \boldsymbol{p}\_{3}]=-[\boldsymbol{p}\_{1} \boldsymbol{p}\_{3} \boldsymbol{p}\_{2}]$となることがすぐ分かります。つまり、頂点の順番によって正負が入れ替わることになります。体積ならば値が負になるのはおかしいです。

ここには、「$\boldsymbol{p}\_{1}$、$\boldsymbol{p}\_{2}$、$\boldsymbol{p}\_{3}$をこの順で辿ったときに右ねじの法則によって決まる三角形の法線方向が多面体の外側を向く」ということが暗黙の前提にあります。もしこの逆、つまり右ねじの法則で決まる三角形の法線方向が多面体の内側を向く（あるいは左ねじの法則で決まる三角形の法線方向が多面体の外側を向く）ならば、$[\boldsymbol{p}\_{1} \boldsymbol{p}\_{2} \boldsymbol{p}\_{3}]$は負の値になります。

このことを逆に利用すれば、任意の多面体の慣性テンソルが容易に求まります。

# 多面体の慣性テンソル
多面体の表面が$n$個の三角形で構成されているとします（三角形が互いに重ならず、表面を埋め尽くしていることを前提にしています）。$i$番目の三角形と原点とで作られる四面体の慣性テンソルを$\boldsymbol{I}_{i}$とおきましょう。このとき、各三角形の頂点の順番は「三点をその順で辿ったときに右ねじの法則によって決まる法線方向が**多面体の外側を向く**」ように与えることにします。

原点が多面体の内側にあるならば、上記の規約は先ほどの（原点を頂点に含む四面体の慣性テンソルを計算したときの）規約と同じものになるので、全ての$\boldsymbol{I}\_{i}$を足せば$\boldsymbol{I}$になります。原点が多面体の頂点に含まれるならば、原点を頂点に含む三角形と原点とがなす四面体は体積ゼロ、慣性テンソルは$\boldsymbol{I}\_{i}=\boldsymbol{O}$になります（ので存在を忘れて構いません）。では、原点が多面体の外側にある場合は？

このときは、原点が上記法線方向と同じ側に存在する面と、逆側に存在する面が出てきます。後者についてはGrassmannの記号が負になります。前者については正になるのですが、このとき計算される$\boldsymbol{I}\_{i}$は多面体の外側領域の分も含んでしまっているので、後者の分をさっぴかなくてはいけません。ということは、結局原点が多面体に対してどこにあろうと、全ての$\boldsymbol{I}\_{i}$を足せば多面体の慣性テンソル$\boldsymbol{I}$が求まるということになります。つまり非常に都合が良いことに、原点が多面体に対してどこにあろうと
```math
\boldsymbol{I}=\sum_{i=1}^{n}\boldsymbol{I}_{i}
```
が常に成り立つわけです。

Zeoでの実装は次の通りです。

```C
/* inertia tensor of a 3D polyhedron. */
zMat3D *zPH3DInertia(zPH3D *ph, double density, zMat3D *inertia)
{
  register int j;
  zMat3D i;

  zMat3DZero( inertia );
  for( j=0; j<zPH3DFaceNum(ph); j++ ){
    zTri3DConeInertia( zPH3DFace(ph,j), density, &i );
    _zMat3DAddDRC( inertia, &i );
  }
  return inertia;
}
```

# 重心まわり慣性テンソル
本記事の冒頭に書いたことのおさらいですが、上記の式で求めた$\boldsymbol{I}$は、多面体の「原点まわり」慣性テンソルです。力学計算ではこれを「多面体の重心まわり慣性テンソル」$\boldsymbol{I}\_{\mathrm{G}}$に変換しておくと都合が良いことが多くあるので、それを行いましょう。

まず、原点および三つの頂点$\boldsymbol{p}\_{i1}$、$\boldsymbol{p}\_{i2}$、$\boldsymbol{p}\_{i3}$からなる四面体の体積を$v\_{i}$、重心を$\boldsymbol{p}\_{\mathrm{G}i}$とそれぞれおきます。頂点の順番によっては$v\_{i}$は負となるのでした。正確さを期すために、念のためこれを「符号付き体積」と呼び変えましょう。次式が成り立ちます。
```math
\begin{align}
v_{i}&=\frac{1}{6}[\boldsymbol{p}_{i1} \boldsymbol{p}_{i2} \boldsymbol{p}_{i3}]
\\
\boldsymbol{p}_{\mathrm{G}i}&=\frac{\displaystyle \boldsymbol{p}_{i1}+\boldsymbol{p}_{i2}+\boldsymbol{p}_{i3}}{4}
\end{align}
```
これらを用いれば、多面体全体の体積$v$および重心$\boldsymbol{p}\_{\mathrm{G}}$は次のように求まります。
```math
\begin{align}
v&=\sum_{i}^{n}v_{i}
\\
\boldsymbol{p}_{\mathrm{G}}&=\frac{\displaystyle\sum_{i}^{n}v_{i}\boldsymbol{p}_{\mathrm{G}i}}{\displaystyle\sum_{i}^{n}v_{i}}
\end{align}
```
結局割り算するので、$v_{i}$は$[\boldsymbol{p}\_{i1} \boldsymbol{p}\_{i2} \boldsymbol{p}\_{i3}]$で置き換えてしまっても（1/6倍しなくても）構いません。
重心が求まれば**平行軸の定理**より
```math
\boldsymbol{I}=\boldsymbol{I}_{\mathrm{G}}-\rho v\left[\boldsymbol{p}_{\mathrm{G}}\times\right]^{2}
```
が成り立つので、
```math
\boldsymbol{I}_{\mathrm{G}}=\boldsymbol{I}+\rho v\left[\boldsymbol{p}_{\mathrm{G}}\times\right]^{2}
```
と求まります。

Zeoでの実装は次の通りです。`zMat3DCatVec3DDoubleOuterProdDRC()`という関数を使っています。

```C
/* inertia tensor about barycenter of a 3D polyhedron. */
zMat3D *zPH3DBaryInertia(zPH3D *ph, double density, zMat3D *i)
{
  zVec3D c;

  zPH3DInertia( ph, density, i );
  zPH3DBarycenter( ph, &c );
  zMat3DCatVec3DDoubleOuterProdDRC( i, density * zPH3DVolume( ph ), &c );
  return i;
}
```

# 剛体の姿勢が変わったら？
今我々は「運動している剛体」を考えていますので、**観測座標系**から見たら多面体を構成する頂点の位置は全て時々刻々変わることになります。頂点はばらばらに運動しているのではなく剛体として互いに相対距離を保っていますので、運動計算するたびにこれらを毎回求めた上で慣性テンソルを再計算するのはいかにも無駄です。

剛体に固定されたある座標系（**物体座標系**と呼びましょう）を考えると、その座標系の中では全ての頂点は不動ですので慣性テンソルも定数行列となります（行列じゃなくてテンソルじゃないのか、という疑問はこの際スルーさせて頂きます）。これを$\bar{\boldsymbol{I}}$とおきましょう。

今、物体座標系から観測座標系への姿勢変換行列が$\boldsymbol{R}$であるとします。剛体が運動（回転）するとはこの$\boldsymbol{R}$が変化するということと同義です。観測座標系に対する剛体の角速度$\boldsymbol{\omega}$を**姿勢だけ**物体座標系に合わせて変換（物体座標系に対しては剛体は運動しないので角速度は$\boldsymbol{0}$となってしまうことに注意）したものを$\bar{\boldsymbol{\omega}}$とおく、つまり
```math
\bar{\boldsymbol{\omega}}=\boldsymbol{R}^{\mathrm{T}}\boldsymbol{\omega}
```
とおくと、角運動量は座標系に依存するベクトルですので、
```math
\boldsymbol{I}\boldsymbol{\omega}=\boldsymbol{R}\bar{\boldsymbol{I}}\bar{\boldsymbol{\omega}}=\boldsymbol{R}\bar{\boldsymbol{I}}\boldsymbol{R}^{\mathrm{T}}\boldsymbol{\omega}
```
が常に成り立ちます。両辺を比較すれば
```math
\boldsymbol{I}=\boldsymbol{R}\bar{\boldsymbol{I}}\boldsymbol{R}^{\mathrm{T}}
```
であると分かります。これは慣性テンソルを重心まわりのもの$\boldsymbol{I}\_{\mathrm{G}}$および$\bar{\boldsymbol{I}}\_{\mathrm{G}}$に置き換えても同じです。

ちなみにZeoでは次の関数`zRotMat3D()`が用意されており、これを使えば上記の計算が1行で書けます。

```C
/* rotational multiplication for 3D matrices. */
zMat3D *zRotMat3D(zMat3D *r, zMat3D *m, zMat3D *rm)
{
  zMulMat3DMat3D( r, m, rm );
  return zMulMat3DMat3DTDRC( rm, r );
}
```

なお、この計算はベクトル姿勢変換に比べて加算回数/積算回数ともに多いので、Newton=Euler法ではこれを避ける工夫がなされています（Articulated-Body-Inertia法では避けられません）。