micropather.h

/*
Copyright (c) 2000-2009 Lee Thomason (www.grinninglizard.com)
Grinning Lizard Utilities.

This software is provided 'as-is', without any express or implied 
warranty. In no event will the authors be held liable for any 
damages arising from the use of this software.

Permission is granted to anyone to use this software for any 
purpose, including commercial applications, and to alter it and 
redistribute it freely, subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must 
not claim that you wrote the original software. If you use this 
software in a product, an acknowledgment in the product documentation 
would be appreciated but is not required.

2. Altered source versions must be plainly marked as such, and 
must not be misrepresented as being the original software.

3. This notice may not be removed or altered from any source 
distribution.
*/


#ifndef GRINNINGLIZARD_MICROPATHER_INCLUDED
#define GRINNINGLIZARD_MICROPATHER_INCLUDED


#include <vector>
#include <float.h>

#ifdef _DEBUG
    #ifndef DEBUG
        #define DEBUG
    #endif
#endif


#if defined( _DEBUG )
#   if defined( _MSC_VER )
#       define MPASSERT( x )        if ( !(x)) { _asm { int 3 } }
#   else
#       include <assert.h>
#       define MPASSERT assert
#   endif
#else
#   define MPASSERT( x ) {}
#endif


#if defined(_MSC_VER) && (_MSC_VER >= 1400 )
    #include <stdlib.h>
    typedef uintptr_t       MP_UPTR;
#elif defined (__GNUC__) && (__GNUC__ >= 3 )
    #include <stdlib.h>
    typedef uintptr_t       MP_UPTR;
#else
    // Assume not 64 bit pointers. Get a new compiler.
    typedef unsigned MP_UPTR;
#endif

//#define MICROPATHER_STRESS

namespace micropather
{
    struct StateCost
    {
        void* state;            
        float cost;             
    };


    class Graph
    {
      public:
        virtual ~Graph() {}
      
        virtual float LeastCostEstimate( void* stateStart, void* stateEnd ) = 0;

        virtual void AdjacentCost( void* state, std::vector< micropather::StateCost > *adjacent ) = 0;

        virtual void  PrintStateInfo( void* state ) = 0;
    };


    class PathNode;

    struct NodeCost
    {
        PathNode* node;
        float cost;
    };


    /*
        Every state (void*) is represented by a PathNode in MicroPather. There
        can only be one PathNode for a given state.
    */
    class PathNode
    {
      public:
        void Init(  unsigned _frame,
                    void* _state,
                    float _costFromStart, 
                    float _estToGoal, 
                    PathNode* _parent );

        void Clear() {
            memset( this, 0, sizeof( PathNode ) );
            numAdjacent = -1;
            cacheIndex  = -1;
        }
        void InitSentinel() {
            Clear();
            Init( 0, 0, FLT_MAX, FLT_MAX, 0 );
            prev = next = this;
        }   

        void *state;            // the client state
        float costFromStart;    // exact
        float estToGoal;        // estimated
        float totalCost;        // could be a function, but save some math.
        PathNode* parent;       // the parent is used to reconstruct the path
        unsigned frame;         // unique id for this path, so the solver can distinguish
                                // correct from stale values

        int numAdjacent;        // -1  is unknown & needs to be queried
        int cacheIndex;         // position in cache

        PathNode *child[2];     // Binary search in the hash table. [left, right]
        PathNode *next, *prev;  // used by open queue

        bool inOpen;
        bool inClosed;

        void Unlink() {
            next->prev = prev;
            prev->next = next;
            next = prev = 0;
        }
        void AddBefore( PathNode* addThis ) {
            addThis->next = this;
            addThis->prev = prev;
            prev->next = addThis;
            prev = addThis;
        }
        #ifdef DEBUG
        void CheckList()
        {
            MPASSERT( totalCost == FLT_MAX );
            for( PathNode* it = next; it != this; it=it->next ) {
                MPASSERT( it->prev == this || it->totalCost >= it->prev->totalCost );
                MPASSERT( it->totalCost <= it->next->totalCost );
            }
        }
        #endif

