1
"""2-D disc robot, circular obstacles, and exact straight-segment checks."""
2
from dataclasses import dataclass
3
import math
4

5

6
def segment_distance(q, a, b):
7
    """计算二维点q到闭线段[a,b]的最小欧氏距离。
8

9
    q、a、b均为同一坐标系中的二元数值序列；返回值与坐标单位一致。
10
    先求无限直线上的正交投影，再将投影参数截断到[0,1]。
11
    本底层函数假定调用者提供合法二维坐标；World负责场景有效性检查。
12
    """
13
    # u=b-a是线段方向；u·u为长度平方，投影公式的分母。
14
    dx, dy = b[0] - a[0], b[1] - a[1]
15
    squared = dx * dx + dy * dy
16
    # 零长度线段退化为一个点；必须在除法之前单独处理。
17
    if squared == 0:
18
        return math.dist(q, a)
19
    # 令距离平方对t的导数为0，得到无限直线上的最近投影参数。
20
    t = ((q[0] - a[0]) * dx + (q[1] - a[1]) * dy) / squared
21
    # t<0时最近点是a；t>1时最近点是b；只有中间投影才落在线段上。
22
    t = max(0.0, min(1.0, t))
23
    return math.dist(q, (a[0] + t * dx, a[1] + t * dy))
24

25

26
def path_length(path):
27
    """累加相邻顶点间的欧氏距离，得到折线长度。
28

29
    这里不判断碰撞，也不自动补返航边；若需要闭合，调用者须显式追加起点。
30
    空列表或单个点没有线段，按求和约定返回0。
31
    """
32
    # zip(path,path[1:])产生连续线段，避免把非相邻顶点误当成一条边。
33
    return sum(math.dist(a, b) for a, b in zip(path, path[1:]))
34

35

36
@dataclass
37
class World:
38
    # 状态只描述机器人中心；朝向对圆盘几何无影响，因此不在状态中。
39
    bounds: tuple  # xmin, xmax, ymin, ymax
40
    circles: tuple = ()  # each is (x, y, obstacle_radius)
41
    robot_radius: float = 0.0
42

43
    def __post_init__(self):
44
        """dataclass初始化后校验场景，尽早拒绝无效几何配置。
45

46
        bounds为(xmin,xmax,ymin,ymax)，circles中每项为(cx,cy,r)。
47
        所有长度须使用同一单位；二维圆盘机器人必须能放进边界内部。
48
        """
49
        if len(self.bounds) != 4 or not all(math.isfinite(v) for v in self.bounds):
50
            raise ValueError('Require four finite bounds')
51
        if not math.isfinite(self.robot_radius) or self.robot_radius < 0:
52
            raise ValueError('Robot radius must be finite and nonnegative')
53
        x0, x1, y0, y1 = self.bounds
54
        if min(x1 - x0, y1 - y0) <= 2 * self.robot_radius:
55
            raise ValueError('Bounds must contain the robot interior')
56
        for circle in self.circles:
57
            if len(circle) != 3 or not all(math.isfinite(v) for v in circle) or circle[2] < 0:
58
                raise ValueError('Invalid circle')
59

60
    def point_free(self, p):
61
        """判断机器人中心p是否合法；非法坐标也返回False。
62

63
        将圆障碍半径增加robot_radius，并将矩形边界向内缩同样距离。
64
        当前约定：可以接触外边界，但不能接触障碍；实际应用可另加余量。
65
        """
66
        if len(p) != 2 or not all(math.isfinite(v) for v in p):
67
            return False
68
        x0, x1, y0, y1 = self.bounds
69
        r = self.robot_radius
70
        # 两个条件缺一不可：整个圆盘须在外边界内，且中心须在膨胀障碍外。
71
        return (x0 + r <= p[0] <= x1 - r and y0 + r <= p[1] <= y1 - r
72
                and all(math.dist(p, (x, y)) > size + r for x, y, size in self.circles))
73

74
    def edge_free(self, a, b):
75
        # The shrunken rectangular boundary is convex, so endpoint bounds suffice.
76
        """检查机器人沿直线a→b移动的整个过程，而非只检查端点。
77

78
        当前运动模型是圆盘平移，不涉及转向、速度和动态障碍。
79
        矩形边界为凸集，端点都在内即可保证整条线段在内。
80
        每个圆都做解析投影检查，因此不依赖沿线采样步长。
81
        """
82
        return (self.point_free(a) and self.point_free(b)
83
                and all(segment_distance((x, y), a, b) > size + self.robot_radius
84
                        for x, y, size in self.circles))

1
from geometry import World
2

3
world = World((-5, 5, -5, 5), ((0, 0, 0.5),))
4
assert world.point_free((-2, 0))
5
assert world.point_free((2, 0))
6
assert not world.edge_free((-2, 0), (2, 0))
7
assert world.edge_free((-2, 2), (2, 2))

