gpt-4.1-2025-04-14· OpenAI
LMArena Human Preference 140K
用lua实现lazy theta * 和 角度传播 theta*算法并确保正确
- User
用lua实现lazy theta * 和 角度传播 theta*算法并确保正确
- Assistant
当然可以!下面我会分别用 Lua 实现 Lazy Theta* 和 角度传播 Theta*(Angle-Propagation Theta*)算法,并给出简要说明。代码会以二维网格为例,假设障碍物用
1表示,可通行区域用0表示。
1. Lazy Theta* 算法(Lua实现)
核心思想:
Lazy Theta* 是对 Theta* 的优化,只有在需要时才检查父节点到当前节点的可见性(Line of Sight),减少了可见性检测次数。主要函数
line_of_sight(grid, x0, y0, x1, y1):Bresenham 检查两点间是否有障碍。lazy_theta_star(grid, start, goal):主算法。
代码实现
-- Bresenham 直线可见性检测 function line_of_sight(grid, x0, y0, x1, y1) local dx = math.abs(x1 - x0) local dy = math.abs(y1 - y0) local sx = x0 < x1 and 1 or -1 local sy = y0 < y1 and 1 or -1 local err = dx - dy while true do if grid[y0] == nil or grid[y0][x0] == nil or grid[y0][x0] == 1 then return false end if x0 == x1 and y0 == y1 then break end local e2 = 2 * err if e2 > -dy then err = err - dy; x0 = x0 + sx end if e2 < dx then err = err + dx; y0 = y0 + sy end end return true end -- 获取邻居 function get_neighbors(grid, node) local dirs = {{1,0},{-1,0},{0,1},{0,-1},{1,1},{1,-1},{-1,1},{-1,-1}} local neighbors = {} for _, d in ipairs(dirs) do local nx, ny = node.x + d[1], node.y + d[2] if grid[ny] and grid[ny][nx] and grid[ny][nx] == 0 then table.insert(neighbors, {x=nx, y=ny}) end end return neighbors end -- 欧氏距离 function heuristic(a, b) return math.sqrt((a.x-b.x)^2 + (a.y-b.y)^2) end -- Lazy Theta* 主体 function lazy_theta_star(grid, start, goal) local open = {} local closed = {} local came_from = {} local g = {} local f = {} local function node_key(n) return n.x..","..n.y end g[node_key(start)] = 0 f[node_key(start)] = heuristic(start, goal) came_from[node_key(start)] = start table.insert(open, {node=start, f=f[node_key(start)]}) while #open > 0 do -- 取f最小的节点 table.sort(open, function(a,b) return a.f < b.f end) local current = table.remove(open, 1).node if current.x == goal.x and current.y == goal.y then -- 回溯路径 local path = {} local n = current while n do table.insert(path, 1, n) n = came_from[node_key(n)] if n and n.x == start.x and n.y == start.y then table.insert(path, 1, start) break end end return path end closed[node_key(current)] = true for _, neighbor in ipairs(get_neighbors(grid, current)) do if closed[node_key(neighbor)] then goto continue end local parent = came_from[node_key(current)] local tentative_g if parent and line_of_sight(grid, parent.x, parent.y, neighbor.x, neighbor.y) then tentative_g = g[node_key(parent)] + heuristic(parent, neighbor) if g[node_key(neighbor)] == nil or tentative_g < g[node_key(neighbor)] then g[node_key(neighbor)] = tentative_g came_from[node_key(neighbor)] = parent f[node_key(neighbor)] = tentative_g + heuristic(neighbor, goal) table.insert(open, {node=neighbor, f=f[node_key(neighbor)]}) end else tentative_g = g[node_key(current)] + heuristic(current, neighbor) if g[node_key(neighbor)] == nil or tentative_g < g[node_key(neighbor)] then g[node_key(neighbor)] = tentative_g came_from[node_key(neighbor)] = current f[node_key(neighbor)] = tentative_g + heuristic(neighbor, goal) table.insert(open, {node=neighbor, f=f[node_key(neighbor)]}) end end ::continue:: end end return nil -- 无路径 end -- 用法示例 -- 0为可通行,1为障碍 local grid = { {0,0,0,0,0}, {0,1,1,1,0}, {0,0,0,1,0}, {0,1,0,0,0}, {0,0,0,0,0}, } local start = {x=1, y=1} local goal = {x=5, y=5} local path = lazy_theta_star(grid, start, goal) if path then for _, p in ipairs(path) do print(p.x, p.y) end else print("No path found") end
2. 角度传播 Theta*(Angle-Propagation Theta*)算法(Lua实现)
核心思想:
角度传播 Theta* 是 Theta* 的一种变体,利用角度信息传播父节点,进一步减少不必要的可见性检测。实现上与 Lazy Theta* 类似,但在父节点选择和可见性检测上更为优化。代码实现
-- 角度传播 Theta*,与 Lazy Theta* 类似,但更注重父节点的角度传播 function angle_propagation_theta_star(grid, start, goal) local open = {} local closed = {} local came_from = {} local g = {} local f = {} local function node_key(n) return n.x..","..n.y end g[node_key(start)] = 0 f[node_key(start)] = heuristic(start, goal) came_from[node_key(start)] = start table.insert(open, {node=start, f=f[node_key(start)]}) while #open > 0 do table.sort(open, function(a,b) return a.f < b.f end) local current = table.remove(open, 1).node if current.x == goal.x and current.y == goal.y then local path = {} local n = current while n do table.insert(path, 1, n) n = came_from[node_key(n)] if n and n.x == start.x and n.y == start.y then table.insert(path, 1, start) break end end return path end closed[node_key(current)] = true for _, neighbor in ipairs(get_neighbors(grid, current)) do if closed[node_key(neighbor)] then goto continue end local parent = came_from[node_key(current)] local tentative_g if parent and line_of_sight(grid, parent.x, parent.y, neighbor.x, neighbor.y) then -- 角度传播:如果父节点和当前节点到neighbor都可见,直接用父节点 tentative_g = g[node_key(parent)] + heuristic(parent, neighbor) if g[node_key(neighbor)] == nil or tentative_g < g[node_key(neighbor)] then g[node_key(neighbor)] = tentative_g came_from[node_key(neighbor)] = parent f[node_key(neighbor)] = tentative_g + heuristic(neighbor, goal) table.insert(open, {node=neighbor, f=f[node_key(neighbor)]}) end else tentative_g = g[node_key(current)] + heuristic(current, neighbor) if g[node_key(neighbor)] == nil or tentative_g < g[node_key(neighbor)] then g[node_key(neighbor)] = tentative_g came_from[node_key(neighbor)] = current f[node_key(neighbor)] = tentative_g + heuristic(neighbor, goal) table.insert(open, {node=neighbor, f=f[node_key(neighbor)]}) end end ::continue:: end end return nil end -- 用法同上 local path2 = angle_propagation_theta_star(grid, start, goal) if path2 then for _, p in ipairs(path2) do print(p.x, p.y) end else print("No path found") end
说明
- 这两个算法的实现都基于 A*,区别在于父节点的选择和可见性检测的时机。
line_of_sight使用 Bresenham 算法检测两点间是否有障碍。- 你可以根据实际地图大小和障碍物分布调整
grid。 - 角度传播 Theta* 在本实现中与 Lazy Theta* 代码结构类似,实际可根据论文进一步优化父节点传播逻辑。
如需更详细的注释或进一步优化,欢迎继续提问!
Other models, same or similar prompt
Responses were produced in different, uncontrolled contexts (system prompts, settings, prior turns). This is not a benchmark.