Pythonで加速度をもとに3D空間上の2つの球が衝突するかどうかを判定するプログラム

半径 r1 と r2 の2つの球が3次元空間上にあると仮定します。それぞれの球の中心は座標 (x1, y1, z1) と (x2, y2, z2) にあり、加速度は (ax1, ay1, az1) と (ax2, ay2, az2) として与えられています。このとき、2つの球が与えられた加速度で動き続けた場合、将来のある時点で出会う（衝突する）ことがあるかどうかを判定するプログラムを作成します。

たとえば、入力が r1 = 1、r2 = 2、pos1 = (0, 0, 0)、acc1 = (100, 0, 0)、pos2 = (4, 0, 0)、acc2 = (0, 0, 0) のような場合、出力は True になります。2つ目の球には加速度がないため動かず、その場にとどまります。一方、1つ目の球はx方向へ加速しながら移動するため、やがて2つの球は衝突します。

解法の考え方

この問題は相対運動の考え方を使うと効率的に解けます。片方の球を基準に見ると、もう片方の球は「相対位置ベクトル」と「相対加速度ベクトル」に従って動きます。2つの球の中心間距離の2乗は、あるパラメータ x を用いて次の二次式で表されます。

dis² = da × x² + 2 × co × x + dp

ここで各変数の意味は以下のとおりです。

• da: 相対加速度ベクトルの大きさの2乗
• dp: 相対位置ベクトルの大きさの2乗
• co: 相対加速度ベクトルと相対位置ベクトルの内積

この二次関数を最小化することで、2つの球が最も接近する瞬間の距離が求められます。その最小距離が両球の半径の和 r1 + r2 以下であれば、2つの球は必ず出会うことになります。

解く手順

以下の手順に従って問題を解きます。

• px := pos1[0] − pos2[0]
• py := pos1[1] − pos2[1]
• pz := pos1[2] − pos2[2]
• ax := acc1[0] − acc2[0]
• ay := acc1[1] − acc2[1]
• az := acc1[2] − acc2[2]
• da := ax × ax + ay × ay + az × az
• dp := px × px + py × py + pz × pz
• co := ax × px + ay × py + az × pz
• x := 0.0
• da が 0 と等しくない場合は、x := −co / da とする
• x := max(x, 0)
• dis := √(da × x × x + 2 × co × x + dp)
• dis ≤ r1 + r2 の場合は True を返す
• そうでなければ False を返す

x := max(x, 0) としているのは、二次式の最小値を与える点が負になる場合でも、時間を過去にさかのぼることはできないため、開始時点（x = 0）における距離を採用するためです。

実装例

理解を深めるために、以下の実装を見てみましょう。

def solve(r1, r2, pos1, acc1, pos2, acc2):
    px, py, pz = pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]
    ax, ay, az = acc1[0] - acc2[0], acc1[1] - acc2[1], acc1[2] - acc2[2]
    da = (ax * ax + ay * ay + az * az)
    dp = (px * px + py * py + pz * pz)
    co = (ax * px + ay * py + az * pz)

    x = 0.0
    if da != 0:
        x = - co / da
    x = max(x, 0)
    dis = (da * x * x + 2 * co * x + dp) ** 0.5
    if dis <= r1 + r2:
        return True
    else:
        return False

r1 = 1
r2 = 2
pos1 = (0, 0, 0)
acc1 = (100, 0, 0)
pos2 = (4, 0, 0)
acc2 = (0, 0, 0)
print(solve(r1, r2, pos1, acc1, pos2, acc2))

入力

1, 2, (0, 0, 0), (100,0,0), (4, 0, 0), (0,0,0)

出力

True

このコードでは、1つ目の球がx方向へ加速しながら静止している2つ目の球に向かって移動するため、2つの球の中心間距離は最終的に0まで縮まります。この距離は半径の和 r1 + r2 = 3 よりも小さいため、「2つの球は必ず出会う（衝突する）」という判定結果 True が得られます。