        void CalcTotalCost() {
            if ( costFromStart < FLT_MAX && estToGoal < FLT_MAX )
                totalCost = costFromStart + estToGoal;
            else
                totalCost = FLT_MAX;
        }

      private:

        void operator=( const PathNode& );
    };


    /* Memory manager for the PathNodes. */
    class PathNodePool
    {
    public:
        PathNodePool( unsigned allocate, unsigned typicalAdjacent );
        ~PathNodePool();

        // Free all the memory except the first block. Resets all memory.
        void Clear();

        // Essentially:
        // pNode = Find();
        // if ( !pNode )
        //      pNode = New();
        //
        // Get the PathNode associated with this state. If the PathNode already
        // exists (allocated and is on the current frame), it will be returned. 
        // Else a new PathNode is allocated and returned. The returned object
        // is always fully initialized.
        //
        // NOTE: if the pathNode exists (and is current) all the initialization
        //       parameters are ignored.
        PathNode* GetPathNode(      unsigned frame,
                                    void* _state,
                                    float _costFromStart, 
                                    float _estToGoal, 
                                    PathNode* _parent );

        // Store stuff in cache
        bool PushCache( const NodeCost* nodes, int nNodes, int* start );

        // Get neighbors from the cache
        // Note - always access this with an offset. Can get re-allocated.
        void GetCache( int start, int nNodes, NodeCost* nodes ) {
            MPASSERT( start >= 0 && start < cacheCap );
            MPASSERT( nNodes > 0 );
            MPASSERT( start + nNodes <= cacheCap );
            memcpy( nodes, &cache[start], sizeof(NodeCost)*nNodes );
        }

        // Return all the allocated states. Useful for visuallizing what
        // the pather is doing.
        void AllStates( unsigned frame, std::vector< void* >* stateVec );

    private:
        struct Block
        {
            Block* nextBlock;
            PathNode pathNode[1];
        };

        unsigned Hash( void* voidval );
        unsigned HashSize() const   { return 1<<hashShift; }
        unsigned HashMask() const   { return ((1<<hashShift)-1); }
        void AddPathNode( unsigned key, PathNode* p );
        Block* NewBlock();
        PathNode* Alloc();

        PathNode**  hashTable;
        Block*      firstBlock;
        Block*      blocks;

        NodeCost*   cache;
        int         cacheCap;
        int         cacheSize;

        PathNode    freeMemSentinel;
        unsigned    allocate;               // how big a block of pathnodes to allocate at once
        unsigned    nAllocated;             // number of pathnodes allocated (from Alloc())
        unsigned    nAvailable;             // number available for allocation

        unsigned    hashShift;  
        unsigned    totalCollide;
    };


    class MicroPather
    {
        friend class micropather::PathNode;

      public:
        enum
        {
            SOLVED,
            NO_SOLUTION,
            START_END_SAME,
        };

        MicroPather( Graph* graph, unsigned allocate = 250, unsigned typicalAdjacent=6 );
        ~MicroPather();

        int Solve( void* startState, void* endState, std::vector< void* >* path, float* totalCost );

        int SolveForNearStates( void* startState, std::vector< StateCost >* near, float maxCost );

        void Reset();

        MP_UPTR Checksum()  { return checksum; }

        // Debugging function to return all states that were used by the last "solve" 
        void StatesInPool( std::vector< void* >* stateVec );

      private:
        MicroPather( const MicroPather& );  // undefined and unsupported
        void operator=( const MicroPather ); // undefined and unsupported
        
        void GoalReached( PathNode* node, void* start, void* end, std::vector< void* > *path );

        void GetNodeNeighbors(  PathNode* node, std::vector< NodeCost >* neighborNode );

        #ifdef DEBUG
        //void DumpStats();
        #endif

        PathNodePool                pathNodePool;
        std::vector< StateCost >    stateCostVec;   // local to Solve, but put here to reduce memory allocation
        std::vector< NodeCost >     nodeCostVec;    // local to Solve, but put here to reduce memory allocation

        Graph* graph;
        unsigned frame;                     // incremented with every solve, used to determine if cached data needs to be refreshed
        MP_UPTR checksum;                       // the checksum of the last successful "Solve".
        
    };
};  // namespace grinliz

#endif

---