1
队列开始：[S]
2
弹出S，加入第一次看到的邻居：[A, B]
3
弹出A，加入新邻居：[B, C]
4
弹出B，继续扩展：[C, ...]

1
"""BFS and A*/Dijkstra on explicit finite graphs; A* permits reopening."""
2
from collections import deque
3
import heapq
4
import itertools
5
import math
6

7

8
def check_graph(graph, start, goal):
9
    """校验显式邻接表：graph[u]是(v,weight)列表。
10

11
    每个邻居都必须是graph中的键，边权必须有限且非负。
12
    不可达应以缺少边表达，不使用无穷或NaN充当普通权重。
13
    """
14
    if start not in graph or goal not in graph:
15
        raise ValueError('Unknown start or goal')
16
    for edges in graph.values():
17
        for node, weight in edges:
18
            if node not in graph or not math.isfinite(weight) or weight < 0:
19
                raise ValueError('Edges need known nodes and finite nonnegative weights')
20

21

22
def recover(parent, start, goal):
23
    """从goal沿parent回溯到start，再反转为正向路径。
24

25
    仅在搜索已找到目标时调用；此时父链必须确实能回到start。
26
    parent不是图本身，只记录当前最佳路径上每个节点的前驱。
27
    """
28
    path = [goal]
29
    while path[-1] != start:
30
        path.append(parent[path[-1]])
31
    return list(reversed(path))
32

33

34
def bfs(graph, start, goal):
35
    """在单位边权图上返回最少边数路径；不可达返回None。
36

37
    BFS第一次发现节点时，就已用最少边数到达它，因此入队即标记seen。
38
    该性质不适用于一般带权图；此实现明确拒绝非1权重。
39
    """
40
    check_graph(graph, start, goal)
41
    if any(weight != 1 for edges in graph.values() for _, weight in edges):
42
        raise ValueError('This BFS example requires unit edge weights')
43
    # 队列维护待扩展层；seen在入队时更新，防止同层节点反复发现同一邻居。
44
    queue, parent, seen = deque([start]), {}, {start}
45
    while queue:
46
        # FIFO保证先处理较浅层；不能换成列表pop()，否则会变为深度优先。
47
        node = queue.popleft()
48
        # 只有有效目标条目被弹出时才能结束；第一次发现目标并不一定最优。
49
        if node == goal:
50
            return recover(parent, start, goal)
51
        for neighbor, _ in graph[node]:
52
            if neighbor not in seen:
53
                # 第一次发现即确定最少边数；这一步仅在单位边权BFS中成立。
54
                seen.add(neighbor)
55
                parent[neighbor] = node
56
                queue.append(neighbor)
57
    return None
58

59

60
def astar(graph, start, goal, heuristic):
61
    """在有限非负权图上搜索，返回(path,cost,expansions)。
62

63
    heuristic(node)提供从节点到goal的估计剩余代价。
64
    为保证最优，调用者需保证h可采纳；本代码只检查数值格式，不能证明下界。
65
    支持在找到更小g后重新扩展节点，因此也能处理可采纳但不一致的h。
66
    失败返回(None,math.inf,expansions)；expansions不含目标和过期条目。
67
    """
68
    check_graph(graph, start, goal)
69
    # 缓存h以固定本次搜索的估计值；输入须是静态启发函数，而非不断变化的预测。
70
    h = {node: float(heuristic(node)) for node in graph}
71
    if any(not math.isfinite(v) or v < 0 for v in h.values()) or h[goal] != 0:
72
        raise ValueError('Require finite nonnegative h, with h(goal)=0')
73
    # 相同f时用递增整数打破平局，避免比较节点对象本身。
74
    counter = itertools.count()
75
    # 条目依次是(f,平局序号,入堆时g快照,节点)；起点g=0，所以f=h(start)。
76
    queue = [(h[start], next(counter), 0.0, start)]
77
    # best只存已发现的节点；未出现的键在松弛时视为无穷远。
78
    best, parent, expansions = {start: 0.0}, {}, 0
79
    while queue:
80
        _, _, queued_g, node = heapq.heappop(queue)
81
        # 更小g出现后旧条目仍留在堆里；用快照判断它是否已经过期。
82
        if queued_g != best[node]:
83
            continue  # a cheaper entry superseded this one
84
        # 只有有效目标条目被弹出时才能结束；第一次发现目标并不一定最优。
85
        if node == goal:
86
            return recover(parent, start, goal), queued_g, expansions
87
        # 没有永久closed：如果以后又发现更小g，同一个节点可以重新扩展。
88
        expansions += 1
89
        for neighbor, weight in graph[node]:
