物理の駅 Physics station by 現役研究者

テクノロジーは共有されてこそ栄える

Pythonで直線同士の最接近距離と最接近点を知る

まずは、

その19 直線同士の最接近距離と最接近点を知る

Pythonへ移植する

import numpy as np

# http://marupeke296.com/COL_3D_No19_LinesDistAndPos.html
def distance_2lines(line1, line2):
    '''
    直線同士の最接近距離と最接近点
    return 直線間の距離, line1上の最近接点、line2上の最近接点
    '''
    line1 = [np.array(line1[0]),np.array(line1[1])]
    line2 = [np.array(line2[0]),np.array(line2[1])]

    if abs(np.linalg.norm(line1[1]))<0.0000001:
        return None,None,None
    if abs(np.linalg.norm(line2[1]))<0.0000001:
        return None,None,None

    p1 = line1[0]
    p2 = line2[0]

    v1 = line1[1] / np.linalg.norm(line1[1])
    v2 = line2[1] / np.linalg.norm(line2[1])

    d1 = np.dot(p2 - p1,v1)
    d2 = np.dot(p2 - p1,v2)
    dv = np.dot(v1,v2)

    if (abs(abs(dv) - 1) < 0.0000001):
        v = np.cross(p2 - p1,v1)
        return np.linalg.norm(v),None,None

    t1 = (d1 - d2 * dv) / (1 - dv * dv)
    t2 = (d2 - d1 * dv) / (dv * dv - 1)

    #外挿を含む最近接点
    q1 = p1 + t1 * v1
    q2 = p2 + t2 * v2

    return np.linalg.norm(q2 - q1), q1, q2

愚直に書いてはみたが、もっといい方法があればこっそり教えてほしい。

次に3次元の直線の描画

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def draw_3dline(lines):
    '''
    3次元の直線を描画する
    '''
    fig = plt.figure()
    ax = Axes3D(fig)
    for line in lines:
        line = np.array([np.array(line[0]),np.array(line[0])+np.array(line[1]),np.array(line[0])-np.array(line[1])])
        ax.plot(line[:,0], line[:,1], line[:,2], "o-", ms=4, mew=0.5)

    # 軸ラベル
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_zlabel('z')

    # 表示
    plt.show()
line1 = [[0,0,0],[0,1,0]]
line2 = [[0,0,0],[1,0,0]]
res = distance_2lines(line1,line2)
print(res)
draw_3dline([line1,line2,[(res[1]+res[2])*0.5,[0,0,0]]])

f:id:onsanai:20200227182226p:plain

line1 = [[0,0,0],[0,1,0]]
line2 = [[0,0,0],[0,0,1]]
res = distance_2lines(line1,line2)
print(res)
draw_3dline([line1,line2,[(res[1]+res[2])*0.5,[0,0,0]]])

f:id:onsanai:20200227181921p:plain

少し距離が離れているケース

line1=[[0,0,0],[0,1,0]]
line2=[[0.2,0,0],[0,0,1]]
res = distance_2lines(line1,line2)
print(res)
draw_3dline([line1,line2,[(res[1]+res[2])*0.5,[0,0,0]]])

f:id:onsanai:20200227182318p:plain

並行な直線のケース

line1=[[0,0,0],[1,1,1]]
line2=[[1,0,0],[1,1,1]]
res = distance_2lines(line1,line2)
print(res)
draw_3dline([line1,line2])

f:id:onsanai:20200227182023p:plain