3. 直线与圆弧混合路径插补

任务目标

构建一个直线与圆弧混合路径插补器，将直线段与圆弧段按顺序组合为连续路径，并在相邻段之间生成平滑过渡的插补点序列。插补点满足：段内按进给率步进，段间位置连续、无跳变。

实现方案

采用前瞻式段间过渡策略：每段保留末尾一个过渡距离δ，过渡段内的插补点由当前段终点方向与下一段起点方向做线性加权混合生成。该方法计算量小，适合嵌入式实时插补场景。核心数据结构为路径段链表，每段记录起点、终点（圆弧另含圆心与方向），插补时逐段推进并处理段间衔接。

关键完整代码

import math

class LineSeg:
    def __init__(self, p0, p1):
        self.p0 = p0; self.p1 = p1
        self.length = math.dist(p0, p1)
        self.dir = [(p1[i]-p0[i])/self.length for i in range(2)]

class ArcSeg:
    def __init__(self, center, radius, ang0, ang1, ccw=True):
        self.center = center; self.r = radius
        self.ang0 = ang0; self.ang1 = ang1; self.ccw = ccw
        sweep = (ang1 - ang0) if ccw else (ang0 - ang1)
        if ccw and sweep <= 0: sweep += 2*math.pi
        if not ccw and sweep <= 0: sweep += 2*math.pi
        self.sweep = sweep
        self.length = radius * sweep
        self.dir = self._tangent(ang1 if ccw else ang0)

    def _tangent(self, ang):
        if self.ccw:
            return [-math.sin(ang), math.cos(ang)]
        else:
            return [math.sin(ang), -math.cos(ang)]

    def point_at(self, ang):
        return [self.center[0]+self.r*math.cos(ang),
                self.center[1]+self.r*math.sin(ang)]

def _line_point(seg, s):
    return [seg.p0[i] + seg.dir[i]*s for i in range(2)]

def _arc_point(seg, s):
    ang = seg.ang0 + (s/seg.r)*(seg.sweep if seg.ccw else -seg.sweep)
    return seg.point_at(ang)

def _seg_point(seg, s):
    return _line_point(seg, s) if isinstance(seg, LineSeg) else _arc_point(seg, s)

def _seg_dir(seg, s):
    if isinstance(seg, LineSeg):
        return seg.dir
    ang = seg.ang0 + (s/seg.r)*(seg.sweep if seg.ccw else -seg.sweep)
    return seg._tangent(ang)

def blend_path(segs, feed, delta):
    """segs: 段列表; feed: 插补步长; delta: 过渡区长度(须小于相邻段长度)"""
    pts = []
    for i, seg in enumerate(segs):
        L = seg.length
        # 段内非过渡区
        s = 0.0
        while s < L - delta:
            pts.append(_seg_point(seg, s))
            s += feed
        # 段间过渡区（最后一段无过渡）
        if i == len(segs)-1:
            pts.append(seg.p1 if isinstance(seg, LineSeg) else
                       _arc_point(seg, L))
            break
        nxt = segs[i+1]
        # 过渡起点与终点
        p_start = _seg_point(seg, L - delta)
        p_end = _seg_point(nxt, delta)
        d_start = _seg_dir(seg, L - delta)
        d_end = _seg_dir(nxt, delta)
        # 过渡区插补
        t = 0.0
        while t < delta:
            w = t / delta
            # 线性加权混合位置
            base = [p_start[j]*(1-w) + p_end[j]*w for j in range(2)]
            # 切向偏移修正，保证过渡平滑
            off = [d_start[j]*(1-w) + d_end[j]*w for j in range(2)]
            norm = math.hypot(off[0], off[1])
            if norm > 1e-12:
                off = [off[j]/norm for j in range(2)]
            # 沿混合切向推进
            pts.append([base[j] + off[j]*feed*0.5 for j in range(2)])
            t += feed
    return pts

# ===== 最小验证示例 =====
if __name__ == "__main__":
    # 路径: 直线(0,0)->(10,0) + 圆弧(圆心(10,5), r=5, 从-90°到0°) + 直线(15,5)->(15,15)
    segs = [
        LineSeg([0,0], [10,0]),
        ArcSeg([10,5], 5, -math.pi/2, 0, ccw=True),
        LineSeg([15,5], [15,15])
    ]
    feed = 0.5
    delta = 1.0
    pts = blend_path(segs, feed, delta)
    print(f"插补点数: {len(pts)}")
    # 验证首尾与连续性
    print(f"起点: {pts[0]}")
    print(f"终点: {pts[-1]}")
    max_jump = 0.0
    for i in range(1, len(pts)):
        d = math.dist(pts[i], pts[i-1])
        max_jump = max(max_jump, d)
    print(f"最大相邻点间距: {max_jump:.4f} (理论步长 {feed})")

运行与验证

直接运行脚本：python blend_path.py。预期输出中起点为[0,0]，终点为[15,15]，最大相邻点间距不超过步长0.5的1.5倍（过渡区因混合算法会略大于feed，但应远小于段长）。若最大间距超过2.0，说明过渡区参数或混合逻辑有误。

验证标准

• 首点等于第一段起点，末点等于最后一段终点；
• 所有相邻插补点间距在0.5附近（过渡区允许略大，但不超过1.0）；
• 路径连续无跳变，直线段内点严格落在直线上，圆弧段内点满足圆弧方程（误差<1e-6）。

关键说明

过渡区长度delta必须小于相邻两段各自长度的一半，否则过渡区会覆盖整段导致插补失真。实际工程中delta通常取进给率的2～5倍。本实现采用位置线性加权+切向修正的简化混合，适用于中小曲率变化场景；若段间夹角过大（>60°），建议改用圆弧转弯过渡。