EPEAStar.h

Go to the documentation of this file.

 
#ifndef EPEAStar_H
#define EPEAStar_H
 
#define __STDC_CONSTANT_MACROS
#include <stdint.h>
// this is defined in stdint.h, but it doesn't always get defined correctly
// even when __STDC_CONSTANT_MACROS is defined before including stdint.h
// because stdint might be included elsewhere first...
#ifndef UINT32_MAX
#define UINT32_MAX        4294967295U
#endif
 
#include "FPUtil.h"
#include <unordered_map>
#include "AStarOpenClosed.h"
#include "BucketOpenClosed.h"
//#include "SearchEnvironment.h" // for the SearchEnvironment class
#include "float.h"
 
#include <algorithm> // for vector reverse
 
#include "GenericSearchAlgorithm.h"
 
template<typename state>
class EPEAOpenClosedData {
public:
        EPEAOpenClosedData() {}
        EPEAOpenClosedData(const state &theData, double gCost, double hCost, uint64_t parent, uint64_t openLoc, dataLocation location)
        :data(theData), g(gCost), h(hCost), parentID(parent), openLocation(openLoc), where(location), special(0) { reopened = false; }
        state data;
        double g;
        double h;
        uint64_t parentID;
        uint64_t openLocation;
        bool reopened;
        dataLocation where;
        uint64_t special;
};
 
template <class state>
struct EPEAStarCompare {
        bool operator()(const EPEAOpenClosedData<state> &i1, const EPEAOpenClosedData<state> &i2) const
        {
                if (fequal(i1.g+i1.h, i2.g+i2.h))
                {
                        return (fless(i1.g, i2.g));
                }
                return (fgreater(i1.g+i1.h, i2.g+i2.h));
        }
};
 
template <class state, class action, class environment>
class EPEAStar : public GenericSearchAlgorithm<state,action,environment> {
public:
        EPEAStar() { ResetNodeCount(); env = 0; stopAfterGoal = true; weight=1; reopenNodes = false; }
        virtual ~EPEAStar() {}
        void GetPath(environment *env, const state& from, const state& to, std::vector<state> &thePath);
        
        void GetPath(environment *, const state& , const state& , std::vector<action> & ) { assert(false); };
        
        AStarOpenClosed<state, EPEAStarCompare<state>, EPEAOpenClosedData<xyLoc> > openClosedList;
        //BucketOpenClosed<state, EPEAStarCompare<state>, EPEAOpenClosedData<xyLoc> > openClosedList;
        state goal, start;
        
        bool InitializeSearch(environment *env, const state& from, const state& to, std::vector<state> &thePath);
        bool DoSingleSearchStep(std::vector<state> &thePath);
        void AddAdditionalStartState(state& newState);
        void AddAdditionalStartState(state& newState, double cost);
        
        state CheckNextNode();
        void ExtractPathToStart(state &node, std::vector<state> &thePath)
        { uint64_t theID; openClosedList.Lookup(env->GetStateHash(node), theID); ExtractPathToStartFromID(theID, thePath); }
        void ExtractPathToStartFromID(uint64_t node, std::vector<state> &thePath);
        virtual const char *GetName();
        
        void PrintStats();
        uint64_t GetUniqueNodesExpanded() { return uniqueNodesExpanded; }
        void ResetNodeCount() { nodesExpanded = nodesTouched = 0; uniqueNodesExpanded = 0; }
        int GetMemoryUsage();
        
        //closedList_iterator GetClosedListIter() const;
        //      void GetClosedListIter(closedList_iterator);
        //      bool ClosedListIterNext(closedList_iterator& it, state& next) const;
        //bool GetClosedListGCost(state &val, double &gCost) const;
        bool GetClosedListGCost(const state &val, double &gCost) const;
        unsigned int GetNumOpenItems() { return openClosedList.OpenSize(); }
        inline const EPEAOpenClosedData<state> &GetOpenItem(unsigned int which) { return openClosedList.Lookat(openClosedList.GetOpenItem(which)); }
        inline const int GetNumItems() { return openClosedList.size(); }
        inline const EPEAOpenClosedData<state> &GetItem(unsigned int which) { return openClosedList.Lookat(which); }
        bool HaveExpandedState(const state &val)
        { uint64_t key; return openClosedList.Lookup(env->GetStateHash(val), key) != kNotFound; }
        
