D:/simple_rts/include/AStarSearch.h

Go to the documentation of this file.

#ifndef __ASTAR_SEARCH_H__
#define __ASTAR_SEARCH_H__

#pragma once

//#define _USE_HEAP_

#include "WaypointList.h"

#include <cassert>
#include <vector>
#include <exception>
#include <algorithm>
#include <iostream>

using std::cout;
using std::endl;

using std::vector;
using std::exception;
using std::find;


/*
        Implement better searching algorithms instead of using an iterator inside of
                _isNodeOpen
                _isNodeClosed


        A UserState class must implement the follow methods
                getNumNeighbours
                        Returns the number of neighbours this UserState has
                
                getNeigbhour
                        Returns a particular UserState based on an index
                
                getTraversalCost
                        How much will it "cost" to use this UserState in the solution
                
                getEstimatedCostToState
                        Approximately how far is the EndState from this current UserState
                
                operator==
                        Compares two UserStates for equality


*/

// ----------------------------------------------------------------------------
template <class UserState>
class AStarSearch
{
        // ----------------------------------------------------------------------------
        class AStarNode
        {
        public:
                // TODO
                //              Make sure that this becomes a constant reference
                UserState*              mUserState;

                // The node that is currently being used to "enter" into this node
                AStarNode*              mSuccessor;

                // Data Storage
                // ----------------------------------------------------------------------------
        private:
                float           mEntryCost;
                float           mEstimatedGoalCost;

                // TODO
                //              Seriously implement this better
                AStarNode*              mNeighbours[8];

                static int                      mNumNodes;
                unsigned int            mID;


        // Construction
        // ----------------------------------------------------------------------------
        public:
                AStarNode ( UserState* state )
                        : mSuccessor ( NULL ), mUserState ( state),
                          mEntryCost ( 0.0 ), mEstimatedGoalCost ( 0.0 )
                {
                        mID = mNumNodes++;

                        // Create new nodes for each of the neighbours of this one
                        for ( int i = 0; i < mUserState->getNumNeighbours(); i++ )
                                mNeighbours[i] = NULL;

                        //cout << "Node Constructed: " << ++nodeCount << endl;\e
                }


                // ----------------------------------------------------------------------------
                ~AStarNode ()
                {
                        //cout << "Node released: " << --nodeCount << endl;
                        // Our node may not have any neighbours created yet
                        if ( mUserState == NULL )
                                return;


                        for ( int i = 0; i < mUserState->getNumNeighbours(); i++ )
                                delete mNeighbours[i];
                }


                // ----------------------------------------------------------------------------
                void createNeighbourNodes ()
                {
                        for ( int i = 0; i < mUserState->getNumNeighbours(); i++ )
                                mNeighbours[i] = new AStarNode( mUserState->getNeighbour(i) );
                }


                // ----------------------------------------------------------------------------
                bool operator== ( AStarNode& rhs ) const
                {
                        return ( mUserState == rhs.mUserState );
                }


                // ----------------------------------------------------------------------------
                bool operator== ( const AStarNode& rhs ) const
                {
                        return ( mUserState == rhs.mUserState );
                }

                
        // Retrieval
        // ----------------------------------------------------------------------------
        public:
                float getTotalCost () const
                {
                        return mEntryCost + mEstimatedGoalCost;
                }

                // ----------------------------------------------------------------------------
                float getEntryCost () const
                {
                        return mEntryCost;
                }

                // ----------------------------------------------------------------------------
                float getEstimatedGoalCost () const
                {
                        return mEstimatedGoalCost;
                }

                // ----------------------------------------------------------------------------
                float getTraversalCost () const
                {
                        return mUserState->getTraversalCost();
                }

                // ----------------------------------------------------------------------------
                AStarNode* getNeighbour ( int index )
                {
                        assert ( index < 8 );

                        return mNeighbours[index];
                }


