open_closed_list.h

Go to the documentation of this file.

/*
 * open_closed_list.h
 *
 * LICENSE HERE
 *
 *  Created on: 2016-09-12
 *      Author: Rick Valenzano
 */

#ifndef OPEN_CLOSED_LIST_H_
#define OPEN_CLOSED_LIST_H_

#include "../../utils/floating_point_utils.h"
#include "node_table.h"
#include <stdio.h>
#include <iostream>

typedef unsigned BFSOpenLocation; 

enum class StateLocation
{
    open, closed, none
};

template<class state_t, class action_t>
class BFSNode
{
public:
    BFSNode(const state_t &node_state, NodeID parent, const action_t &action, double g, double h, double node_eval,
            BFSOpenLocation loc);

    ~BFSNode();

    state_t state; 
    NodeID parent_id; 
    action_t gen_action; 

    double g_cost; 
    double h_value; 
    double eval; 

    BFSOpenLocation location; 
    bool in_open; 

    bool reopened; 
};

template<class state_t, class action_t>
class OpenClosedList
{
public:
    OpenClosedList();

    virtual ~OpenClosedList();

    void clear();

    StateLocation getStateLocation(const state_t &state, StateHash hash_value, NodeID &id);

    BFSNode<state_t, action_t> &getNode(NodeID id);

    const BFSNode<state_t, action_t> &getNode(NodeID id) const;

    virtual NodeID addNewNodeToOpen(const state_t &state, const action_t &action, StateHash hash_value, double g,
            double h, double node_eval, NodeID parent);

    virtual NodeID addInitialNodeToOpen(const state_t &state, const action_t &action, StateHash hash_value,
            double h, double node_eval);

    virtual NodeID getBestNodeAndClose();

    virtual void openNodeEvalChanged(NodeID id);

    virtual void reopenNode(NodeID id);

    std::size_t openListSize() const;

    std::size_t closedListSize() const;

    std::size_t size() const;

    bool isOpenEmpty() const;

protected:
    virtual bool nodeNoWorse(const BFSNode<state_t, action_t> &node_1,
            const BFSNode<state_t, action_t> &node_2) const;

    virtual bool nodeNoWorse(BFSOpenLocation loc_1, BFSOpenLocation loc_2) const;

    bool heapifyUp(BFSOpenLocation loc);

    bool heapifyDown(BFSOpenLocation loc);

    void swapOpenLocations(BFSOpenLocation loc_1, BFSOpenLocation loc_2);

    void printOpen();

    NodeTable<BFSNode<state_t, action_t> > node_table; 
    std::vector<NodeID> open_list_heap; 
};

template<class state_t, class action_t>
BFSNode<state_t, action_t>::BFSNode(const state_t& node_state, NodeID parent, const action_t& action, double g,
        double h, double node_eval, BFSOpenLocation loc)
        : state(node_state), parent_id(parent), gen_action(action), g_cost(g), h_value(h), eval(node_eval),
                location(loc), in_open(true), reopened(false)
{
}

template<class state_t, class action_t>
BFSNode<state_t, action_t>::~BFSNode()
{
}

template<class state_t, class action_t>
inline OpenClosedList<state_t, action_t>::OpenClosedList()
{
}

template<class state_t, class action_t>
inline OpenClosedList<state_t, action_t>::~OpenClosedList()
{
}

template<class state_t, class action_t>
void OpenClosedList<state_t, action_t>::clear()
{
    node_table.clear();
    open_list_heap.clear();
}

template<class state_t, class action_t>
StateLocation OpenClosedList<state_t, action_t>::getStateLocation(const state_t& state, StateHash hash_value,
        NodeID& id)
{
    bool is_stored = node_table.isNodeStored(hash_value, id);

    if(is_stored) {
        if(node_table.getNode(id).in_open)
            return StateLocation::open;
        return StateLocation::closed;
    }
    return StateLocation::none;
}

template<class state_t, class action_t>
NodeID OpenClosedList<state_t, action_t>::addNewNodeToOpen(const state_t& state, const action_t &action,
        StateHash hash_value, double g, double h, double node_eval, NodeID parent)
{
    NodeID new_id = node_table.addNewSearchNode(
            BFSNode<state_t, action_t>(state, parent, action, g, h, node_eval, open_list_heap.size()), hash_value);

    //std::cout << "New ID " << new_id << std::endl;
    //std::cout << "Node Table Size " << node_table.size() << std::endl;
    open_list_heap.push_back(new_id);

    heapifyUp(open_list_heap.size() - 1);

    //printOpen();
    //std::cout << std::endl;

    return new_id;
}

template<class state_t, class action_t>
inline std::size_t OpenClosedList<state_t, action_t>::openListSize() const
{
    return open_list_heap.size();
}

template<class state_t, class action_t>
inline std::size_t OpenClosedList<state_t, action_t>::closedListSize() const
{
    return size() - openListSize();
}