90
            # 松弛：起点到node的当前成本，再加node→neighbor的边权。
91
            new_g = queued_g + weight
92
            # 严格改善才改父节点；相等代价无需重入堆，也可避免零权环造成无限更新。
93
            if new_g < best.get(neighbor, math.inf):
94
                best[neighbor], parent[neighbor] = new_g, node
95
                # 压入新条目而非原地改堆：heapq没有按节点直接降低键值的接口。
96
                heapq.heappush(queue, (new_g + h[neighbor], next(counter), new_g, neighbor))
97
    # 有限图的候选耗尽：当前图中目标不可达；不要返回一个未抵达目标的前缀。
98
    return None, math.inf, expansions
99

100

101
def dijkstra(graph, start, goal):
102
    """把h恒设为0，复用同一松弛引擎，得到Dijkstra。
103

104
    此时队列优先级f=g；非负边权保证有效条目按最短距离推进。
105
    返回格式与astar一致，便于作为A*的正确性对照。
106
    """
107
    return astar(graph, start, goal, lambda node: 0.0)
108

109

110
def grid_graph(width, height, blocked, diagonal=True):
111
    """构造二维整数格点的显式邻接表，作为搜索教学输入。
112

113
    width、height约定为正整数；blocked保存禁止经过的(x,y)格点。
114
    diagonal=False只走四邻域；True增加代价sqrt(2)的对角移动。
115
    为禁止切角，斜向移动要求两个轴向侧邻格也存在。
116
    此函数会物化整张图；大型地图通常改为按需生成邻居。
117
    """
118
    # 先创建所有自由节点，再创建边，保证每条邻接边的目标都存在。
119
    graph = {(x, y): [] for x in range(width) for y in range(height) if (x, y) not in blocked}
120
    offsets = list(itertools.product((-1, 0, 1), repeat=2))
121
    for x, y in graph:
122
        for dx, dy in offsets:
123
            if (dx == dy == 0) or (not diagonal and dx != 0 and dy != 0):
124
                continue
125
            target = x + dx, y + dy
126
            if target not in graph:
127
                continue
128
            # Do not let diagonal edges cut across the corners of blocked cells.
129
            # 斜边两侧有障碍就拒绝，防止从障碍接触的角落穿过去。
130
            if dx and dy and ((x + dx, y) not in graph or (x, y + dy) not in graph):
131
                continue
132
            graph[(x, y)].append((target, math.hypot(dx, dy)))
133
    return graph
134

135

136
def octile(a, b):
137
    """计算无障碍八邻域图的精确距离，供有障碍图作下界。
138

139
    前提：轴向代价1、对角代价sqrt(2)，坐标是同尺度整数网格。
140
    若对角代价或运动规则改变，必须重新推导启发函数。
141
    """
142
    dx, dy = abs(a[0] - b[0]), abs(a[1] - b[1])
143
    return max(dx, dy) + (math.sqrt(2) - 1) * min(dx, dy)

1
from graph_search import dijkstra
2

3
graph = {
4
    'S': [('A', 4), ('B', 1)],
5
    'A': [('G', 1)],
6
    'B': [('A', 2), ('G', 7)],
7
    'G': [],
8
}
9
path, cost, expansions = dijkstra(graph, 'S', 'G')
10
print(path, cost)  # ['S', 'B', 'A', 'G'] 4.0

1
0→1→2→3→0
2
0→1→3→2→0
3
0→2→1→3→0
4
...

1
001：只访问城市1
2
101：访问城市1和3
3
111：访问城市1、2、3

1
"""Small exact TSP solvers plus nearest-neighbor and symmetric 2-opt."""
2
import itertools
3
import math
4

5

6
def validate(cost, limit=None):
7
    """检查教学TSP的成本矩阵，并返回城市数n。
8

9
    此版本要求完整方阵、有限非负成本和至少两个城市。
10
    limit限制精确算法的教学规模，不是普遍的性能或正确性阈值。
11
    """
12
    n = len(cost)
13
    if n < 2 or (limit is not None and n > limit):
14
        raise ValueError('Unsupported teaching instance size')
15
    if any(len(row) != n for row in cost):
16
        raise ValueError('Cost matrix must be square')
17
    if any(not math.isfinite(v) or v < 0 for row in cost for v in row):
18
        raise ValueError('Require finite nonnegative costs')
19
    return n
20

21

22
def tour_cost(cost, order):
23
    """按order中显式列出的相邻城市求和，不自动补闭合边。
24

25
    例如[0,1,2,0]才是闭合路线；[0,1,2]的结果不含2→0。
26
    这里假定编号有效，完整排列校验由相关调用者负责。
27
    """
28
    return sum(cost[a][b] for a, b in zip(order, order[1:]))
