三维异面直线的距离和垂足求解

ゝ一世哀愁。 2023-08-17 16:03 90阅读 0赞

在论文中遇到了求解三维异面直线的距离和垂足的问题，网上资源参差不齐，遂把它记录下来，万一你也用得上呢

本文参考自：[求3维度空间异面直线的垂足点][3]

在此基础上做了更详细的论述、扩展和python实现

**题设：假设有两条直线 L1，L2 ，以及两条直线的方向向量V1，V2，求其最短距离及其垂足。**

#### 直接求距离 ####

对于最短距离，也即两异面直线公垂线的长度，有一个方便快捷的求解方法，选择L1上任意一点P1连接L2上任意一点P2，则线段P1P2在L1，L2的公垂线上的投影即是长度，共垂线可以通过方向向量叉乘得到（注意两直线平行时，该方法无法得到距离），下面为python求解代码：

def distace_3d(p1, p2, q1, q2):
        # 叉乘方法计算三维异面直线距离
        
        # p1,p2为L1上的两个节点，q1,q2为L2上的两个节点
        v1 = np.array([p2[i] - p1[i] for i in range(len(p1))])
        v2 = np.array([q2[i] - q1[i] for i in range(len(q1))])
        
        # 叉乘公式：(a1,a2,a3) X (b1,b2,b3)=(a2b3-a3b2,a3b1-a1b3,a1b2-a2b1)
        chacheng = np.array([v1[1]*v2[2]-v1[2]*v2[1],v1[2]*v2[0]-v1[0]*v2[2],v1[0]*v2[1]-v1[1]*v2[0]])
        segment = np.array([p1[i] - q1[i] for i in range(len(p1))])
        dis = abs(sum(chacheng*segment)) / math.sqrt(sum([chacheng[i]**2 for i in range(len(chacheng))]))
        return dis

#### 求垂足加距离 ####

异面直线共垂线的垂足点求解，就没有上面那么容易了，基本思路如下：

因为直线的定义可以给出如下直线方程  
 L ( t ) = P + t ∗ V L(t) = P + t\*V L(t)=P\+t∗V

则在L1和L2上分别选择任意的p1,p2,以及对应的t1,t2,得到的 L 1 ( t 1 ) , L 2 ( t 2 ) L1(t1),L2(t2) L1(t1),L2(t2)，有如下等式  
 L 1 ( t 1 ) = P 1 + t 1 ∗ V 1 − − − − − − − − − − − − − − − − − − − − − − − − − − ( 1 ) L1(t1) = P1 + t1\*V1--------------------------(1) L1(t1)=P1\+t1∗V1−−−−−−−−−−−−−−−−−−−−−−−−−−(1)

L 2 ( t 2 ) = P 2 + t 2 ∗ V 2 − − − − − − − − − − − − − − − − − − − − − − − − − − ( 2 ) L2(t2) = P2 + t2\*V2--------------------------(2) L2(t2)=P2\+t2∗V2−−−−−−−−−−−−−−−−−−−−−−−−−−(2)

如果 L 1 ( t 1 ) , L 2 ( t 2 ) L1(t1),L2(t2) L1(t1),L2(t2)刚好是各自直线的垂足点，那么可以得出此时 ∣ L 2 ( t 2 ) − L 1 ( t 1 ) ∣ | L2(t2)-L1(t1) | ∣L2(t2)−L1(t1)∣ 的模长为最小值，且构成的  L 2 ( t 2 ) − L 1 ( t 1 ) L2(t2)-L1(t1) L2(t2)−L1(t1) 向量刚好就是公垂线，因此v1和 v2都和公垂线垂直，所以，v1和v2各自和公垂线的点乘都为0

于是我们可以写出如下等式  
 （ L 2 ( t 2 ) − L 1 ( t 1 ) ） ⋅ V 1 = 0 （ L2(t2)-L1(t1) ）\\cdot V1 = 0 （L2(t2)−L1(t1)）⋅V1=0  
 （ L 2 ( t 2 ) − L 1 ( t 1 ) ） ⋅ V 2 = 0 （ L2(t2)-L1(t1) ）\\cdot V2 = 0 （L2(t2)−L1(t1)）⋅V2=0

