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())