如何查找两条3D Polylines的交点或满足距离阈值的邻近点

你此前采用三平面投影求2D交点的方案存在逻辑缺陷：3D线段相交的必要条件是三个投影面都存在交点，但反向不成立，很容易出现假阳性结果，不能直接作为判断依据。以下是可直接落地的实现方案：

优先推荐的库实现方案 #

直接使用Python生态成熟的3D几何计算库trimesh完成计算，无需重复造轮子，性能足以支撑单条300+点的多段线计算需求：

• 第一步：粗筛剪枝，先计算两条多段线的AABB包围盒，若包围盒无交集直接返回无相交、无近距点，跳过后续所有计算
• 第二步：将两条多段线拆解为线段集合（n个点对应n-1条线段），调用trimesh.geometry.segment_segment_distance函数遍历所有线段对，直接得到两条3D线段的最短距离与对应最近点位
• 第三步：按需求过滤结果：距离为0即为严格交点，距离小于设定阈值即为近距点位，记录对应坐标即可
• 可选优化：由于你的路径已插值为等点间距的形式，可增加滑动窗口匹配逻辑，无需全量遍历所有线段对，计算效率可提升10倍以上

如果是用于运动规划避碰仿真，还可额外匹配点位对应的时间戳，过滤掉时间维度不重叠的近距点位，结果更贴合实际场景。

无依赖轻量化实现方案 #

如果不想引入大型3D库，可直接用以下numpy实现的3D线段最短距离计算函数，代码量少、逻辑透明：

import numpy as np

def calc_3d_segment_distance(s1_start: np.ndarray, s1_end: np.ndarray, 
                             s2_start: np.ndarray, s2_end: np.ndarray):
    """
    计算两条3D线段之间的最短距离与对应最近点
    输入均为shape=(3,)的numpy数组
    返回: (最短距离, 线段1上的最近点, 线段2上的最近点)
    """
    u = s1_end - s1_start
    v = s2_end - s2_start
    w = s1_start - s2_start
    a = np.dot(u, u)
    b = np.dot(u, v)
    c = np.dot(v, v)
    d = np.dot(u, w)
    e = np.dot(v, w)
    denom = a * c - b * b
    s, t = 0.0, 0.0

    # 处理线段近似平行的情况
    if denom < 1e-6:
        s = 0.0
        t = (b * s + e) / c if c > 1e-6 else 0.0
        t = np.clip(t, 0.0, 1.0)
    else:
        s = np.clip((b * e - c * d) / denom, 0.0, 1.0)
        t = np.clip((a * e - b * d) / denom, 0.0, 1.0)
    
    closest_p1 = s1_start + s * u
    closest_p2 = s2_start + t * v
    return np.linalg.norm(closest_p1 - closest_p2), closest_p1, closest_p2

300点的多段线对应299条线段，全量遍历也仅需不到9万次计算，Python单线程运行耗时在毫秒级，完全满足仿真需求。

2D库适配3D场景的修正方案 #

如果要沿用你之前的2D库计算思路，只需增加一步校验逻辑即可解决假阳性问题：

1. 分别在x-y、x-z、y-z三个投影面计算两条多段线的2D交点，得到候选交点对应的原多段线线段ID对
2. 仅对这些候选线段对调用上述3D距离计算函数做校验，过滤掉投影相交但实际3D空间不相交的假阳性结果
   该方案可以把原本需要全量计算的近9万次线段对计算，缩减到仅需计算几十次候选对，性能表现最优。

内容的提问来源于stack exchange，提问作者Andrei