spinのぶんか社「DOUBLE-S.T.E.A.L」開発スタッフインタビューで、ぶんか社の永谷さんは、物理計算に関して、「一番コアになる部分は300～400行程度の小さなモジュールで、これは100人が書いたら100人とも同じになるような基礎的なコードです。」と、発言されています。
まぁ、みんな同じものを作っていると思いますが、その割に、ネットに情報があまりないと感じます。
一方、残念なことに、つよぞうさんの「こってり屋」が終了されてしまわれました。
非常に有益なサイトの一つがなくなってしまい、途方にくれている方も多いと思います。
ということで、剛体運動の基礎的なプログラムを書いてみることにしました。
というのは言い訳で、本当は自分で勉強したのを確認するためにページを作りました。
今回のソースは、次のものです（DirectX8.1用です）。
今回は、半球ライティングで用いたソースを変更して作っています。
主な内容は、次のとおりです。
| rigidbody.h | 剛体運動に関する定義。 |
| rigidbody.cpp | 剛体運動の計算部分。 |
| main.h | 基本的な定数など。今回も出番無し。 |
| main.cpp | 描画に関係しないシステム的な部分。変更が無いので、出番無し。 |
| draw.h | 描画の各関数の定義。特に意味無いので出番無し。 |
| draw.cpp | メインの描画部分。 |
| font.h | FPS表示用。既出。 |
| font.cpp | FPS表示用。既出。 |
| bg.cpp | 地面＋天円柱の描画。今回説明無し。 |
| light_eff.cpp | ライトの方向を説明するラインを描画。XFCのものから、抜き出し。 |
| resource.h | メニューに関する定義。 |
| diffuse.vsh | 頂点シェーダープログラム。シンプルな平行光源＋環境光のライト。 |
| specular.vsh | 頂点シェーダープログラム。頂点色によるスペキュラーライト。 |
| texspec_vsh.vsh | 頂点シェーダープログラム。テクスチャーによるスペキュラーライト。 |
| hemisphere.vsh | 頂点シェーダープログラム。平行半球ライト。 |
| hemi_amb.vsh | 頂点シェーダープログラム。平行光源＋半球環境光ライト。 |
| hemi_spec.vsh | 頂点シェーダープログラム。平行光源＋半球環境光＋スペキュラーライト。 |
| soft.vsh | 頂点シェーダープログラム。やわらかいトゥーン。 |
| edge.vsh | 頂点シェーダープログラム。輪郭を描く。 |
あと、いつもの様に、モデルと、実行ファイル及び、プロジェクトファイルが入っています。
操作方法は、
| [ESC][F12] | 終了 |
| [↑] | 加速 |
| [↓] | 減速 |
| [←][→] | 曲がる |
です。
その他に、左ボタンを押しながらのドラッグでカメラが回り、右ボタンを押しながらのドラッグで光源が回ります。
まだ、発展途上なもので、車の動きが不安定なのはご了承ください。
剛体とは、変形しない物体のことです。
質点とは異なり、大きさは持ちます。まぁ、車や飛行機と思ってください。
剛体は、質点と違い大きさを持っているので、剛体の向きも考慮しなくてはなりません。
時刻 0 の剛体の一点の座標を r0 とすると、時刻 t での r0 の位置r(t) は、次のようにかけます。
r(t) = R(t)r0 + x(t)
R(t) は、剛体の中心周りの回転を表現し、x(t)は、全体の平行移動を表現します。
通常、剛体の中心は重心に取ります。
中心を重心に取ると、運動方程式が重心運動と回転運動に分離できたりいろいろと都合がよいので、普通はそのように取ります。
まぁ、剛体を重心を貫くように押すと、回らないで、まっすぐに押せるのを体感できるので、重心が特別な点だということが実感できるのではないでしょうか。
ようはニュートンの運動方程式です。
全体（重心）に関する運動方程式は、通常の運動方程式
d m ─ x(t) = mv(t) = p(t) dt d ─ p(t) = F(t) dt
になります（導出方法は、力学の本をよんでね）。
p(t) は剛体の運動量、F(t) は重心を押す外力、m は剛体の質量です。
並進運動と同じように、（角）運動量や力（トルク）に関する式を導出します。
角速度が ω(t) の回転を考えます。
下の図を見てください。
角速度が ω(t) で回るということは、位置 r(t) が ω(t) Ｘ b の速さと向きで動くということです。
ここで、a は、位置 r(t) の回転軸に平行な成分で、b は、r(t)の回転軸に平行な成分です。
a が回転軸に平行なので、ω(t) Ｘ a = 0 が成り立ちます。
従って、
d ─ r(t) = ω(t) Ｘ r(t) dt
が、成り立ちます。
以上の議論は、並進運動が無い純粋な回転の場合の話です。
従って、剛体の運動に関して、R(t) の時間変化に関する式に関連しています。
R(t) が、
┌ rxx ryx rzx ┐
R(t) = │ rxy ryy rzy │
└ rxz ryz rzz ┘
と、書けたとします。このとき、座標の各単位ベクトルは、
┌ 1 ┐ ┌ rxx ┐ ┌ 0 ┐ ┌ ryx ┐ ┌ 0 ┐ ┌ rzx ┐
R(t)│ 0 │= │ rxy │ R(t)│ 1 │= │ ryy │ R(t)│ 0 │= │ rzy │
└ 0 ┘ └ rxz ┘, └ 0 ┘ └ ryz ┘, └ 1 ┘ └ rzz ┘
になります。
この式から、各単位ベクトルの時間発展（の回転成分）を縦ベクトルとして、３つ並べると、回転行列 R(t) を求めることができます。
つまり、時刻 t の単位ベクトルを ex(t), ey(t), ez(t) とすれば、
R(t) = ( ex(t) ey(t) ez(t) )
に、なります。
このそれぞれの軸は、先ほどの時間発展で動いているので、
d d d d ─R(t) = ( ─ex(t) ─ey(t) ─ez(t) ) = ( ω(t)Ｘex(t) ω(t)Ｘey(t) ω(t)Ｘez(t) ) dt dt dt dt
が、導かれます。
さて、上の式は見慣れない式なので、扱いずらいです。
そこで、ベクトルから連想される行列を用いて、この式を書き換えることがよく行われます。
ベクトル a から連想される行列a*を、次のように定義します。
┌ 0 -az ay ┐
a* ≡ │ az 0 -ax │
└ -ay ax 0 ┘
この行列のベクトルへの作用は、
┌ 0 -az ay ┐┌ bx ┐ ┌ aybz - azby ┐
a*b = │ az 0 -ax ││ by │ = │ azbx - axbz │ = aＸb
└ -ay ax 0 ┘└ bz ┘ └ axby - aybx ┘
になります(書き方も似てますね)。
この行列を用いると、回転運動の時間発展は、
d
─R(t) = ( ω(t)Ｘex(t) ω(t)Ｘey(t) ω(t)Ｘez(t) )
dt
= (ω(t)*ex(t) ω(t)*ey(t) ω(t)*ez(t))
= ω(t)* (ex(t) ey(t) ez(t))
= ω(t)* R(t)
となります。
最終的に回転の時間発展という難しそうなものが、行列の積の形で書けました。
長くなったので、章がえ～
角運動量と回転モーメント(トルク)および、角運動量と回転軸の関係から、回転に関する運動方程式を構成します。
角運動量を、次のように定義します。
d
L(t) ≡ ∑ miri(t) Ｘ ─ri(t)
i dt
ここで、i は、剛体を質点の塊とみなした時の、各質点を区別する添え字です。
mi は各質点の質量、ri(t)は各質点の重心からの相対位置を表現します。
角運動量の時間変化は、次のように計算できます。
d d d
─ L(t) = ∑ ─ { miri(t) Ｘ ─ri(t) }
dt i dt dt
d d d2
= ∑ mi { ─ri(t)Ｘ─ri(t) + ri(t)Ｘ─ri(t) }
i dt dt dt2
d2
= ∑ ri(t) Ｘ mi ─ ri(t)
i dt2
= ∑ ri(t) Ｘ Fi(t)
= τ(t)
τ(t) ≡ ∑ ri(t) Ｘ Fi(t)
Fi(t) の ri(t) と並行な成分は 0 になるので、トルクは、重心を中心として回転させる力の集合になっています。
それでは、角運動量 L(t) と回転軸 ω(t) の関係を導出します。
剛体の各点の速度を、計算すると、
d
L(t) = ∑ miri(t) Ｘ ─ri(t)
i dt
d
= ∑ miri(t) Ｘ (─ R(t)ri(0) + v(t) )
i dt
= ∑ miri(t) Ｘ ( ω(t)Ｘ R(t)ri(0) + v(t) )
= ∑ miri(t) Ｘ ( ω(t)Ｘ(ri(t)-x(t)) + v(t) )
= ∑ miri(t) Ｘ ( ω(t)Ｘri(t) ) + ∑ miri(t) Ｘ (-x(t) + v(t))
= ∑ miri(t) Ｘ ( ω(t)Ｘri(t) )
= ∑ mi [ (ri(t)･ri(t)) ω(t) - (ri(t)･ω(t)) ri(t) ]
= I(t) ω(t)
になります。
中心が重心ということで、∑ miri(t) = 0 を使いました。
また、ベクトルの等式
aＸ(bＸc) = (a･c)b - (a･b)c
を、使いました。慣性テンソル I(t) は、
┌ riy2(t)+riz2(t) -rix(t)riy(t) -rix(t)riz(t) ┐
I(t) ≡ ∑mi │ -rix(t)riy(t) rix2(t)+riz2(t) -riy(t)riz(t) │
└ -rix(t)riz(t) -riy(t)riz(t) rix2(t)+riy2(t) ┘
で、定義されます。
I(t) は、ri(t) に依存するので、いちいち計算するには面倒くさいです。
そこで、もう少し扱いやすい形に変形します。
┌ (rix2(t)-rix2(t))+riy2(t)+riz2(t) -rix(t)riy(t) -rix(t)riz(t) ┐
I(t) = ∑mi │ -rix(t)riy(t) rix2(t)+(riy2(t)-riy2(t))+riz2(t) -riy(t)riz(t) │
└ -rix(t)riz(t) -riy(t)riz(t) rix2(t)+riy2(t)+(riz2(t)-riz2(t)) ┘
┌ 1 0 0 ┐ ┌ rix(t)rix(t) rix(t)riy(t) rix(t)riz(t) ┐
= ∑mi { (rix2(t)+riy2(t)+riz2(t))│ 0 1 0 │ - │ rix(t)riy(t) riy(t)riy(t) riy(t)riz(t) │ }
└ 0 0 1 ┘ └ rix(t)riz(t) riy(t)riz(t) riz(t)riz(t) ┘
さて、ここから計算がシステマチックでわずらわしいので、アインシュタインの規約を使って、記述します。
Iμν(t) = ∑mi [riγ(t)riγ(t)1μν - riμ(t)riν(t)]
= ∑mi [(Rγκ(t)riκ(0))(Rγδ(t)riδ(0))1μν - (Rμκ(t)riκ(0))(Rνδ(t)riδ(0))]
= ∑mi [1δκ(t)riκ(0)riδ(0)1μν - Rμκ(t)Rνδ(t)riκ(0)riδ(0)]
= ∑mi [riκ(0)riκ(0)1μν - Rμκ(t)Rνδ(t)riκ(0)riδ(0)]
= ∑mi [riκ(0)riκ(0)(Rμδ(t)1δγRγν-1(t)) - Rμκ(t)Rνδ(t)riκ(0)riδ(0)]
= Rμκ(t)Rδν-1(t)∑mi [riγ(0)riγ(0)1κδ - riκ(0)riδ(0)]
= Rμκ(t)Iκδ(0)Rδν-1(t)
になります。つまり
Iμν(t) = R(t)I(0)R-1(t)
です。以上の導出に、R(t) が回転行列であること Rμν-1(t) = RμνT(t) = Rνμ(t) を用いました。
慣性テンソルは最初の時点で求めておけば、あとは、剛体の向きを作用させれば、それぞれの時刻での値が求まることになります。
通常は、ローカル座標系での慣性テンソルを求めておき、そこから剛体の向きによって、ワールド座標系での慣性テンソルを求めます。
密度ρが一様な幾何学的な形状の慣性テンソルは計算で求めることができ、
各軸の大きさが x, y, z の直方体(質量m=ρxyz)
m ┌ y2+z2 0 0 ┐
I = ─│ 0 x2+z2 0 │
12└ 0 0 x2+y2 ┘
半径rの球(質量m=4πρｒ3/3)
2m┌ r2 0 0 ┐
I = ─│ 0 r2 0 │
5 └ 0 0 r2 ┘
半径 x, y, z の楕円体(質量m=4πρxyz/3)
2m┌ yz 0 0 ┐
I = ─│ 0 xz 0 │
5 └ 0 0 xy ┘
半径r, 高さh の円筒(質量m=πρr2h)
m ┌ 3r2+h2 0 0 ┐
I = ─│ 0 6r2 0 │
12└ 0 0 3r2+h2 ┘
半径r, 高さh の円錐(質量m=2πρr2h/3, 重心は柱の3/4の位置)
m ┌ 3(r2+h2/4) 0 0 ┐
I = ─│ 0 6r2 0 │
20└ 0 0 3(r2+h2/4) ┘
のようになります（こってり屋の丸写しってばらしちゃだめ！）。
剛体を記述する状態ベクトルと、その時間発展は、次のようになります。
┌ x(t) ┐
Y(t) = │ R(t) │
│ p(t) │
└ L(t) ┘
d ┌ v(t) ┐
─Y(t) = │ ω(t)* R(t) │
dt │ F(t) │
└ τ(t) ┘
p(t) = m v(t)
L(t) = I(t)ω(t)
では、ソースを解説します。
今回は、運動がメインなので、描画部分の解説はしません。
半球ライティングのページを参考に調べてください。
変数は、次のようにしました(rigidbody.h です)。
DirectX用に、D3DXMATRIX を使っていますが、ほかのＡＰＩを使うときでも、適当な行列を使って計算が可能でしょう。
0015: class CRigidBody {
0016: private:
0017: protected:
0018: // 定数
0019: float mass; // 質量
0020: D3DXMATRIX IBody; // ローカル座標での慣性テンソル
0021: D3DXMATRIX IBodyinv; // ローカル座標での慣性テンソルの逆行列
0022: // 状態ベクトル
0023: D3DXVECTOR4 x; // 重心座標
0024: D3DXMATRIX R; // 回転座標
0025: D3DXVECTOR4 p; // 運動量
0026: D3DXVECTOR4 L; // 回転モーメント
0027: // ２次的変数
0028: D3DXVECTOR4 v; // 速度
0029: D3DXMATRIX Iinv; // 慣性テンソルの逆行列
0030: D3DXVECTOR4 omega; // 回転軸
0031: // 外力
0032: D3DXVECTOR4 force;
0033: D3DXVECTOR4 torque;
0034:
･･･
0046: };
状態ベクトル Y(t) に関係するものだけでなく、計算途中で使う速度ベクトルもメンバーとして持っています。
これらの変数は剛体のプログラムに共通のものだと思います。
今回は、CRigidBody から派生させた CCar クラスを実際の計算に用いています。
CCar クラスは、外力を与える等、車の計算に特有な部分を記述します。
毎フレームの更新は、CRigidBody::Update で行います(rigidbody.cpp です)。
この関数で、状態ベクトルに関する更新を行います。
dt は、前のフレームとの時間差分です。フレーム数が違っても、同じ運動になるように調整するために使います。
0065: // ----------------------------------------------------------------------------
0066: // 座標に時間変化を足しこむ
0067: void CRigidBody::Update(float dt)
0068: {
0069: this->ComputeForceAndTorque(dt);
0070: this->Calc2ndValues();
0071:
0072: this->x += dt * this->v;
0073: this->p += dt * this->force;
0074: this->L += dt * this->torque;
0075: this->R += dt * GetSkewSymmetricMatrix(this->omega) * this->R;
0076:
0077: this->R._14 = this->R._24 = this->R._34 = 0.0f;
0078: this->R._41 = this->R._42 = this->R._43 = 0.0f;
0079: this->R._44 = 1.0f;
0080: D3DXVec3Normalize((D3DXVECTOR3*)&this->R._11,(D3DXVECTOR3*)&this->R._11);
0081: D3DXVec3Normalize((D3DXVECTOR3*)&this->R._21,(D3DXVECTOR3*)&this->R._21);
0082: D3DXVec3Normalize((D3DXVECTOR3*)&this->R._31,(D3DXVECTOR3*)&this->R._31);
0083: }
ComputeForceAndTorque で外力を計算します（CRigidBody では、何もしませんが、CCar で関数を上書きして、適当な運動をするような外力を与えています）。
次に、Calc2ndValues で、速度ベクトルなどを計算して、準備を整えてから、足しこみます。
回転行列は、計算誤差の蓄積で正規直行行列からずれていくので、規格化等をして適切な状態になるように調整します。
速度ベクトルなどを計算する部分は、先ほどまで述べた部分に忠実に計算しています。
0053: // ----------------------------------------------------------------------------
0054: // v や ω を計算
0055: void CRigidBody::Calc2ndValues()
0056: {
0057: D3DXVec4Scale(&this->v, &this->p, 1.0f/this->mass);
0058:
0059: D3DXMATRIX Rt;
0060: D3DXMatrixTranspose(&Rt, &this->R);
0061: this->Iinv = this->R * this->IBodyinv * Rt;
0062:
0063: D3DXVec4Transform(&this->omega, &this->L, &this->Iinv);
0064: }
それぞれ定義を確認してみてください。
CCar::ComputeForceAndTorque は、各ソフト独特の部分で、
剛体運動一般に関して成り立つものではないので、今回は解説しません
（まだ、満足できる出来でもないですしね）。
方法だけ言っておくと、タイヤの下の各点が地面にめり込んでいるか調べて、
めり込んでいたらめり込んだ分だけ押し返します（あと、安定化を図る雑多な部分です）。
実際にプログラムをしてみると、安定させるのが難しいですね。
それに、回転行列を作るのもうまく言ってないですね。
まだまだ改良が必要そうです。
それに、解説部分にもけっこう論理に飛躍があるので、完成させるためには、
まだまだ修正が必要ですね。
う～、反省ばかりになってしまった。
参考文献は、