        // Storing
        // ----------------------------------------------------------------------------
        public:
                void setEntryCost ( float entryCost )
                {
                        mEntryCost = entryCost;
                }

                // ----------------------------------------------------------------------------
                void setEstimatedGoalCost ( float cost )
                {
                        mEstimatedGoalCost = cost;
                }

                // ----------------------------------------------------------------------------
                void setSuccessor ( AStarNode* parent )
                {
                        mSuccessor = parent;
                }
        };



// Data Storage
// ----------------------------------------------------------------------------
public:
        enum SearchState
        {
                SS_NOT_STARTED,
                SS_RUNNING,
                SS_PATH_NOT_FOUND,
                SS_PATH_FOUND
        };

private:
        SearchState                     mCurState;
        AStarNode*                      mStartNode;
        UserState*                      mEndState;

        AStarNode*                      mGoalNode;

        vector<AStarNode*>      mOpenList;
        vector<AStarNode*>      mClosedList;

        WaypointList<UserState>*        mSolutionList;


        // ----------------------------------------------------------------------------
        public:
                class HeapCompare
                {
                public:

                        bool operator() ( const AStarNode* x, const AStarNode* y ) const
                        {
                                /*
                                if ( fabs( x->getTotalCost () - y->getTotalCost () ) < 0.01 )
                                        return true;
                                */

                                return x->getTotalCost() > y->getTotalCost();
                        }
                };

// Construction
// ----------------------------------------------------------------------------
public:
        AStarSearch(void)
        {
                mCurState = SS_NOT_STARTED;

                mGoalNode = NULL;
                mStartNode = NULL;

                mSolutionList = NULL;

                mOpenList.clear ();
                mClosedList.clear ();
        }

        // ----------------------------------------------------------------------------
        ~AStarSearch(void)
        {
                _releaseNodes ();
                _releaseSolution ();
        }


        // ----------------------------------------------------------------------------
        void _releaseSolution ()
        {
                delete mSolutionList;
                mSolutionList = NULL;
        }


// Setup
// ----------------------------------------------------------------------------
public:
        void setStartState ( UserState* startState, UserState* endState )
        {
                _releaseNodes ();
                _releaseSolution ();

                mStartNode = NULL;
                mEndState = NULL;

                if ( fabs(endState->getTraversalCost() + 1.0f) < 0.01f )
                {
                        mCurState = SS_PATH_NOT_FOUND;
                        return;
                }

                mStartNode = new AStarNode ( startState );
                mStartNode->createNeighbourNodes ();
                mStartNode->setEstimatedGoalCost ( mStartNode->mUserState->getEstimatedCostToState ( *endState ) );

                mEndState = endState;

                mOpenList.push_back ( mStartNode );
        }


// Stepping
// ----------------------------------------------------------------------------
public:
        SearchState getSearchState ()
        {
                return mCurState;
        }

        bool isRunnable ()
        {
                return (mCurState == SS_RUNNING || mCurState == SS_NOT_STARTED );
        }

        bool isSolved ()
        {
                return (mCurState == SS_PATH_FOUND );
        }


        bool isUnsolvable ()
        {
                return (mCurState == SS_PATH_NOT_FOUND );
        }


