def PathToSmooth(self, v: int):
paths = self.PathTo(v)
paths = list(reversed(paths))
# 把数组变成链表
firstnode = PathEdge(0, None, OTHER)
curnode = firstnode
for v in paths:
curnode.next = PathEdge(v, None, NODE)
curnode = curnode.next
# 平滑路径
e1 = firstnode.next
e2 = e1.next
while e2:
(curcellx, curcelly) = idxTohw(e1.pos, self.manCellWLen)
(nextcellx, nextcelly) = idxTohw(e2.pos, self.manCellWLen)
firstzuobiao = Zuobiao(curcellx * 10, curcelly * 10)
nextzuobiao = Zuobiao(nextcellx * 10, nextcelly * 10)
if self.ob.CanTwoPointBeMove(firstzuobiao, nextzuobiao):
e1.next = e2
e2 = e2.next
else:
e1 = e2
e2 = e2.next
return firstnode.next
def GetPaths(d, ob, beginpos, endpos):
begincellidx = CoordToManIdx(beginpos[0], beginpos[1], d.mapw // 10, ob)
endcellidx = CoordToManIdx(endpos[0], endpos[1], d.mapw // 10, ob)
if endpos == [0, 0]:
return [], NotImplementedError
elif ob.TouchedAnything(idxTohw(endcellidx, d.mapw // 10)):
# 终点不能行走. 那也返回一个终点过去把
lst = [begincellidx, endcellidx]
elif begincellidx != endcellidx:
# 起点终点不同 就规划下
try:
astar = AStartPaths(d.mapw, d.maph, ob, begincellidx, endcellidx)
except IndexError:
return [], IndexError
if img is not None:
lst = astar.PathTo(endcellidx)
for ele in lst:
# 画路径点
mancellwlen = d.mapw // 10
(cellx, celly) = idxTohw(ele, mancellwlen)
drawx, drawy = cellx * 10, celly * 10
cv2.circle(img, (drawx, drawy), 2, (0, 0, 255))
try:
lst = astar.PathToSmoothLst(endcellidx)
except AttributeError:
return [], AttributeError
else:
# 可能起点终点相同
lst = [begincellidx, endcellidx]
return lst, None
def astar(self):
while len(self.openSet) > 0:
current = min(self.openSet, key=lambda s: self.fScore[s])
if current == self.end:
return
self.openSet.remove(current)
self.closedSet.append(current)
adjs = self.GetAdjs(current)
for w in adjs:
if w in self.closedSet:
continue
# 实际距离
tentativeScore = self.gScore[current] + dist_between(
idxTohw(current, self.manCellWLen),
idxTohw(w, self.manCellWLen))
if tentativeScore < self.gScore[w]:
global img
if img is not None:
# 画邻居
ccellx, ccelly = idxTohw(w, self.manCellWLen)
cv2.circle(img, (ccellx * 10, ccelly * 10), 2,
(255, 0, 0))
self.edgeTo[w] = current
self.marked[w] = True
self.gScore[w] = tentativeScore
self.fScore[w] = self.gScore[w] + dist_between(
idxTohw(w, self.manCellWLen),
idxTohw(self.end, self.manCellWLen))
if w not in self.openSet:
self.openSet.append(w)
def __init__(self, MAPW, MAPH, ob, start, end):
self.MAPW = MAPW
self.MAPH = MAPH
self.ob = ob
# start end && 行走的路径使用 10 * 10 长宽的格子
self.manCellWLen = MAPW // 10
self.manCellHLen = MAPH // 10
self.manCellnum = self.manCellHLen * self.manCellWLen
# a * 核心算法
self.closedSet = []
self.openSet = [start]
self.start = start
self.end = end
self.edgeTo = [0] * self.manCellnum
self.marked = [False] * self.manCellnum
# 实际距离
self.gScore = [sys.maxsize] * self.manCellnum
self.gScore[start] = 0
# 估算到终点的距离
self.fScore = [sys.maxsize] * self.manCellnum
self.fScore[start] = dist_between(idxTohw(start, self.manCellWLen),
idxTohw(end, self.manCellWLen))
self.astar()
def DrawAnyPath(beginx, beginy, endx, endy):
global img
meninfo = GetMenInfo()
d = GetGameObstacleData()
ob = Obstacle(d, meninfo.w, meninfo.h)
mancellwlen = d.mapw // 10
img = np.zeros((d.maph, d.mapw, 3), dtype=np.uint8)
img[np.where(img == [0])] = [255]
drawWithOutDoor(img, d)
lst, err = GetPaths(d, ob, [beginx, beginy], [endx, endy])
if err:
print("GetPaths err occur")
return
for ele in lst:
# 画路径点
(cellx, celly) = idxTohw(ele, mancellwlen)
drawx, drawy = cellx * 10, celly * 10
cv2.circle(img, (drawx, drawy), 2, (0, 0, 255))
# 方格子
halfw, halfh = meninfo.w // 2, meninfo.h // 2
cv2.rectangle(img, (drawx - halfw, drawy - halfh),
(drawx + halfw, drawy + halfh), (0, 0, 139), 1)
cv2.imshow('img', img)
cv2.waitKey()
cv2.destroyAllWindows()
def bfs(self):
q = queue.Queue()
q.put(self.s)
while q.qsize() != 0:
v = q.get()
adjs = self.GetAdjs(v)
for w in adjs:
adjcellx, adjcelly = idxTohw(w, self.manCellWLen)
if not self.ob.TouchedAnything([adjcellx, adjcelly]):
return idxTohw(w, self.manCellWLen)
if not self.marked[w]:
self.edgeTo[w] = v
self.marked[w] = True
q.put(w)
logger.warning("坐标重置没有找到合适的点")
return idxTohw(self.s, self.manCellWLen)
def DrawNextDoorPath():
global img
meninfo = GetMenInfo()
d = GetGameObstacleData()
ob = Obstacle(d, meninfo.w, meninfo.h)
mancellwlen = d.mapw // 10
# 画图
img = np.zeros((d.maph, d.mapw, 3), dtype=np.uint8)
img[np.where(img == [0])] = [255]
drawWithOutDoor(img, d)
door = GetNextDoorWrap()
rangeWrap = DoorConvertToRangeWrap(door)
bfsdoor = BfsNextRangeCorrect(d.mapw, d.maph, rangeWrap, ob)
(cellx, celly) = bfsdoor.bfs()
lst, err = GetPaths(d, ob, [meninfo.x, meninfo.y],
[cellx * 10, celly * 10])
if err:
print("GetPaths err occur")
return
for ele in lst:
# 画路径点
(cellx, celly) = idxTohw(ele, mancellwlen)
drawx, drawy = cellx * 10, celly * 10
cv2.circle(img, (drawx, drawy), 2, (0, 0, 255))
# 方格子
halfw, halfh = meninfo.w // 2, meninfo.h // 2
cv2.rectangle(img, (drawx - halfw, drawy - halfh),
(drawx + halfw, drawy + halfh), (0, 0, 139), 1)
cv2.imshow('img', img)
cv2.waitKey()
cv2.destroyAllWindows()
def GetAdjs(self, pos):
# 获取八方位邻居格子. 根据地形和障碍数据过滤掉不必要的
adjs = []
cellx, celly = idxTohw(pos, self.manCellWLen)
# 上下左右. 左上,左下,右上,右下.
checks = [
(cellx, celly - 1),
(cellx, celly + 1),
(cellx - 1, celly),
(cellx + 1, celly),
(cellx - 1, celly - 1),
(cellx - 1, celly + 1),
(cellx + 1, celly - 1),
(cellx + 1, celly + 1),
]
for (adjx, adjy) in checks:
if self.ob.TouchedAnything([adjx, adjy]):
continue
adjs.append(hwToidx(adjx, adjy, self.manCellWLen))
return adjs
def GetAdjs(self, pos):
# 不超过矩形的范围
adjs = []
cellx, celly = idxTohw(pos, self.manCellWLen)
# 上下左右. 左上,左下,右上,右下.
checks = [
(cellx, celly - 1),
(cellx, celly + 1),
(cellx - 1, celly),
(cellx + 1, celly),
(cellx - 1, celly - 1),
(cellx - 1, celly + 1),
(cellx + 1, celly - 1),
(cellx + 1, celly + 1),
]
for (adjx, adjy) in checks:
if not self.OutRange(adjx, adjy):
idx = hwToidx(adjx, adjy, self.manCellWLen)
if idx < self.manCellnum:
adjs.append(idx)
return adjs