template<class state_t, class action_t>
inline std::size_t OpenClosedList<state_t, action_t>::size() const
{
    return node_table.size();
}

template<class state_t, class action_t>
bool OpenClosedList<state_t, action_t>::heapifyUp(BFSOpenLocation loc)
{
    assert(loc < open_list_heap.size());
    if(loc == 0)
        return false;

    BFSOpenLocation parent_loc = (loc - 1) / 2;

    if(!nodeNoWorse(parent_loc, loc)) {
        swapOpenLocations(loc, parent_loc);

        heapifyUp(parent_loc);
        return true;
    }
    return false;
}

template<class state_t, class action_t>
inline bool OpenClosedList<state_t, action_t>::nodeNoWorse(const BFSNode<state_t, action_t>& node_1,
        const BFSNode<state_t, action_t>& node_2) const
{
    return !fp_greater(node_1.eval, node_2.eval);
}

template<class state_t, class action_t>
inline bool OpenClosedList<state_t, action_t>::nodeNoWorse(BFSOpenLocation loc_1, BFSOpenLocation loc_2) const
{
    return nodeNoWorse(getNode(open_list_heap[loc_1]), getNode(open_list_heap[loc_2]));
}

template<class state_t, class action_t>
void OpenClosedList<state_t, action_t>::openNodeEvalChanged(NodeID id)
{
    if(!heapifyUp(node_table[id].location))
        heapifyDown(heapifyUp(node_table[id].location));
}

template<class state_t, class action_t>
void OpenClosedList<state_t, action_t>::reopenNode(NodeID id)
{
    assert(node_table[id].in_open == false);
    node_table[id].in_open = true;
    node_table[id].reopened = true;
    node_table[id].location = open_list_heap.size();
    open_list_heap.push_back(id);

    heapifyUp(node_table[id].location);
}

template<class state_t, class action_t>
NodeID OpenClosedList<state_t, action_t>::getBestNodeAndClose()
{
    assert(!open_list_heap.empty());

    NodeID best_id = open_list_heap[0];
    node_table[best_id].in_open = false;

    open_list_heap[0] = open_list_heap.back();
    node_table.getNode(open_list_heap[0]).location = 0;
    open_list_heap.pop_back();

    heapifyDown(0);

    return best_id;
}

template<class state_t, class action_t>
inline BFSNode<state_t, action_t>& OpenClosedList<state_t, action_t>::getNode(NodeID id)
{
    return node_table[id];
}

template<class state_t, class action_t>
inline const BFSNode<state_t, action_t>& OpenClosedList<state_t, action_t>::getNode(NodeID id) const
{
    return node_table[id];
}

template<class state_t, class action_t>
inline NodeID OpenClosedList<state_t, action_t>::addInitialNodeToOpen(const state_t& state, const action_t& action,
        StateHash hash_value, double h, double node_eval)
{
    return addNewNodeToOpen(state, action, hash_value, 0.0, h, node_eval, 0);
}

template<class state_t, class action_t>
inline bool OpenClosedList<state_t, action_t>::isOpenEmpty() const
{
    return open_list_heap.size() == 0;
}

template<class state_t, class action_t>
bool OpenClosedList<state_t, action_t>::heapifyDown(BFSOpenLocation loc)
{
    BFSOpenLocation left_child_loc = loc * 2 + 1;
    BFSOpenLocation right_child_loc = loc * 2 + 2;
    BFSOpenLocation best_child_loc;

    if(left_child_loc >= open_list_heap.size())
        return false;
    else if(right_child_loc >= open_list_heap.size() || nodeNoWorse(left_child_loc, right_child_loc))
        best_child_loc = left_child_loc;
    else
        best_child_loc = right_child_loc;

    if(!nodeNoWorse(loc, best_child_loc)) {
        swapOpenLocations(loc, best_child_loc);

        heapifyDown(best_child_loc);
        return true;
    }

    return false;
}

template<class state_t, class action_t>
inline void OpenClosedList<state_t, action_t>::swapOpenLocations(BFSOpenLocation loc_1, BFSOpenLocation loc_2)
{
    NodeID id = open_list_heap[loc_1];
    open_list_heap[loc_1] = open_list_heap[loc_2];
    open_list_heap[loc_2] = id;

    node_table.getNode(open_list_heap[loc_1]).location = loc_1;
    node_table.getNode(open_list_heap[loc_2]).location = loc_2;
}

template<class state_t, class action_t>
void OpenClosedList<state_t, action_t>::printOpen()
{
    for(unsigned i = 0; i < open_list_heap.size(); i++) {
        std::cout << "ID " << open_list_heap[i] << ", eval " << getNode(open_list_heap[i]).eval << std::endl;
    }
}

#endif /* OPEN_CLOSED_LIST_H_ */