前回、剛体をちょこっとやってみたのですが、
回転部分が完全には３次元の特殊正規直行行列（まぁSO3だ）になっていないので、
計算をしつづけると、車がゆがんできます。
これを防止するために、回転を表現する別の方法である Quaternion で姿勢を制御します。
今回のソースは、次のものです（DirectX8.1用です）。
今回は、前回のソースの変更版です。
といってもGeForce3以上でないと動かないのも酷だと思ったので、普通の環境で動くようにしています。
| 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 | メニューに関する定義。 |
あと、いつもの様に、モデルと、実行ファイル及び、プロジェクトファイルが入っています。
操作方法は、
| [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 '
= (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 も変化して無いので、本当に回転を表現していることになります。
さて、quaternion を次の形で書きます。
q = |q |(cos(θ/2), sin(θ/2)ω)
ここで、ωは単位ベクトル、|q | は quaternion の大きさ（|q |=Sqrt(∥q∥2)）です。
任意の quaternion はこの形式で掛けますが、この quaterion を用いてベクトルを回転すると、
P' = q P q '
= (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 '(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 ' = M P 但し、 q = (a, v), P = (w, x)
を満たす行列 M を求めます。
変換後のベクトルを計算すると、
P' = q P q ' = (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 にします(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 R; // 回転座標
0025: D3DXVECTOR4 p; // 運動量
0026: D3DXVECTOR3 L; // 回転モーメント
0027: // ２次的変数
0028: D3DXVECTOR4 v;
0029: D3DXMATRIX Iinv;
0030: D3DXVECTOR3 omega;
0031: // 外力
0032: D3DXVECTOR4 force;
0033: D3DXVECTOR3 torque;
****: ...
0046: };
また、ワールド行列を作る時に回転行列を使うのですが、その値を得る関数もquaternionから計算した値に変更します。
D3DX に今まで計算してきた量を作ってくれる関数があるので、それを使います(rigidbody.cpp)。
0069: // ----------------------------------------------------------------------------
0070: D3DXMATRIX CRigidBody::GetRotation() const
0071: {
0072: D3DXMATRIX m;
0073:
0074: D3DXMatrixRotationQuaternion(&m, &this->R);
0075:
0076: return m;
0077: }
回転部分の更新は次のようになります。
角運動量から算出した角速度ωのベクトルの向きと、その速さから回転 quaternion を導出します。
その回転を前のフレームの回転 quaternion に掛けることによって、回転を合成します。
回転量が非常に小さい時は、回転しないものとして、回転処理を省きます。
0052: // ----------------------------------------------------------------------------
0053: // 座標に時間変化を足しこむ
0054: void CRigidBody::Update(float dt)
0055: {
0056: this->ComputeForceAndTorque(dt);
0057: this->Calc2ndValues();
0058:
0059: this->x += dt * this->v;
0060: this->p += dt * this->force;
0061: this->L += dt * this->torque;
0062: D3DXQUATERNION dr;
0063: float arg = -dt*D3DXVec3Length(&this->omega);
0064: if(arg < -0.001f || 0.001f < arg){
0065: D3DXQuaternionRotationAxis( &dr, &this->omega, arg);
0066: D3DXQuaternionMultiply(&this->R, &this->R, &dr);
0067: }
0068: }
その他に回転行列を使っている部分も変更を受けます。
角速度の計算において、ワールド座標での慣性テンソルを求める時に、一度回転行列を求めてからその逆行列で計算します。
0039: // ----------------------------------------------------------------------------
0040: // v や ω を計算
0041: void CRigidBody::Calc2ndValues()
0042: {
0043: D3DXVec4Scale(&this->v, &this->p, 1.0f/this->mass);
0044:
0045: D3DXMATRIX m, mt;
0046: D3DXMatrixRotationQuaternion(&m, &this->R);
0047: D3DXMatrixTranspose(&mt, &m);
0048: this->Iinv = m * this->IBodyinv * mt;
0049:
0050: D3DXVec3TransformNormal(&this->omega, &this->L, &this->Iinv);
0051: }
それ以外の部分も、とりあえず行列を作って、回転部分を処理しています。
ここは、綺麗な方法がありそうですね。
う～、けっこうしんどかったなぁ～
今回は大学のセミナー風に、計算をはしょらないできちんと導出してみました。
まぁ、自分のためにですけど。
やっぱり、きちんと走るようにしなくちゃいけませんね。
諸事情により、出来るかわかりませんけど…