剛体物理シミュレーションを書いた（衝突応答編） - AdC用にとりあえず用意したブログ

↑ https://roodni.github.io/rigidbody/ で遊べます

想像以上に安定して動作したので、実装方法について記しておきます。

作るもの

2Dの剛体シミュレーション
衝突・静止摩擦・動摩擦

剛体が持つ情報

質量 $m$
重心回りの慣性モーメント $I$
重心の位置 $\vec{x}$
重心の速度 $\vec{v}$
重心の加速度 $\vec{a}$
角度 $\theta$
角速度 $\omega$
角加速度 $\alpha$

ステップ（時間を進める）

重力加速度の加算
数値積分
衝突判定
衝突応答
加速度・角加速度のリセット

1. 重力加速度の加算

剛体の加速度（毎回リセットされて $\vec{0}$ になる）に重力加速度を加えてください。重力以外の力もあれば同様に加速度や角加速度に加えるのが良いと思います。

速度に直接加算するのは、本記事の枠組みの中ではよろしくないです。

速度に直接加算すると加えた力が剛体の位置に瞬時に反映されます。すると衝突応答（剛体の速度を変更する）では吸収できない振動が生じてしまいます。
加速度は衝突応答の安定化のために利用できます。

2. 数値積分

オイラー法を使いました。一番単純なやつです。

進める時間を $\Delta t$ として:

$\begin{align} \vec{x}_{i+1} &= \vec{x}_i + \vec{v}\Delta t \\ \vec{v}_{i+1} &= \vec{v}_i + \vec{a}\Delta t \\ \theta_{i+1} &= \theta_i + \omega\Delta t \\ \omega_{i+1} &= \omega_i + \alpha\Delta t \end{align}$

3. 衝突判定

私の実装では今のところ衝突判定で剛体の速度を考慮していません。位置が重なっているかどうかだけを見ます。速すぎると貫通しますが……大抵は大丈夫です。ちゃんとした物理エンジンは貫通対策をしているらしいです、すごいですね。

本記事では衝突判定の詳細には触れません。どうにかして衝突点・法線ベクトル・めり込みの深さを取得してください。面衝突の場合は衝突点が2個あると安定します。

ちなみに凸多角形同士の衝突判定には次の2つの方法が有力な選択肢です:

分離軸判定
GJK-EPA

4. 衝突応答

本記事での衝突応答とは「衝突を剛体の速度に関する拘束と見なして、拘束を満たすように撃力を加えて、剛体の速度を瞬間的に変更する」ことです。

具体例を出しましょう。剛体1と剛体2が反発係数 $e$ で衝突して、衝突前の法線方向の相対速度が $v_{12}$ だったとします。このとき、衝突後の法線方向の相対速度は $-ev_{12}$ であってほしいです。それを満たすように撃力を加えます。

もし静止摩擦があれば、衝突後の接線方向の相対速度がゼロになるという拘束も加わります。ただし摩擦力の上限が垂直抗力の大きさに比例することに注意が必要です。他にもいろいろ考慮して拘束条件を組み立てます。

撃力を求めるには複数の衝突をまとめてLCP（線形相補性問題）に定式化して解くのが一般的らしいですが、本記事では各衝突をそれぞれ単体で解いて撃力を与えるのを繰り返す方法を扱います。

剛体の衝突と撃力

剛体1と剛体2が衝突するときに、剛体1から剛体2に与えられる撃力 $\vec{F}$ を求めます。他の衝突はここでは無視します。

