#算法优化

Junjie

2024-12-03 eaf7901e010cfb97709428ab24c6c8f9e097fdc0

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
import json
 
import numpy as np
import time
import heapq
import sys
import ast
 
startTime = time.perf_counter()
 
# 创建一个简单的网格，0 为通行，1 为障碍
# grid = np.array([
#   [0, 0, 0, 0, 0, 3, 0, 0, 3, 0],
#   [0, 0, 0, 0, 0, 3, 0, 0, 3, 0],
#   [1, 0, 0, 0, 0, 3, 0, 0, 3, 0],
#   [0, 0, 0, 0, 0, 3, 0, 0, 3, 0],
#   [0, 0, 1, 0, 0, 3, 0, 0, 3, 0],
#   [0, 0, 0, 0, 0, 3, 0, 0, 3, 0],
#   [0, 0, 0, 0, 1, 3, 0, 0, 3, 0],
#   [0, 0, 0, 0, 0, 3, 0, 0, 3, 0],
#   [0, 0, 0, 0, 0, 3, 0, 0, 3, 0],
#   [0, 0, 0, 0, 0, 3, 0, 0, 3, 0],
#   [0, 0, 0, 0, 0, 3, 1, 0, 3, 0],
#   [0, 0, 0, 0, 0, 3, 0, 0, 3, 0],
#   [0, 0, 0, 1, 0, 3, 0, 0, 3, 0],
#   [0, 0, 0, 0, 0, 3, 0, 0, 3, 0]
# ])
# grid = np.array([[-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, -1, -1, -1, 0, 0, 0, 0, 0, 0, 0, -1], [-1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, -1, -1, -1, -1, 3, 3, 3, 3, 3, 3, 3, -1], [-1, 0, 0, 0, 0, -1, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, -1, 4, -1, 0, 0, 0, -1, 0, 0, 0, -1], [-1, 0, 0, 0, 0, -1, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, -1, 4, -1, 0, 0, 0, -1, 0, 0, 0, -1], [-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5, 0, 0, 0, 0, 0, 0, 0, -1], [-1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, -1], [-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1], [-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1], [-1, 0, 0, 0, 0, -1, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, -1], [-1, 0, 0, 0, 0, -1, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, -1], [-1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, -1, -1, -1, -1, -1, -1, -1, -1], [-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, -1, -1, 4, -1, -1, -1, -1], [-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, -1, -1, 4, 4, -1, -1, -1]])
 
# A* 算法实现
def heuristic(a, b):
    # 使用曼哈顿距离作为启发式估计
    return abs(a[0] - b[0]) + abs(a[1] - b[1])
 
def astar_search(grid, start, goal, mapDirection):
    rows, cols = grid.shape
    start = (start // cols, start % cols)
    goal = (goal // cols, goal % cols)
 
    open_set = []
    heapq.heappush(open_set, (0, start))
 
    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start, goal)}
 
    while open_set:
        current = heapq.heappop(open_set)[1]
 
        if current == goal:
            return reconstruct_path(came_from, current)
 
        # 确定当前节点值
        current_value = grid[current[0], current[1]]
 
        neighbors = findNebor(current, current_value, mapDirection)
 
        for neighbor in neighbors:
            # 检查邻居是否在网格内且通行
            if 0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols:
                if grid[neighbor] <= -1:
                    continue
 
                if grid[neighbor] in [66]:
                    continue
 
                tentative_g_score = g_score[current] + 1
 
                if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g_score
                    f_score[neighbor] = tentative_g_score + heuristic(neighbor, goal)
 
                    if neighbor not in [i[1] for i in open_set]:
                        heapq.heappush(open_set, (f_score[neighbor], neighbor))
 
    return []
 
def reconstruct_path(came_from, current):
    total_path = [current]
    while current in came_from:
        current = came_from[current]
        total_path.append(current)
    return total_path[::-1]  # 反转路径
 
def findNebor(current,current_value, direction):
    neighbors = []
 
    if direction == "x":
        if current_value == 3:
            neighbors = [(current[0] + 1, current[1]), (current[0] - 1, current[1])]
 
        if current_value == 0 or current_value == 3 or current_value == 5 or current_value == 6 or current_value == 67:
            neighbors.append((current[0], current[1] + 1))
            neighbors.append((current[0], current[1] - 1))
    else:
        if current_value == 3:
            neighbors = [(current[0], current[1] + 1), (current[0], current[1] - 1)]
 
        if current_value == 0 or current_value == 3 or current_value == 5 or current_value == 6 or current_value == 67:
            neighbors.append((current[0] + 1, current[1]))
            neighbors.append((current[0] - 1, current[1]))
    return neighbors
 
maps = sys.argv[1]
start_str = sys.argv[2]
goal_str = sys.argv[3]
mapDirection = sys.argv[4]
 
print(mapDirection)
 
maps = ast.literal_eval(maps)
grid = np.array(maps)
# print(type(maps))
# print(start_str)
# print(goal_str)
 
start_coords = (int(start_str.split('-')[0]), int(start_str.split('-')[1]))
goal_coords = (int(goal_str.split('-')[0]), int(goal_str.split('-')[1]))
 
 
# print(start_coords, goal_coords)
# # 定义起点和终点的坐标
# start_coords = (1, 1)  # 起点坐标 (行, 列)
# goal_coords = (11, 6)    # 终点坐标 (行, 列)
 
# 将坐标转换为一维索引
start = start_coords[0] * grid.shape[1] + start_coords[1]
goal = goal_coords[0] * grid.shape[1] + goal_coords[1]
 
try:
    # 获取路径
    path_coordinates = astar_search(grid, start, goal, mapDirection)
except:
    path_coordinates = []
 
end = time.perf_counter()
# print("最短路径坐标:", path_coordinates)
# print('程序运行时间为: %s Seconds' % (end - startTime))
 
calcResult = 200
if(len(path_coordinates) == 0):
    calcResult = 500
 
result = {
    "start": start_str,
    "target": goal_str,
    "path": json.dumps(path_coordinates),
    "time": (end - startTime),
    "calcResult": calcResult
}
 
print(result)