pathfinderastartile4dir.h

Go to the documentation of this file.

#ifndef _pathfinder_astar_tile4dir_
#define _pathfinder_astar_tile4dir_

/*
        Pathfinder library is
        Copyright (C) 2008 Jeremy Zeiber
        
        Permission is given by the author to freely redistribute and 
        include this code in any program as long as this credit is 
        given where due.
        
        COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, 
        WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, 
        INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE 
        IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
        OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND 
        PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED 
        CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL 
        DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY 
        NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF 
        WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE 
        OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
        THIS DISCLAIMER.
         
        Use at your own risk!
*/

#include "pathfinder.h"

#include <functional>
#include <algorithm>
#include <map>

namespace Pathfinder
{


template <class MOVECOST, class DESTCOST, class MOVEBLOCKED, class COSTTYPE=double>
class AStarTile4Dir
{
public:
        AStarTile4Dir();
        ~AStarTile4Dir();

        const PathResult FindPath(const std::pair<long,long> &start, const std::pair<long,long> &goal, std::vector<std::pair<long,long> > &finalpath, MOVECOST &movecost, DESTCOST &destinationcost);
        const PathResult FindPath(const std::pair<long,long> &start, const std::pair<long,long> &goal, std::vector<std::pair<long,long> > &finalpath, MOVECOST &movecost, MOVEBLOCKED &moveblocked, DESTCOST &destinationcost);

private:
        // Can't be copied
        AStarTile4Dir(const AStarTile4Dir &rhs);
        AStarTile4Dir &operator=(const AStarTile4Dir &rhs);

        struct astarnode
        {
                std::pair<long,long> m_node;
                astarnode *m_parent;
                COSTTYPE m_f;                   // total cost to reach destination from source
                COSTTYPE m_g;                   // actual cost from source to this node
                COSTTYPE m_h;                   // estimated cost from this node to destination
        };

        struct astarnodecompare:public std::binary_function<astarnode *, astarnode *, bool>
        {
                inline bool operator()(const astarnode *left, const astarnode *right)
                {
                        return right->m_f<left->m_f;
                }
        };

        class astarallocator:public std::allocator<astarnode>
        {
        public:
                astarallocator()
                {
                        m_mempos=0;
                        m_mem.resize(1000,NULL);
                }

                ~astarallocator()
                {
                        for(std::vector<astarnode *>::size_type i=0; i<m_mempos; i++)
                        {
                                delete m_mem[i];
                        }
                }

                pointer allocate(size_type count)
                {
                        return m_mempos>0 ? m_mem[--m_mempos] : new astarnode;
                }

                void deallocate(pointer ptr, size_type count)
                {
                        if(m_mempos==m_mem.size())
                        {
                                m_mem.resize(m_mem.size()+1000,NULL);
                        }
                        m_mem[m_mempos++]=ptr;
                }

        private:
                std::vector<astarnode *> m_mem;
                typename std::vector<astarnode *>::size_type m_mempos;
        };

        struct listelement
        {
                listelement():m_node(NULL),m_onopen(false),m_onclosed(false)    {}
                astarnode *m_node;
                bool m_onopen;
                bool m_onclosed;
        };

        void ClearOpenList();
        void ClearClosedList();

        astarallocator m_astarnodeallocator;                    // allocates memory for astarnode

