如何计算射线到线段的距离？及沿单位方向向量D发射射线的方法

我来给你梳理清楚怎么实现这个接近传感器的射线检测——你之前用正交投影做线段最近距离的经验刚好能用上，咱们一步步来：

首先先明确两个核心几何元素的定义，这是后续计算的基础：

• 射线：从起点 QP 出发，沿单位方向向量 D 延伸，参数方程可以写成 P(t) = QP + t*D，这里的t必须≥0（因为我们只关心射线正方向的区域）。
• 线段：连接端点 S0 和 S1，参数方程是 S(s) = S0 + s*(S1 - S0)，其中s的范围是0 ≤ s ≤ 1（s=0对应S0，s=1对应S1）。

我们的目标是找到射线正方向上离线段最近的点的距离——这正是接近传感器需要的：要么检测到前方有障碍物（距离为0，即射线与线段相交），要么返回最近障碍物的距离，或者判断前方没有该线段的遮挡。

第一步：计算基础向量与点积 #

先算出几个关键的向量和点积值，这些是后续分情况讨论的核心：

• V = QP - S0：从线段起点到射线起点的向量
• E = S1 - S0：线段的方向向量
• 点积值：
  • a = D · D：因为D是单位向量，这个值固定为1
  • b = D · E：射线方向和线段方向的夹角余弦相关值
  • c = E · E：线段方向向量的模长平方
  • d = D · V：射线方向与V的点积
  • e = E · V：线段方向与V的点积
• denom = a*c - b*b：这个分母用来判断射线和线段是否平行

第二步：分情况计算最近距离 #

根据射线和线段是否平行，我们分两种场景处理：

场景1：射线与线段平行（abs(denom) < 1e-8） #

当分母denom接近0时，说明射线和线段方向平行，这时候我们只需要计算线段两个端点到射线的最近距离，取较小的那个即可：

• 对每个端点（比如S0），计算它在射线上的投影对应的t值：t = D · (S0 - QP)
  • 如果t < 0：说明端点在射线起点的反方向，最近点就是QP，距离就是QP到S0的直线距离
  • 如果t ≥ 0：最近点是射线上的QP + t*D，距离是这个点到S0的垂直距离
• 对S1做同样的计算，最后取两个距离中的最小值

场景2：射线与线段不平行（denom ≠ 0） #

这种情况我们可以先算出理论上的最优参数s和t，再根据它们的范围调整：

s = (b*d - a*e) / denom
t = (b*e - c*d) / denom

然后根据s和t的取值范围，分四种子情况处理：

1. 当s < 0：线段上的最近点是S0，按照场景1中计算S0到射线距离的方式处理即可
2. 当s > 1：线段上的最近点是S1，同样按照场景1中计算S1到射线距离的方式处理
3. 当0 ≤ s ≤ 1且t ≥ 0：这说明射线正方向上存在到线段的最近点，直接计算P(t)和S(s)之间的距离即可——如果距离为0，说明射线和线段相交，传感器检测到碰撞
4. 当0 ≤ s ≤ 1但t < 0：线段的最近点在射线起点的反方向，此时传感器"看不到"这个线段，要么返回QP到该最近点的距离，要么根据传感器逻辑返回一个代表"无遮挡"的极大值

第三步：适配接近传感器的逻辑 #

如果你的传感器只关心**前方（t≥0）**的障碍物，那么可以额外处理：

• 当计算出的最近点对应的t < 0时，直接忽略该线段，返回一个很大的数值（比如float('inf')）表示未检测到
• 如果存在t≥0的交点，距离返回0，表示检测到障碍物
• 否则返回计算出的最小距离，代表前方最近障碍物的距离

实用伪代码参考 #

下面是一份Python风格的伪代码，你可以直接适配到自己的项目中：

import math

def ray_segment_proximity(QP, D, S0, S1):
    # 辅助向量运算函数
    def dot(v1, v2):
        return v1[0]*v2[0] + v1[1]*v2[1]
    def vec_sub(v1, v2):
        return (v1[0]-v2[0], v1[1]-v2[1])
    def vec_add(v1, v2):
        return (v1[0]+v2[0], v1[1]+v2[1])
    def vec_mul(v, scalar):
        return (v[0]*scalar, v[1]*scalar)
    def vec_norm(v):
        return math.sqrt(v[0]**2 + v[1]**2)
    
    E = vec_sub(S1, S0)
    V = vec_sub(QP, S0)
    a = dot(D, D)  # 单位向量点积为1
    b = dot(D, E)
    c = dot(E, E)
    d = dot(D, V)
    e = dot(E, V)
    denom = a * c - b * b
    eps = 1e-8  # 浮点精度阈值
    
    if abs(denom) < eps:
        # 平行场景，计算两个端点到射线的距离
        def point_ray_dist(point):
            t = dot(D, vec_sub(point, QP))
            if t < 0:
                return vec_norm(vec_sub(point, QP))
            closest = vec_add(QP, vec_mul(D, t))
            return vec_norm(vec_sub(point, closest))
        
        dist_s0 = point_ray_dist(S0)
        dist_s1 = point_ray_dist(S1)
        return min(dist_s0, dist_s1)
    else:
        s = (b*d - a*e) / denom
        t = (b*e - c*d) / denom
        
        if s < 0:
            # 最近点是S0
            return ray_segment_proximity(QP, D, S0, S0)
        elif s > 1:
            # 最近点是S1
            return ray_segment_proximity(QP, D, S1, S1)
        else:
            if t >= 0:
                # 射线正方向上的最近点
                ray_point = vec_add(QP, vec_mul(D, t))
                seg_point = vec_add(S0, vec_mul(E, s))
                return vec_norm(vec_sub(ray_point, seg_point))
            else:
                # 最近点在射线后方，传感器不检测
                return float('inf')

注意事项 #

• 务必保证D是单位向量，否则计算出的t值不是实际距离，需要额外做归一化处理
• 浮点精度问题：一定要用一个小的epsilon（比如1e-8）来判断是否平行，避免因浮点误差导致的错误判断
• 传感器逻辑可以根据需求调整：比如如果需要检测所有方向的最近距离，就去掉t<0时返回无穷大的逻辑

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