3차원 상의 두 직선 사이 가장 가까운 점 구하기 (서로 다른 두 점으로 이루어진 두 직선 사이 가장 가까운 점들 구하기)

3차원 상의 두 직선 사이 가장 가까운 점 구하기 (서로 다른 두 점으로 이루어진 두 직선 사이 가장 가까운 점들 구하기)

• 직선은 서로 다른 두 점으로 이루어짐
• 이런 직선이 2개가 있다고 가정하였을 때, 두 직선 위 점들 중 서로 가장 가까운 점은 어떻게 찾아낼 수 있을까?
• 계산 방식과 원리가 궁금하지만 일에서 빠르게 적용하기 위해 stackoverflow(스택오버플로우)의 도움을 받음

 

두 직선 사이 가장 가까운 점 구하는 함수

• 두 직선을 이루는 각각의 점들은 다음과 같이 표현할 수 있음

1. line A : a0 및 a1
2. line B : b0 및 b1

• 이 함수는 위에서 설명된 4점을 입력으로 받음 (a0, a1, b0, b1)
• clamp은 직선을 연장하여 볼 것인지에 대한 입력 변수임
• 출력으로 각각 line 위의 가장 가까운 점과 이 두 점 사이의 거리 값을 return 함
• 함수의 소스 코드는 아래와 같음 

import numpy as np
 
def closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=False,clampA0=False,clampA1=False,clampB0=False,clampB1=False):
 
    ''' Given two lines defined by numpy.array pairs (a0,a1,b0,b1)
    Return the closest points on each segment and their distance
    '''
 
    # If clampAll=True, set all clamps to True
    if clampAll:
        clampA0=True
        clampA1=True
        clampB0=True
        clampB1=True
 
 
    # Calculate denomitator
    A = a1 - a0
    B = b1 - b0
    magA = np.linalg.norm(A)
    magB = np.linalg.norm(B)
    
    _A = A / magA
    _B = B / magB
    
    cross = np.cross(_A, _B);
    denom = np.linalg.norm(cross)**2
    
    
    # If lines are parallel (denom=0) test if lines overlap.
    # If they don't overlap then there is a closest point solution.
    # If they do overlap, there are infinite closest positions, but there is a closest distance
    if not denom:
        d0 = np.dot(_A,(b0-a0))
        
        # Overlap only possible with clamping
        if clampA0 or clampA1 or clampB0 or clampB1:
            d1 = np.dot(_A,(b1-a0))
            
            # Is segment B before A?
            if d0 <= 0 >= d1:
                if clampA0 and clampB1:
                    if np.absolute(d0) < np.absolute(d1):
                        return a0,b0,np.linalg.norm(a0-b0)
                    return a0,b1,np.linalg.norm(a0-b1)
                
                
            # Is segment B after A?
            elif d0 >= magA <= d1:
                if clampA1 and clampB0:
                    if np.absolute(d0) < np.absolute(d1):
                        return a1,b0,np.linalg.norm(a1-b0)
                    return a1,b1,np.linalg.norm(a1-b1)
                
                
        # Segments overlap, return distance between parallel segments
        return None,None,np.linalg.norm(((d0*_A)+a0)-b0)
        
    
    
    # Lines criss-cross: Calculate the projected closest points
    t = (b0 - a0);
    detA = np.linalg.det([t, _B, cross])
    detB = np.linalg.det([t, _A, cross])
 
    t0 = detA/denom;
    t1 = detB/denom;
 
    pA = a0 + (_A * t0) # Projected closest point on segment A
    pB = b0 + (_B * t1) # Projected closest point on segment B
 
 
    # Clamp projections
    if clampA0 or clampA1 or clampB0 or clampB1:
        if clampA0 and t0 < 0:
            pA = a0
        elif clampA1 and t0 > magA:
            pA = a1
        
        if clampB0 and t1 < 0:
            pB = b0
        elif clampB1 and t1 > magB:
            pB = b1
            
        # Clamp projection A
        if (clampA0 and t0 < 0) or (clampA1 and t0 > magA):
            dot = np.dot(_B,(pA-b0))
            if clampB0 and dot < 0:
                dot = 0
            elif clampB1 and dot > magB:
                dot = magB
            pB = b0 + (_B * dot)
    
        # Clamp projection B
        if (clampB0 and t1 < 0) or (clampB1 and t1 > magB):
            dot = np.dot(_A,(pB-a0))
            if clampA0 and dot < 0:
                dot = 0
            elif clampA1 and dot > magA:
                dot = magA
            pA = a0 + (_A * dot)
 
    
    return pA,pB,np.linalg.norm(pA-pB)

 

검증 및 확인

• 검증 및 확인은 matplotlib을 통해 확인하였음
• 파란색 라인(A)과 주황색 라인(B) 위의 점과 두 점 사이를 직선으로 표현하였음 (수직을 이룸을 확인할 수 있음)

clamp를 False로 두고 확인.

clamp를 True로 두고 확인.

• 이에 대한 소스코드는 다음과 같음
  (4점의 값을 바꾸어 가며 clamp를 test 할 수 있음; closestDistanceBetweenLines가 사전에 정의되어 있어야 함)

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# need function : closestDistanceBetweenLines
 
a1=np.array([0.000015,-0.000046,387.265137])
a0=np.array([0.000004,-0.000015,52.138947])
b0=np.array([39.939014,11.414734,-4.960205])
b1=np.array([-43.097198,11.414734,5.373566])
 
t0=closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=True)[0]
t1=closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=True)[1]
 
# closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=True) # in the area
# closestDistanceBetweenLines(a0,a1,b0,b1,clampAll=False)
 
x1 = np.linspace(a1[0],a0[0],100)
y1 = np.linspace(a1[1],a0[1],100)
z1 = np.linspace(a1[2],a0[2],100)
 
x2 = np.linspace(b1[0],b0[0],100)
y2 = np.linspace(b1[1],b0[1],100)
z2 = np.linspace(b1[2],b0[2],100)
 
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
 
ax.plot(x1,y1,z1)
ax.plot(x2,y2,z2)
 
ax.scatter(t0[0], t0[1], t0[2])
ax.scatter(t1[0], t1[1], t1[2])
 
x3 = np.linspace(t1[0],t0[0],100)
y3 = np.linspace(t1[1],t0[1],100)
z3 = np.linspace(t1[2],t0[2],100)
 
ax.plot(x3,y3,z3)
 
plt.show()

• 더 정확한 검증은 수직함을 확인하거나, 여러 점 중 가장 가까운 거리 값을 가지는지 확인해볼 수 있음

 

Reference

• https://stackoverflow.com/questions/2824478/shortest-distance-between-two-line-segments

Shortest distance between two line segments