即  
 （ t 2 ∗ V 2 − t 1 ∗ V 1 + P 2 − P 1 ） ⋅ V 1 = 0 （t2\*V2-t1\*V1 + P2 - P1 ）\\cdot V1 = 0 （t2∗V2−t1∗V1\+P2−P1）⋅V1=0  
 （ t 2 ∗ V 2 − t 1 ∗ V 1 + P 2 − P 1 ） ⋅ V 2 = 0 （t2\*V2-t1\*V1 + P2 - P1 ）\\cdot V2 = 0 （t2∗V2−t1∗V1\+P2−P1）⋅V2=0

整理

t 2 V 2 ⋅ V 1 − t 1 V 1 ⋅ V 1 + ( P 2 − P 1 ) ⋅ V 1 = 0 t2V2\\cdot V1 - t1V1\\cdot V1 + (P2 - P1)\\cdot V1 = 0 t2V2⋅V1−t1V1⋅V1\+(P2−P1)⋅V1=0  
 t 2 V 2 ⋅ V 2 − t 1 V 1 ⋅ V 2 + ( P 2 − P 1 ) ⋅ V 2 = 0 t2V2\\cdot V2 - t1V1\\cdot V2 + (P2 - P1)\\cdot V2 = 0 t2V2⋅V2−t1V1⋅V2\+(P2−P1)⋅V2=0

令  
 a = V 1 ⋅ V 2 = V 2 ⋅ V 1 a = V1 \\cdot V2 = V2\\cdot V1 a=V1⋅V2=V2⋅V1  
 b = V 1 ⋅ V 1 b = V1\\cdot V1 b=V1⋅V1  
 c = V 2 ⋅ V 2 c = V2\\cdot V2 c=V2⋅V2  
 d = ( P 2 − P 1 ) ⋅ V 1 d = (P2 - P1) \\cdot V1 d=(P2−P1)⋅V1  
 e = ( P 2 − P 1 ) ⋅ V 2 e = (P2 - P1)\\cdot V2 e=(P2−P1)⋅V2

则上式可化简为

a t 2 − b t 1 + d = 0 at2 - bt1 + d = 0 at2−bt1\+d=0  
 c t 2 − a t 1 + e = 0 ct2 - at1 + e = 0 ct2−at1\+e=0

以下分三种情形讨论

1、当a = 0 ，即  V 1 ⋅ V 2 = V 2 ⋅ V 1 = 0 V1 \\cdot V2 = V2\\cdot V1 = 0 V1⋅V2=V2⋅V1=0时，表明原始两条直线互相垂直，则  
 t 1 = d / b t1 = d / b t1=d/b  
 t 2 = − e / c t2 = -e / c t2=−e/c

2、当a != 0时候，解上述方程得（不同的消元方式会得到不同形式的解）  
 t 1 = ( a e − c d ) / ( a a − b c ) t1 = (ae -cd)/(aa-bc) t1=(ae−cd)/(aa−bc)  
 t 2 = b / a ∗ t 1 − d / a t2 = b/a \* t1 - d/a t2=b/a∗t1−d/a

3、这里发现当 ( a ∗ a − b ∗ c ) = 0 (a \* a - b \* c) = 0 (a∗a−b∗c)=0时，即 ( V 2 ⋅ V 1 ) ∗ ( V 2 ⋅ V 1 ) = ( V 1 ⋅ V 1 ) ∗ ( V 2 ⋅ V 2 ) = 0 (V2\\cdot V1) \* (V2\\cdot V1) = (V1\\cdot V1) \* (V2\\cdot V2) = 0 (V2⋅V1)∗(V2⋅V1)=(V1⋅V1)∗(V2⋅V2)=0时，也就是两条直线平行或重合，此时有垂足无数对，通过在任一一条直线上随便指定一个点，另一条直线上的垂足也就随之确定，

t 1 = 0 t1 = 0 t1=0

t 2 = − d / a t2 = - d/a t2=−d/a

将t1，t2带入到直线方程中就可以得到对应的垂足了，而两垂直足间的距离即为两直线的距离

该方法对应的python代码见：[https://github.com/hichenway/UsefulCode/blob/master/distance\_3d\_line.py][https_github.com_hichenway_UsefulCode_blob_master_distance_3d_line.py]

[3]: https://www.jianshu.com/p/34a7c4e1f3f5
[https_github.com_hichenway_UsefulCode_blob_master_distance_3d_line.py]: https://github.com/hichenway/UsefulCode/blob/master/distance_3d_line.py