線分と円の当たり判定 - 電算倶楽部　富山県のコンピュータ社会人サークル

概要

前の記事の続きで、カメラで撮った画像から線を検出し、それが正しい位置にあるかを判定するプログラムを目指して開発していきます。今回は線分と円が交わっているかを判定するプログラムについて考えてみます。

まず解法（アルゴリズム）を考えていきます。当たり判定を行うライブラリもあるかと思いますが、このくらいならば高校数学の知識でアルゴリズムを作ることができます。

問題の整理

線分と円が交わっているか、と書いても曖昧なので、以下のように整理します。

問題: 点$O$を中心とする半径$r$の円(内部含む)と、点$A$と点$B$を結ぶ線分が衝突している条件を求めよ。ただし、円の半径$r$は$0$よりも大きく、点$A$と点$B$は同一でないものとする。

色々と解き方がありますが、今回はベクトルで解く¹前提で、もう少し条件を整理します。

次の1~4を満たす点$P$が存在すること。

点$P$は点$O$を中心とする半径$r$の円に含まれる。すなわち、 $ |\overrightarrow{OP}| \le r $が成り立つ。
点Pは線分$AB$上に位置する。すなわち、ある0以上1以下の実数$s$が存在し、 $\overrightarrow{OP} = (1-s)\overrightarrow{OA} + s\overrightarrow{OB}$ が成り立つ。
$r > 0$
$\overrightarrow{OA} \not= \overrightarrow{OB}$

f:id:s-densan:20200922210148p:plain

解法

詳細は飛ばしますが、次の2次不等式を解けばOKです。

$$ (|\overrightarrow{OA}|^{2} + |\overrightarrow{OB}|^{2} - 2\overrightarrow{OA} \cdot \overrightarrow{OB})s^{2} + (-2|\overrightarrow{OA}|^{2} + 2\overrightarrow{OA} \cdot \overrightarrow{OB})s + (|\overrightarrow{OA}|^{2} - r^{2}) \le 0 $$

簡単のために、実数$a$,$b$,$c$を定義しておきます。 $$ a = |\overrightarrow{OA}|^{2} + |\overrightarrow{OB}|^{2} - 2\overrightarrow{OA} \cdot \overrightarrow{OB} $$ $$ b = -2|\overrightarrow{OA}|^{2} + 2\overrightarrow{OA} \cdot \overrightarrow{OB} $$ $$ c = |\overrightarrow{OA}|^{2} - r^{2} $$

なお、$s^{2}$の係数である$a$は必ず0よりも大きくなります²のでプログラムを作るときは考慮しなくてOKです。

2次方程式$as^{2} + bs + c = 0$の判別式$D$を求めます。 $$ D = b ^ {2} - 4 a c $$

下に凸の2次関数なので、$D<0$の場合は解無しです。

$D\ge0$とし、2次方程式$as^{2} + bs + c = 0$の解を求めます。

$$ s = \frac{-b \pm \sqrt{b^{2} - 4ac}}{2a} $$

$f(s) = 0$ の2解を$s_1$, $s_2$ （ $ s_1 \le s_2 $ 、重解の場合は $ s_1 = s_2 $ ）とすると、$s_1 \le 1$ かつ $0 \le s_2$ ならば $s$ が存在します。

つまり、$D\ge0$ かつ $s_1 \le 1$ かつ $0 \le s_2$ が条件となります。

プログラムを作る

長々と書きましたが、数式を作ってしまえばそのままプログラムにしてしまえば動きます。（本当は式を変形して根号や割り算を極力減らしたほうが高速になります）

import math

def collision_detect(circle, line):
    # ベクトルの大きさの2乗を求める
    def len_vec2(vec):
        return vec[0] * vec[0] + vec[1] * vec[1]
    # 2つのベクトルの内積を求める
    def inner_prod(vec1, vec2):
        return vec1[0] * vec2[0] + vec1[1] * vec2[1]
    # 円の中心座標
    pos_o = circle[0], circle[1]
    # 円の半径
    r = circle[2]
    # 線分の端点1(点A)
    pos_a = line[0], line[1]
    # 線分の端点2(点B)
    pos_b = line[2], line[3]

    # A=Bの場合は線分でないため衝突していないとする
    if pos_a == pos_b:
        return False

    # r <= 0 の場合は円のサイズが無いため衝突していないとする
    if r <= 0:
        return False

    vec_oa = pos_a[0] - pos_o[0], pos_a[1] - pos_o[1]
    vec_ob = pos_b[0] - pos_o[0], pos_b[1] - pos_o[1]

    len_oa2 = len_vec2(vec_oa)
    len_ob2 = len_vec2(vec_ob)
    inner_ab = inner_prod(vec_oa, vec_ob)

    # ABをs : 1-s で内分した点Pが円Oに含まれる条件を立式し、整理する。
    # 2次不等式 as^2 + bs + c <= 0 と表し、係数を変数に代入。
    # ※a > 0
    a = len_oa2 + len_ob2 - 2 * inner_ab
    b = -2 * len_oa2 + 2 * inner_ab
    c = len_oa2 - r * r
    
    # 2次方程式の解の判定式
    det = b * b - 4 * a * c
    if det < 0:
        return False

    # 解の公式をとき、2解をそれぞれs1, s2に設定
    s1 = (-b - math.sqrt(det)) / (2 * a)
    s2 = (-b + math.sqrt(det)) / (2 * a)

    return (s1 <= 1) and (0 <= s2)

print(collision_detect((0, 0, 10), (10, 0, 0, 10)))
print(collision_detect((0, 0, 10), (15, 0, 0, 15)))