        bool advanceSearch ()
        {
                if ( mCurState == SS_NOT_STARTED )
                        mCurState = SS_RUNNING;

                if ( mCurState != SS_RUNNING )
                        throw exception ( "AStarSearch::singleStep - Search not in a runnable state" );

                // We couldn't find a path
                if ( mOpenList.empty () )
                {
                        mCurState = SS_PATH_NOT_FOUND;
                }


                // Grab a node from the Open list
                AStarNode* curNode = _findBestOpenNode ();

                // We've reached the goal node so build our path
                if ( curNode->mUserState->isNode ( *mEndState ) )
                {
                        mCurState = SS_PATH_FOUND;
                        _constructionWaypointList ( curNode );
                        _releaseNodes ();
                        return true;
                }

                // Check all the neighbours for better matches and update any we find
                for ( int i = 0; i < curNode->mUserState->getNumNeighbours(); i++ )
                {
                        AStarNode* neighbour = curNode->getNeighbour ( i );
                        if ( neighbour == NULL )
                                continue;

                        // This neighbour is inpassable
                        if ( fabs( neighbour->getTraversalCost () + 1.0f ) < 0.01f )
                                continue;

                        // Determine if the node is on the closed list
                        AStarNode* closedNode = _getFromClosed ( neighbour );
                        if ( closedNode )
                        {
                                // If a node was already on the closed list, then it has been
                                // explored and has had it's neighbours explored. We may
                                // have a better way "in to" the node so consider it again
                                // with the new information
                                float newEntryCost = curNode->getEntryCost() + neighbour->getTraversalCost();
                                if ( newEntryCost < closedNode->getEntryCost() )
                                {
                                        // Update the neighbour to use the current node as the entry point
                                        neighbour->setSuccessor ( curNode );
                                        neighbour->setEntryCost ( newEntryCost );

                                        // We need to re-examine this node
                                        _removeFromClosed ( closedNode );
                                        _addToOpen ( neighbour );
                                }
                                continue;
                        }

                        // The node was set to be explored but we have found a better way in
                        // so consider this way into the node
                        AStarNode* openNode = _getFromOpen ( neighbour );
                        if ( openNode )
                        {
                                // If a node was already on the open list, it's estimated cost
                                // to the goal has already been set

                                float newEntryCost = curNode->getEntryCost() + neighbour->getTraversalCost();
                                if ( newEntryCost < neighbour->getEntryCost() )
                                {
                                        // Update the neighbour to use the current node as the entry point
                                        neighbour->setSuccessor ( curNode );
                                        neighbour->setEntryCost ( newEntryCost );
                                        
                                        _removeFromOpen ( openNode );
                                        _addToOpen ( neighbour );
                                }
                                continue;
                        }

                        // The neighbour state hasn't been considered before
                        // so set it up and add it to the open list for consideration
                        else
                        {
                                neighbour->setSuccessor ( curNode );
                                neighbour->setEntryCost ( curNode->getEntryCost() + neighbour->getTraversalCost () );
                                neighbour->setEstimatedGoalCost( neighbour->mUserState->getEstimatedCostToState( *mEndState ));
                                neighbour->createNeighbourNodes ();
                                _addToOpen ( neighbour );
                        }
                }

                _addToClosed ( curNode );

                return false;
        }

        // ----------------------------------------------------------------------------
        const WaypointList<UserState>*  getSolutionPath ()
        {
                if ( !mSolutionList )
                        throw exception ( "AStarSearch::getSolutionPath - No solution exists" );

                return mSolutionList;
        }


// Helpers
// ----------------------------------------------------------------------------
private:

        AStarNode* _findBestOpenNode ()
        {
                if ( mOpenList.empty () )
                        throw exception ( "AStarSearch::_findBestOpenNode - empty open list" );

                
#ifdef _USE_HEAP_
                AStarNode* bestNode = mOpenList.front();
                pop_heap ( mOpenList.begin(), mOpenList.end(), HeapCompare() );
                mOpenList.pop_back();
#else
                // Search the list backwards until we implement a priority heap
                AStarNode* bestNode = mOpenList.front ();
                float bestCost = bestNode->getTotalCost ();

                vector<AStarNode*>::iterator bestNodeIter = mOpenList.begin();
                vector<AStarNode*>::iterator iter = mOpenList.begin ();

                while ( iter != mOpenList.end () )
                {
                        if ( (*iter)->getTotalCost() < bestCost )
                        {
                                bestNode = (*iter);
                                bestCost = bestNode->getTotalCost();
                                bestNodeIter = iter;
                        }

                        iter++;
                }

                if ( bestNodeIter != mOpenList.end() )
                {
                        mOpenList.erase ( bestNodeIter );
                }
#endif

#if _DEBUG
                //cout << "Best Open Node: (" << bestNode->mUserState->mX << ", " << bestNode->mUserState->mY << ")" << endl;
                cout << "\tEntry Cost: " << bestNode->getEntryCost () << endl;
                cout << "\tGoal Cost: " << bestNode->getEstimatedGoalCost () << endl;
#endif

                return bestNode;
        }