29

30

31
def brute_force(cost):
32
    """固定起点0，枚举其余城市全部排列，返回(路线,最小成本)。
33

34
    同时适用于对称和非对称矩阵。规模限制为9，适合作为小实例的真值对照。
35
    成本相同时保留先遇到的解；最优路线不一定唯一。
36
    """
37
    n = validate(cost, limit=9)
38
    best, answer = math.inf, None
39
    # 固定起点0消除循环平移重复；其余城市仍需枚举全部(n-1)!排列。
40
    for perm in itertools.permutations(range(1, n)):
41
        # 最后一个0是真正的返程边，不能省掉，否则解的是开放路线。
42
        order = [0, *perm, 0]
43
        length = tour_cost(cost, order)
44
        if length < best:
45
            best, answer = length, order
46
    return answer, best
47

48

49
def held_karp(cost):
50
    """用状态压缩动态规划精确求解闭合TSP，返回(路线,最小成本)。
51

52
    dp[(mask,j)]：从0出发，恰好访问mask包含的非起点城市，停在j的最小成本。
53
    第j-1位代表城市j；起点0不在mask中。parent用于恢复实现最小值的前驱。
54
    支持非对称矩阵，时间O(n²·2ⁿ)、空间O(n·2ⁿ)，教学限制为16城市。
55
    """
56
    n = validate(cost, limit=16)
57
    # Bit j-1 indicates whether non-start node j has been visited.
58
    # 键包含集合和终点；只用终点j会混淆访问过不同城市的子问题。
59
    dp, parent = {}, {}
60
    for j in range(1, n):
61
        # 基例：只访问j时，唯一前缀是0→j，不需要记录中间前驱。
62
        dp[(1 << (j - 1), j)] = cost[0][j]
63
    # mask从小到大：删除一个已置位bit得到更小整数，因此依赖状态已经算过。
64
    for mask in range(1, 1 << (n - 1)):
65
        for j in range(1, n):
66
            # 城市1对应最低位；城市0从不进入集合。
67
            bit = 1 << (j - 1)
68
            # 终点必须属于已访问集合；不满足条件的状态没有意义。
69
            if not mask & bit:
70
                continue
71
            # 此时bit已确认存在，异或才等价于移除j；不能无条件用异或“删除”。
72
            previous = mask ^ bit
73
            # 单城市状态已由基例初始化，不再对空前驱集合求min。
74
            if previous == 0:
75
                continue
76
            # 枚举最后一步k→j；min返回最小成本及实现它的前驱k，平局按编号。
77
            value, k = min((dp[(previous, k)] + cost[k][j], k)
78
                           for k in range(1, n) if previous & (1 << (k - 1)))
79
            dp[(mask, j)], parent[(mask, j)] = value, k
80
    # 全1掩码表示所有非起点城市均已访问。
81
    full = (1 << (n - 1)) - 1
82
    # DP前缀还没回到0；这里加返程cost[j][0]，然后选择最优最后一城。
83
    value, last = min((dp[(full, j)] + cost[j][0], j) for j in range(1, n))
84
    reverse, mask = [], full
85
    while mask:
86
        reverse.append(last)
87
        # 先读取旧状态的前驱，再删除当前终点的bit；顺序反了会查错状态。
88
        previous_last = parent.get((mask, last))
89
        mask ^= 1 << (last - 1)
90
        # 只剩基例时前驱为None，同时mask归零，下一轮不会再访问None。
91
        last = previous_last
92
    return [0, *reversed(reverse), 0], value
93

94

95
def nearest_neighbor(cost):
96
    """从城市0开始，每次贪心选择最近的未访问城市，再返回0。
97

98
    返回合法闭合排列和成本，但不保证最优；可作为2-opt的初始解。
99
    使用城市编号打破相同成本的平局，使结果更容易复现。
100
    """
101
    n = validate(cost)
102
    order, unused = [0], set(range(1, n))
103
    while unused:
104
        # 贪心只看当前一步：速度快，但没有为最终返程和以后选择保留全局保证。
105
        node = min(unused, key=lambda j: (cost[order[-1]][j], j))
106
        order.append(node)
107
        unused.remove(node)
108
    order.append(0)
109
    return order, tour_cost(cost, order)
110

111

112
def two_opt(cost, order):
113
    """在对称TSP上反复接受第一个改善交换，返回局部改进路线和成本。
114

115
    输入order必须从0出发、覆盖所有城市一次并返回0；不会修改调用者列表。
116
    每次反转route[i:j+1]，只比较被替换的两条边与新增的两条边。
117
    此四边差分依赖对称性；非对称矩阵的内部反向成本也会改变，因此拒绝。
118
    1e-12用于忽略浮点级微小改善，最终是该容差和邻域下的局部稳定结果。
119
    """
