def compute_cost(node: Node, neighbour: Node, grid: Grid,
agent_characteristics: AgentCharacteristics) -> None:
parent = node.parent or node
mp_cost = euclidean(neighbour, parent)
if line_of_sight(parent, neighbour, grid, agent_characteristics):
if parent.g + mp_cost < neighbour.g:
neighbour.parent = parent
neighbour.g = parent.g + mp_cost
else:
m_cost = euclidean(neighbour, node)
if node.g + m_cost < neighbour.g:
neighbour.parent = node
neighbour.g = node.g + m_cost
def compute_cost(node: Node, neighbour: Node, grid: Grid,
agent_characteristics: AgentCharacteristics) -> None:
m_cost = euclidean(
neighbour, node
) # 1 if node.x == neighbour.x or node.y == neighbour.y else 1.41 # Cost of the connection
if node.g + m_cost < neighbour.g:
neighbour.parent = node
neighbour.g = node.g + m_cost
def identifySuccessors(self, node, method):
"""
Identify successors for the given node. Runs a jump point search in the
direction of each available neighbor, adding any points found to the open
list.
"""
endX = self.endNode.x
endY = self.endNode.y
x = node.x
y = node.y
neighbors = self.findNeighbors(node, method)
self.expendCount = self.expendCount + len(neighbors)
for i in range(len(neighbors)):
neighbor = neighbors[i]
if method == 'A*':
jumpPoint = neighbor
else:
jumpPoint = self.jump(neighbor[0], neighbor[1], x, y)
# 找到跳点
if jumpPoint:
jx = jumpPoint[0]
jy = jumpPoint[1]
jumpNode = self.grid.getNodeAt(jx, jy)
if jumpNode.closed:
continue
if (method == 'A*') or (method == 'jps'):
d = heuristics.manhattan(abs(jx - x), abs(jy - y))
else:
d = heuristics.diagonal(abs(jx - x), abs(jy - y))
ng = node.g + d # next `g` value
if not jumpNode.opened or ng < jumpNode.g:
jumpNode.g = ng
jumpNode.h = heuristics.diagonal(abs(jx - endX), abs(jy - endY))
if (method == 'A*') or (method == 'jps'):
jumpNode.h = heuristics.manhattan(abs(jx - endX), abs(jy - endY))
else:
# todo: 使用改进优秀的启发函数
dx1 = abs(jx - endX)
dy1 = abs(jy - endY)
dx2 = abs(self.startNode.x - endX)
dy2 = abs(self.startNode.y - endY)
cross = abs(dx1 * dy2 - dx2 * dy1) / (heuristics.euclidean(dx2, dy2) + 1)
jumpNode.h = heuristics.diagonal(abs(jx - x), abs(jy - y)) + cross
jumpNode.f = jumpNode.g + jumpNode.h
jumpNode.parent = node
if not jumpNode.opened:
heapq.heappush(self.openList, (jumpNode.f, jumpNode))
jumpNode.opened = True
else:
for i in range(len(self.openList)):
if self.openList[i][1] == jumpNode:
self.openList[i] = (jumpNode.f, jumpNode)
def identify_successors(
node: Node,
openlist: List[Node],
agent_characteristics: AgentCharacteristics,
end_node: Node,
grid: Grid,
options: SearchOptions,
to_clear: Dict[Node, bool],
heuristic: Heuristic,
) -> None:
"""
Searches for successors of a given node in the direction of each of its neighbours.
This is a generic translation of the algorithm 1 in the paper:
http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf
Also, we notice that processing neighbours in a reverse order producing a natural
looking path, as the pathfinder tends to keep heading in the same direction.
In case a jump point was found, and this node happened to be diagonal to the
node currently expanded in a straight mode search, we skip this jump point.
"""
neighbours = find_neighbours(grid, options, node, agent_characteristics)
neighbours.reverse()
for neighbour in neighbours:
skip = False
jump_node = jump(grid, neighbour, node, end_node,
agent_characteristics, options)
if jump_node and not options.allow_diagonal:
if jump_node.x != node.x and jump_node.y != node.y:
skip = True
# Perform regular A* on a set of jump points
if jump_node and not skip and not jump_node.closed:
# Update the jump node and move it in the closed list if it wasn't there
extraG = euclidean(jump_node, node)
new_g = node.g + extraG
if not jump_node.opened or new_g < jump_node.g:
to_clear[jump_node] = True
jump_node.g = new_g
jump_node.h = jump_node.h or heuristic(jump_node, end_node)
jump_node.parent = node
if not jump_node.opened:
heapq.heappush(openlist, jump_node)
jump_node.open()
else:
heapq.heapify(openlist)
def length(self) -> int:
path_length = 0
for i in range(1, len(self.nodes)):
path_length += euclidean(self.nodes[i], self.nodes[i - 1])
return path_length