        // ----------------------------------------------------------------------------
        AStarNode* _getFromClosed ( AStarNode* node )
        {
                vector<AStarNode*>::iterator iter = mClosedList.begin ();
                while ( iter != mClosedList.end () )
                {
                        if ( *(*iter) == (*node) )
                                return *iter;

                        iter++;
                }

                return NULL;
        }


        // ----------------------------------------------------------------------------
        AStarNode* _getFromOpen ( AStarNode* node )
        {
                vector<AStarNode*>::iterator iter = mOpenList.begin ();
                while ( iter != mOpenList.end () )
                {
                        if ( *(*iter) == (*node) )
                                return *iter;

                        iter++;
                }

                return NULL;
        }


        // ----------------------------------------------------------------------------
        bool _isNodeClosed ( AStarNode* node )
        {
                vector<AStarNode*>::iterator iter = mClosedList.begin ();
                while ( iter != mClosedList.end () )
                {
                        if ( *(*iter) == (*node) )
                                return true;

                        iter++;
                }
                return false;
        }

        // ----------------------------------------------------------------------------
        bool _isNodeOpen ( AStarNode* node )
        {
                vector<AStarNode*>::iterator iter = mClosedList.begin ();
                while ( iter != mClosedList.end () )
                {
                        if ( *(*iter) == (*node) )
                                return true;

                        iter++;
                }
                return false;
        }

        // ----------------------------------------------------------------------------
        void _removeFromClosed ( AStarNode* node )
        {
                vector<AStarNode*>::iterator iter = mClosedList.begin ();
                while ( iter != mClosedList.end () )
                {
                        if ( *(*iter) == (*node) )
                        {
                                mClosedList.erase ( iter );
                                return;
                        }

                        iter++;
                }

                throw exception ( "AStarSearch::_removeFromClosed - Node not found" );
        }

        // ----------------------------------------------------------------------------
        void _removeFromOpen ( AStarNode* node )
        {
                vector<AStarNode*>::iterator iter = mOpenList.begin ();
                while ( iter != mOpenList.end () )
                {
                        if ( *(*iter) == (*node) )
                        {
                                mOpenList.erase ( iter );
#ifdef _USE_HEAP_
                                make_heap ( mOpenList.begin(), mOpenList.end(), HeapCompare() );
#endif
                                return;
                        }

                        iter++;
                }

                throw exception ( "AStarSearch::_removeFromOpen - Node not found" );
        }

        // ----------------------------------------------------------------------------
        void _addToOpen ( AStarNode* node )
        {
                mOpenList.push_back ( node );
#ifdef _USE_HEAP_
                push_heap ( mOpenList.begin(), mOpenList.end(), HeapCompare() );
#endif
        }

        // ----------------------------------------------------------------------------
        void _addToClosed ( AStarNode* node )
        {
                mClosedList.push_back ( node );
        }

        // ----------------------------------------------------------------------------
        void _constructionWaypointList ( AStarNode* endNode )
        {
                mSolutionList = new WaypointList<UserState>();

                AStarNode* curNode = endNode;
                while ( curNode != NULL )
                {
                        mSolutionList->addWaypointToFront ( curNode->mUserState );
                        curNode = curNode->mSuccessor;
                }
        }

        // ----------------------------------------------------------------------------
        void _releaseNodes ()
        {
                // All nodes in the search are part of a connected tree with
                // mStartNode as the root
                delete mStartNode;
                mStartNode = NULL;
        }

};

template<typename UserState> int AStarSearch<UserState>::AStarNode::mNumNodes = 0;

#endif

---