120
    n = validate(cost)
121
    if any(cost[i][j] != cost[j][i] for i in range(n) for j in range(n)):
122
        raise ValueError('Four-edge 2-opt delta requires a symmetric matrix')
123
    if len(order) != n + 1 or order[0] != 0 or order[-1] != 0 or sorted(order[:-1]) != list(range(n)):
124
        raise ValueError('Require a closed permutation starting at 0')
125
    # 复制输入，避免教学实验中的初始路线被原地改变而无法对照。
126
    route = list(order)
127
    while True:
128
        # 采用first-improvement：发现一处改善就执行，然后从新路线重新扫描。
129
        changed = False
130
        for i in range(1, n - 1):
131
            for j in range(i + 1, n):
132
                # 切断a→b和c→d；反转b…c后，新接a→c和b→d。
133
                a, b, c, d = route[i - 1], route[i], route[j], route[j + 1]
134
                delta = cost[a][c] + cost[b][d] - cost[a][b] - cost[c][d]
135
                # 忽略浮点舍入导致的极小变化；并非宣称1e-12适合任意代价尺度。
136
                if delta < -1e-12:
137
                    # j+1使切片包含j；否则只换边的公式与实际反转范围不一致。
138
                    route[i:j + 1] = reversed(route[i:j + 1])
139
                    changed = True
140
                    break
141
            if changed:
142
                break
143
        # 整个2-opt邻域都未发现超过容差的改善：局部稳定，不代表全局最优。
144
        if not changed:
145
            return route, tour_cost(cost, route)

1
from tsp import held_karp, nearest_neighbor, two_opt
2

3
cost = [[0, 10, 15, 20], [10, 0, 35, 25],
4
        [15, 35, 0, 30], [20, 25, 30, 0]]
5
print(held_karp(cost))  # 最优成本80，排列可能有等价解
6
route, length = nearest_neighbor(cost)
7
print(two_opt(cost, route))

1
当前状态：(111, 2)
2
移除城市2：111 XOR 010 = 101
3
前驱候选：城市1和城市3
4
经城市1：DP[101,1] + c[1][2] = 45 + 35 = 80
5
经城市3：DP[101,3] + c[3][2] = 35 + 30 = 65
6
选择城市3，parent[(111,2)] = 3

1
旧：0 → A → B → C → D → 0
2
新：0 → C → B → A → D → 0
3
删除：0→A、C→D
4
增加：0→C、A→D
5
内部：A→B→C 变成 C→B→A

1
def astar(graph, start, goal, heuristic):
2
    """在有限非负权图上搜索，返回(path,cost,expansions)。
3

4
    heuristic(node)提供从节点到goal的估计剩余代价。
5
    为保证最优，调用者需保证h可采纳；本代码只检查数值格式，不能证明下界。
6
    支持在找到更小g后重新扩展节点，因此也能处理可采纳但不一致的h。
7
    失败返回(None,math.inf,expansions)；expansions不含目标和过期条目。
8
    """
9
    check_graph(graph, start, goal)
10
    # 缓存h以固定本次搜索的估计值；输入须是静态启发函数，而非不断变化的预测。
11
    h = {node: float(heuristic(node)) for node in graph}
12
    if any(not math.isfinite(v) or v < 0 for v in h.values()) or h[goal] != 0:
13
        raise ValueError('Require finite nonnegative h, with h(goal)=0')
14
    # 相同f时用递增整数打破平局，避免比较节点对象本身。
15
    counter = itertools.count()
16
    # 条目依次是(f,平局序号,入堆时g快照,节点)；起点g=0，所以f=h(start)。
17
    queue = [(h[start], next(counter), 0.0, start)]
18
    # best只存已发现的节点；未出现的键在松弛时视为无穷远。
19
    best, parent, expansions = {start: 0.0}, {}, 0
20
    while queue:
21
        _, _, queued_g, node = heapq.heappop(queue)
22
        # 更小g出现后旧条目仍留在堆里；用快照判断它是否已经过期。
23
        if queued_g != best[node]:
24
            continue  # a cheaper entry superseded this one
25
        # 只有有效目标条目被弹出时才能结束；第一次发现目标并不一定最优。
26
        if node == goal:
27
            return recover(parent, start, goal), queued_g, expansions
28
        # 没有永久closed：如果以后又发现更小g，同一个节点可以重新扩展。
29
        expansions += 1
30
        for neighbor, weight in graph[node]:
31
            # 松弛：起点到node的当前成本，再加node→neighbor的边权。
32
            new_g = queued_g + weight
33
            # 严格改善才改父节点；相等代价无需重入堆，也可避免零权环造成无限更新。
34
            if new_g < best.get(neighbor, math.inf):
