点が三角形と四面体の中にあるか調べる方法

11861 ワード
Python 線形代数 幾何学 Python テキストリンク

はじめに

点群の処理など行う場合に、この判定を行いたいときがあります。以下のように外積を使用する方法が一般に知られているようです。
https://blog.goo.ne.jp/field_light/e/9692d20174a8045b674d14a260a244a5

今回のこの記事では逆行列を用いて判定に必要な時間を削減させています。

このすばらしい方法を思いついた時にいくらgoogle検索しても同様な方法はヒットしませんでした。これは世紀の発見かと思いましたが、以下のPythonコードを組んでいく中でここで既に提案されていると知ってテンション下がりました。少なくとも日本語での解説ページは無いみたいなので書いておきます(レイトレーシングの解説中にあるかも知れません)。

今は東京オリンピック開催中のお盆。5連の台風が通り過ぎています。

三角形の点の内外判定

理論

まず2次元の三角形ABCの中に点Pが含まれるか否かの判定方法を説明します。以降は点名ABCPでは無くベクトル名$\vec{a},\vec{b},\vec{c},\vec{p}$で説明します。

$\vec{p}$は以下のように表すことができます。

\vec{p} = w_a\vec{a} + w_b\vec{b}

これをx,y成分を明示して表現します。

\begin{equation}
\begin{split}
\begin{bmatrix}
p_x \\
p_y \\
\end{bmatrix}
& = w_a
\begin{bmatrix}
a_x \\
a_y \\
\end{bmatrix}
+
w_b
\begin{bmatrix}
b_x \\
b_y \\
\end{bmatrix}
\\
& =
\begin{bmatrix}
w_a a_x + w_b b_x \\
w_a a_y + w_b b_y \\
\end{bmatrix}
\end{split}
\end{equation}

上記を変形すると、逆行列を使って$w_a, w_b$を求める式に出来ます。

\begin{equation}
\begin{split}
\begin{bmatrix}
p_x \\
p_y \\
\end{bmatrix}
& =
\begin{bmatrix}
a_x & b_x \\
a_y & b_y \\
\end{bmatrix}
\begin{bmatrix}
w_a \\
w_b \\
\end{bmatrix}
\\
\begin{bmatrix}
w_a \\
w_b \\
\end{bmatrix}
& =
\begin{bmatrix}
a_x & b_x \\
a_y & b_y \\
\end{bmatrix}^{-1}
\begin{bmatrix}
p_x \\
p_y \\
\end{bmatrix}

\end{split}
\end{equation}

この$w_a, w_b$が以下条件を満たす場合に三角形中にあると言えます。

\begin{cases}
0 \leq w_a \leq 1\\
0 \leq w_b \leq 1\\
w_a+w_b \leq 1
\end{cases}

Pythonコード

a,b,c,pはa = np.matrix([[2],[2]])みたいに定義してます。

def is_in_triangle(a, b, c, p):
    v_a = b - a
    v_b = c - a
    v_p = p - a
    A = np.concatenate([v_a, v_b], axis=1)
    w = np.linalg.inv(A) * v_p
    if np.all(w>=0)and np.all(w<=1) and w.sum() <= 1.0:
        return True
    else:
        return False

結果

ランダムで点を発生させ、上記コードで中にあると判定できた場合は赤で点を打っています。コードはこちら

速度比較

10万点の処理時間は以下でした。今回提示した逆行列を使用した方法が1.58倍早いです。

方法名 時間[sec] コードリンク
外積 48.5 リンク
逆行列 30.7 リンク

四面体の点の内外判定

理論

三角形と同様で、サイズが一つ増えただけです。

\begin{bmatrix}
w_a \\
w_b \\
w_c \\
\end{bmatrix}
 =
\begin{bmatrix}
a_x & b_x & c_x \\
a_y & b_y & c_y \\
a_z & b_z & c_z \\
\end{bmatrix}^{-1}
\begin{bmatrix}
p_x \\
p_y \\
p_z \\
\end{bmatrix}

条件も同様に以下です。

\begin{cases}
0 \leq w_a \leq 1\\
0 \leq w_b \leq 1\\
0 \leq w_c \leq 1\\
w_a+w_b+w_c \leq 1
\end{cases}

Pythonコード

自分でも作りましたが速度的に早かったのでここから取ってきた以下を採用しました。
上記と異なりa=np.array([1,2,3])のように点を定義してます。

def tetraCoord_Dorian(A,B,C,D):
    v1 = B-A ; v2 = C-A ; v3 = D-A
    # mat defines an affine transform from the tetrahedron to the orthogonal system
    mat = np.array((v1,v2,v3)).T
    # The inverse matrix does the opposite (from orthogonal to tetrahedron)
    M1 = np.linalg.inv(mat)
    return(M1) 

def pointInside_Dorian(v1,v2,v3,v4,p):
    # Find the transform matrix from orthogonal to tetrahedron system
    M1=tetraCoord_Dorian(v1,v2,v3,v4)
    # apply the transform to P
    newp = M1.dot(p-v1)
    # perform test
    return (np.all(newp>=0) and np.all(newp <=1) and np.sum(newp)<=1)

結果

ランダムで点を発生させ、上記コードで中にあると判定できた場合は赤で点を打っています。コードはこちら

終わりに

逆行列つかってますので、正則でない場合エラーとなりますので例外処理必要です。そもそも面積・体積が0のぺしゃんこな形みたいな場合にそうなるので気にする必要ないかも知れませんが。

参考

https://stackoverflow.com/a/61703017
https://www.youtube.com/watch?v=HYAgJN3x4GA&list=PLJRWtU2MT2WUesYENZXO9PFvMQW9OJuAl&index=1