衝突位置 $\vec{p}$
剛体1から剛体2への衝突法線 (単位ベクトル） $\vec{n}$
接線（単位ベクトル） $\vec{t} = R(\frac{\pi}{2}) \vec{n}$ （ $R$ は回転行列）
反発係数 $e$
摩擦係数 $\mu$ （簡単のため静止摩擦係数 = 動摩擦係数とする）

その他文字一覧:

$\begin{align} \vec{r}_1 &= \vec{p} - \vec{x}_1 \\ \vec{r}_2 &= \vec{p} - \vec{x}_2 \\ M^{-1}_{n} &= \frac{1}{m_1} + \frac{1}{m_2} + \frac{(\vec{r}_1\times\vec{n})^2}{I_1} + \frac{(\vec{r}_2\times\vec{n})^2}{I_2} \\ M^{-1}_{t} &= \frac{1}{m_1} + \frac{1}{m_2} + \frac{(\vec{r}_1\times\vec{t})^2}{I_1} + \frac{(\vec{r}_2\times\vec{t})^2}{I_2} \\ M^{-1}_{nt} &= \frac{(\vec{r}_1\times\vec{n})(\vec{r_1}\times\vec{t})}{I_1} + \frac{(\vec{r}_2\times\vec{n})(\vec{r_2}\times\vec{t})}{I_2} \end{align}$

（ $\times$ は外積です。2次元での外積はスカラーになることに留意してください）

導出は省きますが、位置 $\vec{p}$ での相対速度 $\vec{\nu}_{12}$ と、撃力 $\vec{F}=\lambda_n\vec{n} + \lambda_t\vec{t}$ を相互に作用させたあとの相対速度 $\vec{\nu}'_{12}$ はそれぞれ次のように表せます。

$\begin{align} \vec{\nu}_{12} &= \vec{v}_2 + \omega_2 R\left(\frac{\pi}{2}\right) \vec{r}_2 - \vec{v}_1 - \omega_1 R\left(\frac{\pi}{2}\right) \vec{r}_1 \\ \vec{n}\cdot\vec{\nu}'_{12} &= \vec{n}\cdot\vec{\nu}_{12} + \lambda_n M^{-1}_n + \lambda_t M^{-1}_{nt} \\ \vec{t}\cdot\vec{\nu}'_{12} &= \vec{t}\cdot\vec{\nu}_{12} + \lambda_n M^{-1}_{nt} + \lambda_t M^{-1}_{t} \end{align}$

まずは静止摩擦が働く場合の撃力を求めてみましょう。法線方向の目標相対速度を $u$ （基本は $u=-e\vec{n}\cdot\vec{\nu}_{12}$ ですが補正が入ります）として、連立方程式

$\left\{\begin{aligned} \vec{n}\cdot\vec{\nu}'_{12} &= u \\ \vec{t}\cdot\vec{\nu}'_{12} &= 0 \end{aligned}\right.$

を $\lambda_n$ と $\lambda_t$ について解きます。解は省略しますがゼロ除算は発生しないので安心してください。

さて、解が $|\lambda_t| \leq \mu\lambda_n$ を満たしていればそれで良いのですが、そうでなければ動摩擦に切り替えます。先ほど求めた $\lambda_t$ の符号を $s$ として、 $\lambda_t = s\mu\lambda_n$ を $\vec{n}\cdot\vec{\nu}'_{12} = u$ に代入して解けば、次の解が得られます:

$\displaystyle \lambda_n = \frac{u-\vec{n}\cdot\vec{\nu}_{12}}{M^{-1}_{n} + s\mu M^{-1}_{nt}}$

先ほどの連立方程式の解について $|\lambda_t| > \mu\lambda_n$ ならば $M^{-1}_{n} + s\mu M^{-1}_{nt} > 0$ が成り立つので安心してください。

以上で撃力 $\vec{F}=\lambda_n\vec{n} + \lambda_t\vec{t}$ が求まりました。ただし法線方向の目標相対速度 $u$ の設定についてもう少し考える必要があります。

法線方向の目標相対速度の設定

ベースは $u=-e\vec{n}\cdot\vec{\nu}_{12}$ ですが、安定化のために補正を入れます。

重力加速度による補正

重力加速度 $g$ で落下する質点が水平面に速さ $v$ で衝突して、反発係数 $e$ で跳ね返ったとします。計算は省きますが、本記事で説明した数値積分の方法では、再衝突の速さは最大で $ev + g\Delta t$ になります。

再衝突の速さが本来より $g\Delta t$ だけ大きくなる可能性があるわけです。このままでは、とくに反発係数が大きいときに、相対速度がゼロに収束せず跳ね続けるという事態が発生します。

そこで目標相対速度から $g\Delta t$ に相当する量を引くとうまくいきました。具体的には $u$ の値を次のとおりに変更します:

\begin{align} u_{g} &= \min(0,\ \vec{n}\cdot \vec{g}\Delta t) \\ u &= \max(0,\ -e\vec{n}\cdot \vec{\nu}_{12} + u_g) \end{align}

$\vec{g}$ のかわりに剛体の相対加速度を使うのもアリだと思います。

ついでに、衝突前の相対速度が十分小さいとき、この補正によって目標相対速度がゼロになります。これで床に置かれた剛体が重力による加速で振動することも防げます。

めり込み解消のための補正

めり込みの深さを $\mathit{depth}$ 、許容めり込み量を $\mathit{slop}$ として $\frac{\mathit{depth}-\mathit{slop}}{\Delta t}$

ただし $\mathit{depth}$ が大きいと反発係数以上に反発することになるので、適切に上限を設定した方が良いかもしれません。

反復

各衝突をそれぞれ単体で解くのを繰り返して撃力を伝播させます。

最初に各衝突について法線方向の目標相対速度を計算しておきます。これをすべて達成することを目指して撃力を計算します。反復の途中で目標相対速度を更新する必要はたぶんないです。

以下を繰り返す:

各衝突について以下の操作を行う:
1. 前回の反復で与えた撃力を反転して速度に加算する（打ち消す）
2. 法線方向の相対速度が目標相対速度より大きければ何もしない。撃力はゼロ
3. そうでなければ、上で述べた計算式で撃力を計算する
4. 撃力を剛体の速度に加算する

反復を開始する前に前回のタイムステップで求めた撃力を加えること (Warm start) で撃力計算が安定します。時刻をまたいだ衝突点の同定がやや面倒ですが、本当に目に見えて安定するのでおすすめです。

おわり

ここまで実装すれば動きます。

本記事では私が実装で迷った部分にできるだけ言及したつもりです。実のところ、数値積分の方法からして正しいかどうか自信がないのですが、それっぽく動いたので大丈夫ということにして記事を書きました。物理エンジン自作に興味があるがよくわからない、という方の参考になれば幸いです。