２つの線の交点を表す数式を作ってpython matplotlibで検証

XY座標に点A1(Xa1, Ya1), A2(Xa2, Ya2), B1(Xb1, Yb1), B2(Xb2, Yb2)があったとして、

線A1A2 と 線B1B2の交点を数学的に求め、数式があってるか確かめようという話。

以下のように数学的に求めます。

• まず交点の式をだします。

• 次に上画像の一番下に書いてある方程式を解きます。

• 逆行列をどうやって求めるかは、numpyライブラリを使わずに数学公式を使います。

行列Mとその逆行列invMの関係は以下になります。

M = ((a,b),(c,d))
invM = 1/(a*d - b*c) * ((d, -b),(-c, a))

参考：
2x2行列と3x3行列と4x4行列の逆行列の公式

• 準備が整ったのでpythonで実装します。

交点を求める自作関数get_line_intersectionで上記で求めた数式を使っています。

from numpy import sin, cos, pi
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider


class point:
    def __init__(self, x, y):
        self.x = float(x)
        self.y = float(y)

def get_line_intersection(A1, A2, B1, B2):
    a = A2.y - A1.y
    b = -B2.y + B1.y
    c = A2.x - A1.x
    d = -B2.x + B1.x
    C1 = B1.y - A1.y
    C2 = B1.x - A1.x
    
    tmp = a * d - b * c
    if tmp:
        invMa = d  / tmp
        invMb = -b / tmp
        invMc = -c / tmp
        invMd = a  / tmp
        
        m = invMa * C1 + invMb * C2
        n = invMc * C1 + invMd * C2

        if 0<=m<=1 and 0<=n<=1:#両方の線上に点がある場合は交点の座標を返す
            return point(A1.x + m * (A2.x - A1.x), A1.y + m * (A2.y - A1.y))
        else:
            return point(0, 0)#辺の延長線上に交点がある場合は(0,0)を返すこととする
    else:
        return point(0, 0)#逆行列が存在しない時は強制的に（0,0)を返すことにする

################################################################
# 描画
################################################################
fig, ax = plt.subplots()
ax.set_xlim(-11,29)
ax.set_ylim(-11,29)
ax.set_aspect('equal')

plt.subplots_adjust(left=0.25, bottom=0.35)
l1, = plt.plot([6, 6], [6 + 5 * cos(pi/2), 6 + 5 * sin(pi/2)])
l2, = plt.plot([12, 12], [12 + 5 * cos(pi/2), 12 + 5 * sin(pi/2)])
l3, = plt.plot([0],[0], marker="o")


axcolor = 'lightgoldenrodyellow'
axRA = plt.axes([0.25, 0.1, 0.65, 0.03], facecolor=axcolor)
axTA = plt.axes([0.25, 0.15, 0.65, 0.03], facecolor=axcolor)

axRB = plt.axes([0.25, 0.2, 0.65, 0.03], facecolor=axcolor)
axTB = plt.axes([0.25, 0.25, 0.65, 0.03], facecolor=axcolor)


RA = Slider(axRA, 'RadiusA', 0.0, 20.0, valinit=5.0)
TA = Slider(axTA, 'ThetaA', 0.0, 4 * pi, valinit=pi/2)

RB = Slider(axRB, 'RadiusB', 0.0, 20.0, valinit=5.0)
TB = Slider(axTB, 'ThetaB', 0.0, 4 * pi, valinit=pi/2)

def update(val):
    l1.set_xdata([6, 6 + RA.val * cos(TA.val)])
    l1.set_ydata([6, 6 + RA.val * sin(TA.val)])
    l2.set_xdata([12, 12 + RB.val * cos(TB.val)])
    l2.set_ydata([12, 12 + RB.val * sin(TB.val)])
    p = get_line_intersection(point(6,6), point(6+RA.val*cos(TA.val), 6+RA.val*sin(TA.val)), point(12,12), point(12+RB.val*cos(TB.val), 12+RB.val*sin(TB.val)))
    l3.set_xdata([p.x])
    l3.set_ydata([p.y])
    fig.canvas.draw_idle()

RA.on_changed(update)
TA.on_changed(update)
RB.on_changed(update)
TB.on_changed(update)

plt.show()

• 実行結果

youtu.be

• 計算時間

CPU Core i7 2.0Ghzで計算したが計算時間は100万回やっても5秒くらいらしい

100000 loops, best of 3: 4.76 µs per loop