2002 May. 28 修正（修正前のドキュメント）
前回、剛体をちょこっとやってみたのですが、
回転部分が完全には３次元の特殊正規直行行列（まぁSO3だ）になっていないので、
計算をしつづけると、車がゆがんできます。
これを防止するために、回転を表現する別の方法である Quaternion で姿勢を制御します。
今回のソースは、次のものです（DirectX8.1用です）。
今回は、前回のソースの変更版です。
といってもGeForce3以上でないと動かないのも酷だと思ったので、普通の環境で動くようにしています。
| rigidbody.h | 剛体運動に関する定義。 |
| rigidbody.cpp | 剛体運動の計算部分。 |
| main.h | 基本的な定数など。今回も出番無し。 |
| main.cpp | 描画に関係しないシステム的な部分。変更が無いので、出番無し。 |
| draw.h | 描画の各関数の定義。特に意味無いので出番無し。 |
| draw.cpp | メインの描画部分。 |
| font.h | FPS表示用。既出。 |
| font.cpp | FPS表示用。既出。 |
| load.h | ファイルの読み込み。 |
| load.cpp | ファイルの読み込み。 |
| bg.cpp | 地面＋天円柱の描画。今回説明無し。 |
| light_eff.cpp | ライトの方向を説明するラインを描画。XFCのものから、抜き出し。 |
| resource.h | メニューに関する定義。 |
あと、いつもの様に、モデルと、実行ファイル及び、プロジェクトファイルが入っています。
操作方法は、
| [ESC][F12] | 終了 |
| [↑] | 加速 |
| [↓] | 減速 |
| [←][→] | 曲がる |
| [SPACE] | リセット |
です。
その他に、左ボタンを押しながらのドラッグでカメラが回り、右ボタンを押しながらのドラッグで光源が回ります。
今回も、車の動きが不安定です。ご了承ください。
Quaternion とは、複素数が２次元の自由度だった（複素数は平面に書ける）ものを、４次元に自由度を拡張したものです。
Quaternion は、一般に次のように書けます。
q = a + bi + cj + dk
ここで、a,b,c,d は実数です。
i, j, k (と、暗黙の1) は、Quaternion の基底です。
基底の積に関して、次の規則が決められています。
i･1 = 1･i = i, j･1 = 1･j = j, (ようは1に何かけても1) k･1 = 1･k = k, i･j =-j･i = k, j･k =-k･j = i, (他の基底は積の結果が巡回的) k･i =-i･k = j.
この規則を用いると、２つの quaternion
q1 = a1 + b1i + c1j + d1k, q2 = a2 + b2i + c2j + d2k,
の和と差と積は次のようになります。
q1 + q2 = a1 + a2 + (b1 + b2)i + (c1 + c2)j + (d1 + d2)k, q1 - q2 = a1 - a2 + (b1 - b2)i + (c1 - c2)j + (d1 - d2)k.
q1･q2 = (a1 + b1i + c1j + d1k)･(a2 + b2i + c2j + c2j)
= (a1 + b1i + c1j + d1k)･a2
+ (a1 + b1i + c1j + d1k)･b2i
+ (a1 + b1i + c1j + d1k)･c2j
+ (a1 + b1i + c1j + d1k)･d2k
↓基底の積の規則を使って
= (a1 + b1i + c1j + d1k)･a2
+ (a1i - b1 - c1k + d1j)･b2
+ (a1j + b1k - c1 - d1i)･c2
+ (a1k - b1j + c1i - d1 )･d2
↓それぞれの基底でまとめて
= (a1a2 - b1b2 - c1c2 - d1d2)
+ (a1b2 + b1a2 + c1d2 - d1c2)i
+ (a1c2 - b1d2 + c1a2 + d1b2)j
+ (a1d2 + b1c2 - c1b2 + d1a2)k
さて、上の書き方は非常に項が多くて、やってられません。
ということで、ベクトルを用いた表記方法が使われています。
それは、３次元ベクトルと１つの実数の集合として表記する方法で、
q = (a, v), ⇔ q = a + bi + cj + dk v = < b c d >
のように書きます。
この表記法を用いると、和や積は
q1 + q2 = (a1 + a2, v1 + v2), q1 - q2 = (a1 - a2, v1 - v2), q1 ･ q2 = (a1a2 - v1･v2, a1v2 + a2v1 + v1×v2),
と、積はベクトルの内積や外積を使いますが、より短い形で書けます。
さらに、短い書き方として、ベクトルと実数の区別が付く事から、
q = a + v
もあります。
Quaternion は複素数を拡張したものなのでほとんどの quaternion q に関して
その逆数（掛けて1になるもの）q-1が存在します。
次の回転で使うので、ここで紹介しておきます。
次の quaternion の積に注目します。
(a, v)(a, -v) = (aa + v･v, -av + av - v×v) ↓v×v=0
= (a2 + v･v, 0)
= (a2 + v･v)1
1 ≡ (1, < 0 0 0 >) : 単位 quaternion
上の式をよく見ると、次の quaternion が逆 quaternion になります。
q '
q -1 = ── q ' = (a, -v) :q の共役
∥q∥2 ∥q∥2 = a2 + v･v :q のノルム
では、Quaternion の何がありがたられるかということですが、
それは、Quaternion が直接的にベクトルの回転を表現するからです。
例えば、４元ベクトルの quaternion による表現
P = (w, x) = (w, < x y z >)
を考えます。
この両辺に quaternion q =(a, v) と、その逆 quaternion を作用させると、
P' = q P q -1
= (a, v) (w, x) (a, -v)/∥q∥2
↓q1 ･ q2 = (a1a2 - v1･v2, a1v2 + a2v1 + v1×v2)
= (aw-v･x, ax+wv+v×x) (a, -v)/∥q∥2
= (a2w-av･x+(ax･v+wv･v+(v×x)v), -awv+(v･x)v+a2x+awv+av×x-ax×v-wv×v-(v×x)×v)/∥q∥2
↓黄色とオレンジの部分は消し合い、灰色と黒の部分はベクトルの計算の性質上０になる
= ((a2+v･v)w, (v･x)v+a2x+2av×x-(v×x)×v)/∥q∥2
= (∥q∥2w, a2x+2av×x+(v･x)v-(v×x)×v)/∥q∥2
= (w, x')
x' = (a2x+2av×x+(v･x)v-(v×x)×v)/∥q∥2
さて、この変換後のベクトルの大きさを調べてみます。
計算の簡単のために、∥q∥4 を掛けておくと、
∥q∥4x'2 = (a2x+2av×x+(v･x)v-(v×x)×v)･(a2x+2av×x+(v･x)v-(v×x)×v)
= a4x･x + 2a3x･(v×x) + a2(v･x)(x･v) - a2x･((v×x)×v))
+ 2a3(v×x)･x + 4a2(v×x)･(v×x) + 2a(v･x)((v×x)･v) - 2a(v×x)･((v×x)×v)
+ a2(v･x)(v･x) + 2a(v･x)(v･(v×x)) + (v･x)2v･v - (v･x)(v･((v×x)×v))
- a2((v×x)×v)･x - 2a((v×x)×v)･(v×x) - (v･x)(((v×x)×v)･v) + ((v×x)×v)･((v×x)×v)
↓灰色の部分はベクトルの計算の性質上０になる
= a4x･x + a2(v･x)2 - a2x･((v×x)×v)) + 4a2(v×x)2 + a2(v･x)2 + (v･x)2v･v
- a2((v×x)×v)･x + ((v×x)×v)･((v×x)×v)
= a4x･x + 2a2(v･x)2 - 2a2x･((v×x)×v)) + 4a2(v×x)2 + (v･x)2v･v + ((v×x)×v)･((v×x)×v)
↓(a×b)×c = b(c･a) - a(c･b)
= a4x･x + 2a2(v･x)2 - 2a2x･(x(v･v)-v(v･x)) + 4a2(v×x)2 + (v･x)2v･v
+ (x(v･v)-v(v･x))･((v×x)×v)
= a4x･x - 2a2(x･x)(v･v) + (v･x)2v･v + (v･v)(x･((v×x)×v))
+ 4a2(v･x)2 + 4a2(v×x)2
↓(v･x)2 = |v|2|x|2cos2Θ、(v×x)2 = |v|2|x|2sin2Θ
= a4x･x + 2a2(x･x)(v･v) + (v･x)2v･v + (v･v)(x･((v×x)×v))
= a4x･x + 2a2(x･x)(v･v) + (v･x)2v･v + (v･v)(x･(x(v･v)-v(v･x)))
= a4x･x + 2a2(x･x)(v･v) + (v･v)2x･x
= (a2 + (v･v))2(x･x)
= ∥q∥4x2
になります。
大きさが変わらないということは、「ベクトルの向きを変えた」すなわち回転したことになります。
しかも、４元ベクトルの第四成分 w も変化して無いので、本当に回転を表現していることになります。
iforさんから、この部分が分からないとの指摘を受けました。 （3次元）ベクトルは、突き詰めていけば、（x,y,z）の成分を持つ数字の集合です。 このベクトルは、単位ベクトルeとスカラーaを用いて (x,y,z)=ae と、必ず書くことが出来ます。 ベクトルの変換は、aを変化させる（拡大縮小）のと、eを変化させる（回転）の合成になります。 平行移動も、その結果は回転と拡縮の結果に帰着させられるということです。 その上で、「ベクトルの大きさが変わらない変換」というのは、残された「回転しか変換をしていない」ということになるので、 「大きさが変わらないので回転したことになる」という表現を使いました。
さて、quaternion を次の形で書きます。
q = |q |(cos(θ/2), sin(θ/2)ω)
ここで、ωは単位ベクトル、|q | は quaternion の大きさ（|q |=Sqrt(∥q∥2)）です。
任意の quaternion はこの形式で掛けますが、この quaterion を用いてベクトルを回転すると、
P' = q P q -1
= (w, x')
x' = (a2x+2av×x+(v･x)v-(v×x)×v)/∥q∥2
= cos2(θ/2)x + 2cos(θ/2)sin(θ/2)ω×x + sin2(θ/2)(ω･x)ω - sin2(θ/2)(ω×x)×ω
↓(a×b)×c = b(c･a) - a(c･b)
= cos2(θ/2)x + 2cos(θ/2)sin(θ/2)ω×x + sin2(θ/2)(ω･x)ω - sin2(θ/2)(x(ω･ω)-ω(ω･x))
= (cos2(θ/2)-sin2(θ/2))x + 2cos(θ/2)sin(θ/2)ω×x + 2sin2(θ/2)(ω･x)ω
= cosθ x + sinθ ω×x + (1-cosθ)(ω･x)ω
= (ω･x)ω + (x-(ω･x)ω)cosθ + ω×x sinθ
さて、上の式をよく見ると、ベクトルを回転していることが見えてきます。
下の図を見てください。
先ず、一つ目の項です。
最初の項は、ωの軸へのxの射影になっています。
２つめの項は、元のベクトルから、ωの軸へ射影を引いた値。
つまり、ωに直行した射影にcosθを掛けた形になっています。
最後の項は、ω×x つまり、まえの２つの項に直行したベクトルです。
また、その大きさは、｜ω×x｜=|ω||x|sin(ω,x) で、
|x|sin(ω,x)は、丁度xのωに直行した成分の大きさになっています。
従って、上の図にある円の円上にω×xはあります。
２、３項目にそれぞれ cos, sin がかかっているので、上の式は、x をωを軸にして、θだけまわしていることを表現しています。
この議論は |q |≠0の時に成り立つので、quaternion の大きさが0にならないようにだけ注意してください。
では、有名な球面線型補間です。
2つのベクトルを直線的にではなく、弧を描いて補間します。
ベクトルq0をq1まで補間します。
q0とq1の大きさは等しいとします。
球面線型補間は、q0からq1まで回転させる補間なので、
先ほどまで扱っていた quaternion による回転で表現できます。
２つのベクトルを回転させる軸は、両方のベクトルに直行するので、それぞれのベクトルの外積になります。
パラメータt=0の時q0、t=1の時q1になるように補間することにすると、
パラメータの値がtの時の回転角は、θtになります（θはそれぞれのベクトルのなす角）。
以上から、ベクトルを回転させる quaternion は、
q0×q1
R(t) = (cos(θt/2), ──── sin(θt/2))
|q0×q1|
になります。
さて、quaternion としての球面線型補間は以上でおしまいですが、あえて、回転の計算を行うと、
ベクトル部分の回転結果は、
q(t) = R(t) q0 R -1(t)
= a2q0+2av×q0+(v･q0)v-(v×q0)×v a=cos(θt/2), v=sin(θt/2)q0×q1/|q0×q1|
↓灰色の部分はベクトルの計算の性質上０になる
= a2q0+2av×q0+(v･q0)v-(v･v)q0
↓展開
= cos2(θt/2)q0+2cos(θt/2) (sin(θt/2)q0×q1/|q0×q1|)×q0+0-sin2(θt/2)q0
↓倍角の公式
= cos(θt)q0 + sin(θt)/|q0×q1|(q0×q1)×q0
= cos(θt)q0 + sin(θt)/|q0×q1|((q0･q0)q1-(q0･q1)q0)
↓q0とq1のベクトルの大きさが同じと仮定する
= cos(θt)q0 + sin(θt)/sin(θ)(q1-cos(θ)q0)
cos(θt)sin(θ) - sin(θt)cos(θ) sin(θt)
= ──────────────── q0 + ──── q1
sin(θ) sin(θ)
sin(θ(1-t)) sin(θt)
= ───── q0 + ──── q1
sin(θ) sin(θ)
となります。
Quaternion を使わなくても、この計算をすれば、球面線型補間が実現できます。
いままで、quaterinon の世界での回転を扱っていましたが、我々が実際に欲しいのは行列です。
そこで、
P' = q P q -1 = M P 但し、 q = (a, v), P = (w, x)
を満たす行列 M を求めます。
変換後のベクトルを計算すると、
P' = q P q -1 = (w, x')
x' = (a2x+2av×x+(v･x)v-(v×x)×v)/∥q∥2
↓(a×b)×c = b(c･a) - a(c･b)
= (a2x+2av×x+(v･x)v-((v･v)x-(v･x)v))/∥q∥2
↓各項をまとめて
= ((a2-v･v)x+2av×x+2(v･x)v)/∥q∥2
↓成分で展開
= ((a2-v･v)(xi+yj+zk)
+2a((vyz-vzy)i+(vzx-vxz)j+(vxy-vyx)k)
+2(vxx+vyy+vzz)(vxi+vyj+vzk))/∥q∥2
↓縦ベクトルで整理する
1 ┌ a2-v･v+2vx2 2vxvy-2avz 2vxvz+2avy ┐┌ x ┐
= ─ │ 2vxvy+2avz a2-v･v+2vy2 2vyvz-2avx ││ y │
∥q∥2└ 2vxvz-2avy 2vyvz+2avx a2-v･v+2vz2 ┘└ z ┘
1 ┌ ∥q∥2-2vy2-2vz2 2vxvy-2avz 2vxvz+2avy ┐┌ x ┐
= ─ │ 2vxvy+2avz ∥q∥2-2vz2-2vx2 2vyvz-2avx ││ y │
∥q∥2└ 2vxvz-2avy 2vyvz+2avx ∥q∥2-2vy2-2vy2 ┘└ z ┘
なので、変換行列 M は、
1 ┌ ∥q∥2-2vy2-2vz2 2vxvy-2avz 2vxvz+2avy 0 ┐
M = ─ │ 2vxvy+2avz ∥q∥2-2vz2-2vx2 2vyvz-2avx 0 │
∥q∥2│ 2vxvz-2avy 2vyvz+2avx ∥q∥2-2vy2-2vy2 0 │
└ 0 0 0 ∥q∥2┘
になります。
特に、回転 quaternion の表記
q = |q |(cos(θ/2), sin(θ/2)ω)
の時には、
┌ 1-2sin2(θ/2)(ωy2+ωz2) 2sin2(θ/2)ωxωy-2cos(θ/2)sin(θ/2)ωz 2sin2(θ/2)ωxωz+2cos(θ/2)sin(θ/2)ωy 0 ┐
M =│ 2sin2(θ/2)ωxωy+2cos(θ/2)sin(θ/2)ωz 1-2sin2(θ/2)(ωz2+ωx2) 2sin2(θ/2)ωyωz-2cos(θ/2)sin(θ/2)ωx 0 │
│ 2sin2(θ/2)ωxωz-2cos(θ/2)sin(θ/2)ωy 2sin2(θ/2)ωyωz+2cos(θ/2)sin(θ/2)ωx 1-2sin2(θ/2)(ωx2+ωy2) 0 │
└ 0 0 0 1 ┘
┌ 1-2sin2(θ/2)(ωy2+ωz2) 2sin2(θ/2)ωxωy-sin(θ)ωz 2sin2(θ/2)ωxωz+sin(θ)ωy 0 ┐
=│ 2sin2(θ/2)ωxωy+sin(θ)ωz 1-2sin2(θ/2)(ωz2+ωx2) 2sin2(θ/2)ωyωz-sin(θ)ωx 0 │
│ 2sin2(θ/2)ωxωz-sin(θ)ωy 2sin2(θ/2)ωyωz+sin(θ)ωx 1-2sin2(θ/2)(ωx2+ωy2) 0 │
└ 0 0 0 1 ┘
┌ 0 -ωz +ωy 0 ┐ ┌ ωy2+ωz2 -ωxωy -ωxωz 0 ┐
= 1+sinθ │+ωz 0 -ωx 0 │-2sin2(θ/2)│ -ωxωy ωz2+ωx2 -ωyωz 0 │
│-ωy +ωx 0 0 │ │ -ωxωz -ωyωz ωx2+ωy2 0 │
└ 0 0 0 0 ┘ └ 0 0 0 0 ┘
┌ 0 -ωz +ωy 0 ┐ ┌ ωy2+ωz2 -ωxωy -ωxωz 0 ┐
= 1+sinθ │+ωz 0 -ωx 0 │-(1-cos(θ))│ -ωxωy ωz2+ωx2 -ωyωz 0 │
│-ωy +ωx 0 0 │ │ -ωxωz -ωyωz ωx2+ωy2 0 │
└ 0 0 0 0 ┘ └ 0 0 0 0 ┘
になります。
まとめると、ベクトルの時間発展は、時間発展 quaternion q(t) を求めて、 その行列表示 M
P(t) = q(t) P(0) q(t) -1 = M P
を求めればMが回転行列になります。
q(t) は、角速度ωが一定だとすると(ωt=θ(回した角度)なので)、回転軸の方向をeとして、
q(t) = |q |(cos(ωt/2), sin(ωt/2)e)
が成り立ちます。
さて、角速度ωや回転軸eは、時間とともに変化します。
これらの量が変化する場合の q(t) を求めます。
q(t) の時間変化を求めると、
d d
―q(t) = ―(cos(ωt/2), sin(ωt/2)e)
dt dt
1 t dω
= (-―ωsin(ωt/2)-― ― sin(ωt/2)
2 2 dt
de ω t dω
, sin(ωt/2) ― + ― cos(ωt/2)e + ― ― cos(ωt/2)e)
dt 2 2 dt
d ω
―q(0) = (0, ― e)
dt 2
になります。ここで、時間発展に関係しない|q |は省きました。
この結果を、オイラー法
df
f(Δt) = f(0) + ― Δt
dt
を用いれば、
ω ω
q(Δt) = q(0) + (0, ― e) Δt = (1, ― e Δt )
2 2
になります。
この結果は、微小時間の時間発展です。
過去からの時間発展を含めると、いままでの時間発展の結果をさらに微小時間だけ時間発展させるわけですから、
ω
q(t) = q(Δt)q(t-Δt) = (1, ― e Δt )q(t-Δt)
2
が最終結果になります。q(t-Δt)は、前のフレームのq(t)です。
ちょいと Baraff と違いますが、こちらが正解でしょう。
では、実際のプログラムです。
剛体のクラスを一部変形して、回転行列の部分を quaternion にします(rigidbody.h)。
0015: class CRigidBody {
0016: private:
0017: protected:
0018: // 定数
0019: float mass;
0020: D3DXMATRIX IBody;
0021: D3DXMATRIX IBodyinv;
0022: // 状態ベクトル
0023: D3DXVECTOR4 x; // 重心座標
0024: D3DXQUATERNION q; // 回転quaternion
0025: D3DXVECTOR4 p; // 運動量
0026: D3DXVECTOR3 L; // 回転モーメント
0027: // ２次的変数
0028: D3DXVECTOR4 v;
0029: D3DXMATRIX Iinv;
0030: D3DXMATRIX R; // 回転座標
0031: D3DXVECTOR3 omega;
0032: // 外力
0033: D3DXVECTOR4 force;
0034: D3DXVECTOR3 torque;
****: ...
0047: };
２次的変数に回転座標のRを追加しました。
回転quaternion q の計算をした後に、描画に必要になる行列 R を必ず求めることにします。
回転部分の更新は次のようになります。
前の節の結果を使い、角速度ベクトルωの向きと、その速さから回転 quaternion を導出します。
その回転を前のフレームの回転 quaternion に掛けることによって、回転を合成します。
0055: // ----------------------------------------------------------------------------
0056: // 座標に時間変化を足しこむ
0057: void CRigidBody::Update(float dt)
0058: {
0059: this->ComputeForceAndTorque(dt);
0060: this->Calc2ndValues();
0061:
0062: this->x += dt * this->v;
0063: this->p += dt * this->force;
0064: this->L += dt * this->torque;
0065:
0066: D3DXQUATERNION tmp = D3DXQUATERNION(
0.5f * dt * this->omega.x
, 0.5f * dt * this->omega.y
, 0.5f * dt * this->omega.z
, 1.0f);
0067: this->q = this->q * tmp;
0068: }
ちなみに、前節における角速度ωと向きeと、角速度ベクトルωは
ω = ω e
の関係があり、その結果を使っています。
その他に回転行列を使っている部分も変更を受けます。
速度を求めるときに、同時に回転行列を求めます。
0040: // ----------------------------------------------------------------------------
0041: // v や ω を計算
0042: void CRigidBody::Calc2ndValues()
0043: {
0044: D3DXVec4Scale(&this->v, &this->p, 1.0f/this->mass);
0045:
0046: D3DXMATRIX m;
0047: D3DXMatrixTranspose(&m, &this->R);
0048: this->Iinv = this->R * this->IBodyinv * m;
0049:
0050: D3DXVec3TransformNormal(&this->omega, &this->L, &this->Iinv);
0051:
0052: D3DXQUATERNION n;
0053: D3DXMatrixRotationQuaternion(&this->R, D3DXQuaternionNormalize(&n,&this->q));
0054: }
D3DXMatrixRotationQuaternion を使うためには、規格化をしないといけないようなので、規格化しています。
D3DX には、今まで説明してきたような関数が用意されています。
D3DX関数を利用することは、省力化や安定性、再利用性を考えてもよいことだと思うので、(自分でも組めることが前提ですが)積極的に利用するのも悪くないと思います。
う～、けっこうしんどかったなぁ～
今回は大学のセミナー風に、計算をはしょらないできちんと導出してみました。
まぁ、自分のためにですけど。
やっぱり、きちんと走るようにしなくちゃいけませんね。
諸事情により、出来るかわかりませんけど…