        std::vector<astarnode *> m_openlist;                    // list of open nodes
        std::vector<astarnode *> m_closedlist;                  // list of closed nodes - keep around just for the pointers
        std::map<std::pair<long,long>, listelement> m_helperlist;       // helper list if node is on open/closed list

};

template <class MOVECOST, class DESTCOST, class MOVEBLOCKED, class COSTTYPE>
AStarTile4Dir<MOVECOST,DESTCOST,MOVEBLOCKED,COSTTYPE>::AStarTile4Dir()
{

}

template <class MOVECOST, class DESTCOST, class MOVEBLOCKED, class COSTTYPE>
AStarTile4Dir<MOVECOST,DESTCOST,MOVEBLOCKED,COSTTYPE>::~AStarTile4Dir()
{
        ClearOpenList();
        ClearClosedList();
}

template <class MOVECOST, class DESTCOST, class MOVEBLOCKED, class COSTTYPE>
void AStarTile4Dir<MOVECOST,DESTCOST,MOVEBLOCKED,COSTTYPE>::ClearClosedList()
{
        for(std::vector<astarnode *>::iterator i=m_closedlist.begin(); i!=m_closedlist.end(); i++)
        {
                m_astarnodeallocator.deallocate((*i),1);
        }
        m_closedlist.clear();
}

template <class MOVECOST, class DESTCOST, class MOVEBLOCKED, class COSTTYPE>
void AStarTile4Dir<MOVECOST,DESTCOST,MOVEBLOCKED,COSTTYPE>::ClearOpenList()
{
        for(std::vector<astarnode *>::iterator i=m_openlist.begin(); i!=m_openlist.end(); i++)
        {
                m_astarnodeallocator.deallocate((*i),1);
        }
        m_openlist.clear();
}

template <class MOVECOST, class DESTCOST, class MOVEBLOCKED, class COSTTYPE>
const PathResult AStarTile4Dir<MOVECOST,DESTCOST,MOVEBLOCKED,COSTTYPE>::FindPath(const std::pair<long,long> &start, const std::pair<long,long> &goal, std::vector<std::pair<long,long> > &finalpath, MOVECOST &movecost, DESTCOST &destinationcost)
{
        // clear the lists
        ClearOpenList();
        ClearClosedList();
        m_helperlist.clear();

        astarnode *astaropen;

        // setup current nodes
        astarnode *currentnode;
        astarnode *newnode=m_astarnodeallocator.allocate(1);
        newnode->m_node=start;
        newnode->m_parent=NULL;
        newnode->m_g=destinationcost(start,goal);
        newnode->m_h=0;
        newnode->m_f=newnode->m_g+newnode->m_h;

        // push the start node on the open list
        m_openlist.push_back(newnode);
        std::push_heap(m_openlist.begin(),m_openlist.end(),astarnodecompare());
        m_helperlist[newnode->m_node].m_node=newnode;
        m_helperlist[newnode->m_node].m_onopen=true;

        while(m_openlist.size()>0)
        {
                currentnode=m_openlist.front();
                std::pop_heap(m_openlist.begin(),m_openlist.end(),astarnodecompare());  // front element has now been placed at the back of the vector
                m_openlist.pop_back();
                m_helperlist[currentnode->m_node].m_onopen=false;

                // push the current node on the closed list
                m_closedlist.push_back(currentnode);
                m_helperlist[currentnode->m_node].m_onclosed=true;

                if(currentnode->m_node!=goal)
                {
                        for(long i=1; i<8; i+=2)
                        {
                                std::pair<long,long> pos(currentnode->m_node.first+(i%3)-1,currentnode->m_node.second+(i/3)-1);

                                // ignore node if it is already on the closed list
                                if(m_helperlist[pos].m_onclosed==false)
                                {

                                        // get the astarnode from the list - will be NULL if it doesn't already exist
                                        astaropen=m_helperlist[pos].m_node;

                                        // already on open list - recalculate values if F will be less using this path
                                        if(astaropen!=NULL)
                                        {
                                                if(currentnode->m_g+movecost(currentnode->m_node,pos)<astaropen->m_g)
                                                {
                                                        astaropen->m_g=currentnode->m_g+movecost(currentnode->m_node,pos);
                                                        astaropen->m_f=astaropen->m_g+astaropen->m_h;
                                                        astaropen->m_parent=currentnode;
                                                        std::make_heap(m_openlist.begin(),m_openlist.end(),astarnodecompare());
                                                }
                                        }
                                        // not on the open list - calculate values and add
                                        else
                                        {
                                                newnode=m_astarnodeallocator.allocate(1);
                                                newnode->m_node=pos;
                                                newnode->m_parent=currentnode;
                                                newnode->m_g=currentnode->m_g+movecost(currentnode->m_node,pos);
                                                newnode->m_h=destinationcost(pos,goal);
                                                newnode->m_f=newnode->m_g+newnode->m_h;

                                                m_openlist.push_back(newnode);
                                                std::push_heap(m_openlist.begin(),m_openlist.end(),astarnodecompare());
                                                m_helperlist[newnode->m_node].m_onopen=true;
                                                m_helperlist[newnode->m_node].m_node=newnode;
                                        }

                                }
                        }
                }
                else
                {
                        finalpath.clear();
                        while(currentnode)
                        {
                                finalpath.push_back(currentnode->m_node);
                                currentnode=currentnode->m_parent;
                        }
                        return PATH_FOUND;
                }

        }

        return PATH_NOTFOUND;
}

template <class MOVECOST, class DESTCOST, class MOVEBLOCKED, class COSTTYPE>
const PathResult AStarTile4Dir<MOVECOST,DESTCOST,MOVEBLOCKED,COSTTYPE>::FindPath(const std::pair<long,long> &start, const std::pair<long,long> &goal, std::vector<std::pair<long,long> > &finalpath, MOVECOST &movecost, MOVEBLOCKED &moveblocked, DESTCOST &destinationcost)
{
        // clear the lists
        ClearOpenList();
        ClearClosedList();
        m_helperlist.clear();

        astarnode *astaropen;

        // setup current nodes
        astarnode *currentnode;
        astarnode *newnode=m_astarnodeallocator.allocate(1);
        newnode->m_node=start;
        newnode->m_parent=NULL;
        newnode->m_g=destinationcost(start,goal);
        newnode->m_h=0;
        newnode->m_f=newnode->m_g+newnode->m_h;

        // push the start node on the open list
        m_openlist.push_back(newnode);
        std::push_heap(m_openlist.begin(),m_openlist.end(),astarnodecompare());
        m_helperlist[newnode->m_node].m_node=newnode;
        m_helperlist[newnode->m_node].m_onopen=true;

        while(m_openlist.size()>0)
        {
                currentnode=m_openlist.front();
                std::pop_heap(m_openlist.begin(),m_openlist.end(),astarnodecompare());  // front element has now been placed at the back of the vector
                m_openlist.pop_back();
                m_helperlist[currentnode->m_node].m_onopen=false;

                // push the current node on the closed list
                m_closedlist.push_back(currentnode);
                m_helperlist[currentnode->m_node].m_onclosed=true;

                if(currentnode->m_node!=goal)
                {
                        for(long i=1; i<8; i+=2)
                        {
                                std::pair<long,long> pos(currentnode->m_node.first+(i%3)-1,currentnode->m_node.second+(i/3)-1);

                                // ignore node if it is already on the closed list
                                if(moveblocked(currentnode->m_node,pos)==false && m_helperlist[pos].m_onclosed==false)
                                {

                                        // get the astarnode from the list - will be NULL if it doesn't already exist
                                        astaropen=m_helperlist[pos].m_node;

                                        // already on open list - recalculate values if F will be less using this path
                                        if(astaropen!=NULL)
                                        {
                                                if(currentnode->m_g+movecost(currentnode->m_node,pos)<astaropen->m_g)
                                                {
                                                        astaropen->m_g=currentnode->m_g+movecost(currentnode->m_node,pos);
                                                        astaropen->m_f=astaropen->m_g+astaropen->m_h;
                                                        astaropen->m_parent=currentnode;
                                                        std::make_heap(m_openlist.begin(),m_openlist.end(),astarnodecompare());
                                                }
                                        }
                                        // not on the open list - calculate values and add
                                        else
                                        {
                                                newnode=m_astarnodeallocator.allocate(1);
                                                newnode->m_node=pos;
                                                newnode->m_parent=currentnode;
                                                newnode->m_g=currentnode->m_g+movecost(currentnode->m_node,pos);
                                                newnode->m_h=destinationcost(pos,goal);
                                                newnode->m_f=newnode->m_g+newnode->m_h;

                                                m_openlist.push_back(newnode);
                                                std::push_heap(m_openlist.begin(),m_openlist.end(),astarnodecompare());
                                                m_helperlist[newnode->m_node].m_onopen=true;
                                                m_helperlist[newnode->m_node].m_node=newnode;
                                        }

                                }
                        }
                }
                else
                {
                        finalpath.clear();
                        while(currentnode)
                        {
                                finalpath.push_back(currentnode->m_node);
                                currentnode=currentnode->m_parent;
                        }
                        return PATH_FOUND;
                }

        }

        return PATH_NOTFOUND;
}

}       // namespace

#endif  // _pathfinder_astar_tile4dir_

---