        void SetReopenNodes(bool re) { reopenNodes = re; }
        bool GetReopenNodes() { return reopenNodes; }
        
        void SetHeuristic(Heuristic<state> *h) { theHeuristic = h; }
        
        uint64_t GetNodesExpanded() const { return nodesExpanded; }
        uint64_t GetNodesTouched() const { return nodesTouched; }
        
        void LogFinalStats(StatCollection *) {}
        
        void SetStopAfterGoal(bool val) { stopAfterGoal = val; }
        bool GetStopAfterGoal() { return stopAfterGoal; }
        
        void OpenGLDraw() const;
        
        void SetWeight(double w) {weight = w;}
private:
        uint64_t nodesTouched, nodesExpanded;
        environment *env;
        bool stopAfterGoal;
        
        double weight; 
        
        bool reopenNodes;
        uint64_t uniqueNodesExpanded;
        Heuristic<state> *theHeuristic;
};
 
//static const bool verbose = false;
 
template <class state, class action, class environment>
const char *EPEAStar<state,action,environment>::GetName()
{
        static char name[32];
        sprintf(name, "EPEAStar[]");
        return name;
}
 
template <class state, class action, class environment>
void EPEAStar<state,action,environment>::GetPath(environment *_env, const state& from, const state& to, std::vector<state> &thePath)
{
        //discardcount=0;
        if (!InitializeSearch(_env, from, to, thePath))
        {       
                return;
        }
        while (!DoSingleSearchStep(thePath)) {}
}
 
template <class state, class action, class environment>
bool EPEAStar<state,action,environment>::InitializeSearch(environment *_env, const state& from, const state& to, std::vector<state> &thePath)
{
        theHeuristic = _env;
        thePath.resize(0);
        //if (useRadius)
        //std::cout<<"Using radius\n";
        env = _env;
        //      closedList.clear();
        //      openQueue.reset();
        //      assert(openQueue.size() == 0);
        //      assert(closedList.size() == 0);
        openClosedList.Reset();
        //openClosedList.Print();
        ResetNodeCount();
        start = from;
        goal = to;
        
        if (env->GoalTest(from, to) && (stopAfterGoal)) //assumes that from and to are valid states
        {
                return false;
        }
        
        openClosedList.AddOpenNode(start, env->GetStateHash(start), 0, weight*theHeuristic->HCost(start, goal));
        //openClosedList.Print();
        
        return true;
}
 
template <class state, class action, class environment>
void EPEAStar<state,action,environment>::AddAdditionalStartState(state& newState)
{
        openClosedList.AddOpenNode(newState, env->GetStateHash(newState), 0, weight*theHeuristic->HCost(start, goal));
}
 
template <class state, class action, class environment>
void EPEAStar<state,action,environment>::AddAdditionalStartState(state& newState, double cost)
{
        openClosedList.AddOpenNode(newState, env->GetStateHash(newState), cost, weight*theHeuristic->HCost(start, goal));
}
 