35
                best[neighbor], parent[neighbor] = new_g, node
36
                # 压入新条目而非原地改堆：heapq没有按节点直接降低键值的接口。
37
                heapq.heappush(queue, (new_g + h[neighbor], next(counter), new_g, neighbor))
38
    # 有限图的候选耗尽：当前图中目标不可达；不要返回一个未抵达目标的前缀。
39
    return None, math.inf, expansions

1
from graph_search import astar, grid_graph, octile
2

3
blocked = {(5, y) for y in range(10) if y != 7}
4
graph = grid_graph(12, 10, blocked)
5
start, goal = (1, 1), (10, 1)
6
path, cost, expansions = astar(graph, start, goal, lambda p: octile(p, goal))
7
print(cost, expansions)

1
1. Sample：生成随机目标q_rand
2
2. Nearest：找树里离q_rand最近的节点q_near
3
3. Steer：从q_near朝q_rand前进最多η，得到q_new
4
4. CollisionFree：检查整条q_near→q_new
5
5. Add：如果可行，把q_new加入树并记录父节点

1
def steer(source, target, step):
2
    """从source向target沿直线走不超过step的距离。
3

4
    调用者保证step为正、两点为合法二维坐标；本函数不检查碰撞。
5
    目标很近就直接到达目标，不能用固定步长越过采样点。
6
    """
7
    distance = math.dist(source, target)
8
    if distance == 0:
9
        return source
10
    # 比例最多为1，确保目标比步长更近时不会越过目标。
11
    ratio = min(1.0, step / distance)
12
    return tuple(a + ratio * (b - a) for a, b in zip(source, target))

1
# 示意伪代码；可执行版本见sampling.py
2
nodes = [start]
3
for _ in range(iteration_budget):
4
    random_target = sample()
5
    nearest = nearest_node(nodes, random_target)
6
    new_point = steer(nearest.point, random_target, step)
7
    if not edge_free(nearest.point, new_point):
8
        continue
9
    new_node = add_node(new_point, parent=nearest)
10
    if close_to_goal(new_point) and edge_free(new_point, goal):
11
        return recover_path(new_node, goal)
12
return None

1
from geometry import World
2
from sampling import plan
3

4
world = World((0, 10, 0, 10), ((5, 5, 1.8), (3, 7, 0.8)), 0.15)
5
result = plan(world, (1, 1), (9, 9), star=False, seed=7,
6
              iterations=1500, step=0.8, goal_bias=0.05)
7
print(result['cost'], result['iterations'])
8
assert result['path'] is not None

1
RRT：最近邻 → 新节点
2
RRT*：附近节点中选更好的父节点 → 新节点 → 尝试改善附近旧节点

1
def reparent(nodes, child, parent):
2
    """将child子树挂到parent下面，并重新计算整棵子树的累计cost。
3

4
    调用者须先确认新边无碰撞、且满足所需的改善条件。
5
    本函数只负责结构与代价维护，不自行做障碍检查，也不要求新cost一定降低。
6
    根节点不能改父节点，任何会导致环的修改都会拒绝。
7
    """
8
    if child == 0 or is_ancestor(nodes, child, parent):
9
        raise ValueError('Reparenting would disconnect the root or create a cycle')
10
    # 重接必须同时维护父指针和两个父节点的children集合，避免结构不一致。
11
    old_parent = nodes[child].parent
12
    nodes[old_parent].children.remove(child)
13
    nodes[parent].children.add(child)
14
    nodes[child].parent = parent
15
    # Descendants keep their geometric edges, but all root-path costs change.
16
    # 父节点先更新，再把孩子压栈；后代计算cost时其父cost已经是新值。
17
    stack = [child]
18
    while stack:
19
        index = stack.pop()
20
        node = nodes[index]
21
        node.cost = nodes[node.parent].cost + math.dist(node.point, nodes[node.parent].point)
22
        # 排序使遍历顺序稳定；对子树cost的数学结果没有影响。
23
        stack.extend(sorted(node.children))

1
"""Readable planar RRT/RRT* with linear scans and continuous edge checking."""
2
from dataclasses import dataclass, field
3
import math
4
import random
5

6
from geometry import path_length
7

8

9
@dataclass
10
class Node:
11
    # 节点索引是稳定ID；坐标相同也不等同于同一个父子结构。
12
    point: tuple
13
    # 根的parent为None，其他节点的parent是nodes列表中的整数索引。
14
    parent: object
15
    # cost是从起点沿父链累计的边长，不是节点到目标的距离。
16
    cost: float
17
    # default_factory让每个节点拥有独立集合；不能让所有节点共享同一个可变默认值。
18
    children: set = field(default_factory=set)
19

20

