まずは、
を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]]])
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]]])
少し距離が離れているケース
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]]])
並行な直線のケース
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])