template <class state, class action, class environment>
bool EPEAStar<state,action,environment>::DoSingleSearchStep(std::vector<state> &thePath)
{
        if (openClosedList.OpenSize() == 0)
        {
                thePath.resize(0); // no path found!
                //closedList.clear();
                return true;
        }
        uint64_t nodeid = openClosedList.Peek();
        const state &currOpenNode = openClosedList.Lookup(nodeid).data;
        
        if (!openClosedList.Lookup(nodeid).reopened)
                uniqueNodesExpanded++;
        nodesExpanded++;
        
        if ((stopAfterGoal) && (env->GoalTest(currOpenNode, goal)))
        {
                ExtractPathToStartFromID(nodeid, thePath);
                // Path is backwards - reverse
                reverse(thePath.begin(), thePath.end()); 
                return true;
        }
        state next;
        
        bool validMove;
        bool moreMoves = env->GetNextSuccessor(currOpenNode, goal, next,
                                                                                   openClosedList.Lookup(nodeid).h,
                                                                                   openClosedList.Lookup(nodeid).special,
                                                                                   validMove);
        if (!moreMoves)
        {
                openClosedList.Close();
                //openClosedList.Print();
        }
        if (!validMove)
        {
                return false;
        }
        else if (moreMoves) {
                openClosedList.KeyChanged(nodeid);
                //openClosedList.Print();
        }
        
        double edgeCost = env->GCost(currOpenNode, next);
        
        nodesTouched++;
        //double edgeCost;
        
        uint64_t theID;
        switch (openClosedList.Lookup(env->GetStateHash(next), theID))
        {
                case kClosedList:
                        if (reopenNodes)
                        {
                                // do something here...
                        }
                        break;
                case kOpenList:
                        //edgeCost = env->GCost(openClosedList.Lookup(nodeid).data, neighbors[x]);
                        if (fless(openClosedList.Lookup(nodeid).g+edgeCost, openClosedList.Lookup(theID).g))
                        {
                                openClosedList.Lookup(theID).parentID = nodeid;
                                openClosedList.Lookup(theID).g = openClosedList.Lookup(nodeid).g+edgeCost;
                                openClosedList.KeyChanged(theID);
                                //openClosedList.Print();
                        }
                        break;
                case kNotFound:
                        openClosedList.AddOpenNode(next,
                                                                           env->GetStateHash(next),
                                                                           openClosedList.Lookup(nodeid).g+edgeCost,
                                                                           weight*theHeuristic->HCost(next, goal),
                                                                           nodeid);
                        //openClosedList.Print();
        }
        
        return false;
}
 
template <class state, class action, class environment>
state EPEAStar<state, action,environment>::CheckNextNode()
{
        uint64_t key = openClosedList.Peek();
        return openClosedList.Lookup(key).data;
        //assert(false);
        //return openQueue.top().currNode;
}
 
 
template <class state, class action,class environment>
void EPEAStar<state, action,environment>::ExtractPathToStartFromID(uint64_t node,
                                                                                                                                   std::vector<state> &thePath)
{
        do {
                thePath.push_back(openClosedList.Lookup(node).data);
                node = openClosedList.Lookup(node).parentID;
        } while (openClosedList.Lookup(node).parentID != node);
        thePath.push_back(openClosedList.Lookup(node).data);
}
 
template <class state, class action, class environment>
void EPEAStar<state, action,environment>::PrintStats()
{
        printf("%u items in closed list\n", (unsigned int)openClosedList.ClosedSize());
        printf("%u items in open queue\n", (unsigned int)openClosedList.OpenSize());
}
 
template <class state, class action, class environment>
int EPEAStar<state, action,environment>::GetMemoryUsage()
{
        return openClosedList.size();
}
 
template <class state, class action, class environment>
bool EPEAStar<state, action,environment>::GetClosedListGCost(const state &val, double &gCost) const
{
        uint64_t theID;
        dataLocation loc = openClosedList.Lookup(env->GetStateHash(val), theID);
        if (loc == kClosedList)
        {
                gCost = openClosedList.Lookat(theID).g;
                return true;
        }
        return false;
}
 
template <class state, class action, class environment>
void EPEAStar<state, action,environment>::OpenGLDraw() const
{
        double transparency = 1.0;
        if (openClosedList.size() == 0)
                return;
        uint64_t top = -1;
        if (openClosedList.OpenSize() > 0)
                top = openClosedList.Peek();
        for (unsigned int x = 0; x < openClosedList.size(); x++)
        {
                const EPEAOpenClosedData<state> &data = openClosedList.Lookat(x);
                if (x == top)
                {
                        env->SetColor(1.0, 1.0, 0.0, transparency);
                        env->OpenGLDraw(data.data);
                }
                if ((data.where == kOpenList) && (data.reopened))
                {
                        env->SetColor(0.0, 0.5, 0.5, transparency);
                        env->OpenGLDraw(data.data);
                }
                else if (data.where == kOpenList) 
                {
                        env->SetColor(0.0, 1.0, 0.0, transparency);
                        env->OpenGLDraw(data.data);
                }
                else if ((data.where == kClosedList) && (data.reopened))
                {
                        env->SetColor(0.5, 0.0, 0.5, transparency);
                        env->OpenGLDraw(data.data);
                }
                else if (data.where == kClosedList)
                {
                        env->SetColor(1.0, 0.0, 0.0, transparency);
                        env->OpenGLDraw(data.data);
                }
        }
}
 
#endif