21
def steer(source, target, step):
22
    """从source向target沿直线走不超过step的距离。
23

24
    调用者保证step为正、两点为合法二维坐标；本函数不检查碰撞。
25
    目标很近就直接到达目标，不能用固定步长越过采样点。
26
    """
27
    distance = math.dist(source, target)
28
    if distance == 0:
29
        return source
30
    # 比例最多为1，确保目标比步长更近时不会越过目标。
31
    ratio = min(1.0, step / distance)
32
    return tuple(a + ratio * (b - a) for a, b in zip(source, target))
33

34

35
def is_ancestor(nodes, ancestor, child):
36
    """沿child的父链判断ancestor是否在链上，包含自身。
37

38
    用于重接前防环：若child已是新父节点的祖先，就不能反过来挂到它下面。
39
    调用前树结构应有效；此函数不是用来修复已经存在的环。
40
    """
41
    while child is not None:
42
        if child == ancestor:
43
            return True
44
        child = nodes[child].parent
45
    return False
46

47

48
def reparent(nodes, child, parent):
49
    """将child子树挂到parent下面，并重新计算整棵子树的累计cost。
50

51
    调用者须先确认新边无碰撞、且满足所需的改善条件。
52
    本函数只负责结构与代价维护，不自行做障碍检查，也不要求新cost一定降低。
53
    根节点不能改父节点，任何会导致环的修改都会拒绝。
54
    """
55
    if child == 0 or is_ancestor(nodes, child, parent):
56
        raise ValueError('Reparenting would disconnect the root or create a cycle')
57
    # 重接必须同时维护父指针和两个父节点的children集合，避免结构不一致。
58
    old_parent = nodes[child].parent
59
    nodes[old_parent].children.remove(child)
60
    nodes[parent].children.add(child)
61
    nodes[child].parent = parent
62
    # Descendants keep their geometric edges, but all root-path costs change.
63
    # 父节点先更新，再把孩子压栈；后代计算cost时其父cost已经是新值。
64
    stack = [child]
65
    while stack:
66
        index = stack.pop()
67
        node = nodes[index]
68
        node.cost = nodes[node.parent].cost + math.dist(node.point, nodes[node.parent].point)
69
        # 排序使遍历顺序稳定；对子树cost的数学结果没有影响。
70
        stack.extend(sorted(node.children))
71

72

73
def recover(nodes, index, goal):
74
    """从一个已确认可连接goal的树节点回溯完整路径。
75

76
    父链先得到终点到起点的逆序，反转后再追加精确goal。
77
    最后一段边已经由plan验证；这里不以“接近goal”代替实际到达。
78
    """
79
    points = []
80
    while index is not None:
81
        points.append(nodes[index].point)
82
        index = nodes[index].parent
83
    points.reverse()
84
    # 避免节点恰等于goal时再追加一个相同终点，产生冗余零长度边。
85
    if points[-1] != goal:
86
        points.append(goal)
87
    return points
88

89

90
def plan(world, start, goal, *, star=False, seed=7, iterations=1500,
91
         step=0.8, goal_bias=0.05, gamma=20.0):
92
    """在World中运行二维RRT或RRT*，返回含路径和树的字典。
93

94
    start/goal约定为二维坐标元组；碰撞、边界由world负责。
95
    star=False：第一条可行路出现就返回。star=True：运行到预算结束并改善路径。
96
    step是最大扩展长度；goal_bias是直接采样目标的概率；seed控制局部随机序列。
97
    gamma是二维RRT*邻域半径系数，需结合地图尺度理解，不是通用最佳参数。
98
    iterations统计采样尝试次数，包括因重复或碰撞而被拒绝的尝试。
99
    输出path为None表示预算内未找到解，不是无路证明；失败cost为math.inf。
100
    nodes保存树；history仅记录最佳可行总代价严格下降的(迭代,代价)。
101
    """
102
    if not world.point_free(start) or not world.point_free(goal):
103
        raise ValueError('Invalid start or goal')
104
    if not isinstance(iterations, int) or iterations <= 0:
105
        raise ValueError('Iterations must be a positive integer')
106
    if not all(math.isfinite(v) for v in (step, goal_bias, gamma)) or step <= 0 or gamma <= 0 or not 0 <= goal_bias < 1:
107
        raise ValueError('Require step > 0, gamma > 0, and 0 <= goal_bias < 1')
108
    # 初始化根；树中没有任何节点时，最近邻查询无从谈起。
109
    nodes = [Node(tuple(start), None, 0.0)]
110
    if start == goal:
111
        return {'path': [start], 'cost': 0.0, 'nodes': nodes, 'history': [(0, 0.0)], 'iterations': 0}
112
    # 局部随机生成器只初始化一次；不影响调用者全局随机状态，也不能在循环中反复重置。
113
    rng = random.Random(seed)
