def verify_node(node, minX, minY, xWidth, yWidth, obstacleMap):
# Check if the node is outside map bounds
# ***
if node.getX() - minX >= xWidth:
return False
elif node.getX() - minX <= 0:
return False
if node.getY() - minY >= yWidth:
return False
elif node.getY() - minY <= 0:
return False
# ***
# Check if the node is colliding with an obstacle
if obstacleMap[int(node.getX() - minX)][int(node.getY() - minY)]:
return False
# Node has been successfuly verified
return True
def calc_index(node, xWidth, minX, minY):
return (node.getY() - minY) * xWidth + (node.getX() - minX)
def calc_cost(node, goalNode):
return node.getCost() + heuristic_cost(node.getX() - goalNode.getX(),
node.getY() - goalNode.getY())