lucasmcdonald3
9/28/2018 - 7:46 PM

A*

def astar(maze):
    """A* Search (Multiple Objectives)
    Returns an optimal path through all objectives
    and how many nodes were expanded / visited."""

    # Tracking (time, expanded nodes)
    start_time = time.time()
    num_expanded = 0

    # Initialization
    objectives = set(maze.getObjectives())
    objectives_hit = ()
    start = maze.getStart()
    state = (objectives_hit, start)
    path = {state: [start]}
    pqueue = PriorityQueue()

    # Calculate A* heuristic for start
    heuristic = astar_heuristic(state, objectives, path)
    pqueue.push(state, heuristic)
    visited = {state: False}
    neighbors = {}
    heuristics = {}

    # Main A* loop
    while len(pqueue) != 0:

        # Current node information
        state = pqueue.pop()
        num_expanded += 1
        objectives_hit = set(state[0])
        node = state[1]

        # Check if current state hits all objectives
        if objectives.issubset(objectives_hit):
            print_info("A* (Multiple Obj)", time.time()-start_time, num_expanded, len(pqueue), len(path[state]))
            return path[state], num_expanded

        # If state is not visited yet, visit it
        if not visited[state]:
            visited[state] = True

            # Add neighbors to neighbor dictionary if not already found
            if node not in neighbors:
                neighbors[node] = maze.getNeighbors(node[0], node[1])

            # Examine all neighbors and add to priority queue
            for neighbor in neighbors[node]:
                # Note: new_state is always (tuple(objectives_hit.union({neighbor})), neighbor) ...
                # but algorithm is sped up significantly when the set union only occurs when needed
                if (neighbor in objectives) and (neighbor not in objectives_hit):
                    new_objectives_hit = objectives_hit.union({neighbor})
                else:
                    new_objectives_hit = objectives_hit
                new_state = (tuple(new_objectives_hit), neighbor)

                # Note: Similar optimization above... only add heuristic when needed
                if state not in heuristics:
                    heuristics[state] = astar_heuristic(state, objectives, path)

                if new_state not in visited:
                    heuristic = heuristics[state]
                    pqueue.push(new_state, heuristic)
                    visited[new_state] = False
                    path[new_state] = path[state] + [neighbor]
    
    # Objective not reached
    return [], 0