114
    x0, x1, y0, y1 = world.bounds
115
    r = world.robot_radius
116
    # connectors存已验证能直连goal的节点索引；几何不变，但累计cost会因重接变化。
117
    connectors, history, best = set(), [], math.inf
118
    # 处理起终点本就能在一个步长内直连的边界情况；仍然检查整条边。
119
    if math.dist(start, goal) <= step and world.edge_free(start, goal):
120
        connectors.add(0)
121
        best = math.dist(start, goal)
122
        history.append((0, best))
123
        if not star:
124
            return {'path': [start, goal], 'cost': best, 'nodes': nodes, 'history': history, 'iterations': 0}
125
    for iteration in range(1, iterations + 1):
126
        # 以goal_bias概率直接采目标；否则在缩小后的边界均匀采样，障碍内采样后由边检查拒绝。
127
        target = goal if rng.random() < goal_bias else (rng.uniform(x0 + r, x1 - r), rng.uniform(y0 + r, y1 - r))
128
        # 教学版线性查最近邻；这是几何距离最近，不代表根路径累计cost最小。
129
        nearest = min(range(len(nodes)), key=lambda i: math.dist(nodes[i].point, target))
130
        new = steer(nodes[nearest].point, target, step)
131
        # 拒绝近乎重复的点与穿障扩展；一次被拒绝的采样仍消耗迭代预算。
132
        if math.dist(new, nodes[nearest].point) < 1e-10 or not world.edge_free(nodes[nearest].point, new):
133
            continue
134
        # 最近邻到new的边已验证，是父节点选择的有效保底，即使near为空也能加入树。
135
        parent = nearest
136
        near = []
137
        if star:
138
            n = len(nodes) + 1
139
            # 二维r_n=min(η,γ√(log n/n))；若换成d维，指数应为1/d。
140
            radius = min(step, gamma * math.sqrt(math.log(n) / n))
141
            near = [i for i, node in enumerate(nodes) if math.dist(node.point, new) <= radius]
142
            for i in near:
143
                # 选父比较的是g(i)+边长；不能只比较i到new这一段有多短。
144
                candidate = nodes[i].cost + math.dist(nodes[i].point, new)
145
                current = nodes[parent].cost + math.dist(nodes[parent].point, new)
146
                # 先看是否值得改善，再花计算成本做候选连接的完整碰撞检查。
147
                if candidate < current and world.edge_free(nodes[i].point, new):
148
                    parent = i
149
        # 先确定最优父节点再加入；此时near只包含旧节点，不会把自己当候选。
150
        index = len(nodes)
151
        nodes.append(Node(new, parent, nodes[parent].cost + math.dist(nodes[parent].point, new)))
152
        nodes[parent].children.add(index)
153
        # 重接阶段与选父阶段方向相反：尝试让旧节点通过新节点回到起点。
154
        for i in near:
155
            candidate = nodes[index].cost + math.dist(new, nodes[i].point)
156
            if (i != 0 and candidate + 1e-12 < nodes[i].cost
157
                    # 即使浮点或未来代价模型变化，也显式阻止把新节点祖先挂到自己下面形成环。
158
                    and not is_ancestor(nodes, i, index) and world.edge_free(new, nodes[i].point)):
159
                reparent(nodes, i, index)
160
        # Check the whole last edge, not just distance to the goal.
161
        if math.dist(new, goal) <= step and world.edge_free(new, goal):
162
            connectors.add(index)
163
        if connectors:
164
            # 所有旧连接候选都重新比较，因为它们的祖先可能刚被重接并降低了cost。
165
            end = min(connectors, key=lambda i: (nodes[i].cost + math.dist(nodes[i].point, goal), i))
166
            value = nodes[end].cost + math.dist(nodes[end].point, goal)
167
            # 仅记录严格改善，画图时应理解为阶梯保持，而非每轮都下降。
168
            if value < best:
169
                best = value
170
                history.append((iteration, best))
171
            # 基本RRT首解即停；RRT*继续后续采样，才有机会改善已有路径。
172
            if not star:
173
                path = recover(nodes, end, goal)
174
                return {'path': path, 'cost': path_length(path), 'nodes': nodes, 'history': history, 'iterations': iteration}
175
    # 预算耗尽仍没有可用目标连接：只能说本次没找到，不能说已证明无路。
176
    if not connectors:
177
        return {'path': None, 'cost': math.inf, 'nodes': nodes, 'history': history, 'iterations': iterations}
178
    end = min(connectors, key=lambda i: (nodes[i].cost + math.dist(nodes[i].point, goal), i))
179
    path = recover(nodes, end, goal)
180
    return {'path': path, 'cost': path_length(path), 'nodes': nodes, 'history': history, 'iterations': iterations}