micropather.h

/* ScummVM - Graphic Adventure Engine
 *
 * ScummVM is the legal property of its developers, whose names
 * are too numerous to list here. Please refer to the COPYRIGHT
 * file distributed with this source distribution.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

/*

This is a lightly modified version of MicroPather, from
github.com/leethomason/MicroPather.  Modifications were made to fit with
ScummVM coding style and APIs.

The original copyright message is:

-------

Copyright (c) 2000-2013 Lee Thomason (www.grinninglizard.com)
Micropather

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 TETRAEDGE_TE_MICROPATHER
#define TETRAEDGE_TE_MICROPATHER

#include "common/array.h"
#include "common/util.h"
#include "common/types.h"
#include "math/utils.h"


namespace Tetraedge
{

namespace micropather
{

    typedef uintptr MP_UPTR;

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


    class Graph
    {
    public:
        virtual ~Graph() {}

        virtual float LeastCostEstimate( void* stateStart, void* stateEnd ) = 0;

        virtual void AdjacentCost( void* state, Common::Array< 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();
        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 TETRAEDGE_MICROPATHER_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 );

        // Get a pathnode that is already in the pool.
        PathNode* FetchPathNode( void* state );

        // 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 );

        // Return all the allocated states. Useful for visuallizing what
        // the pather is doing.
        void AllStates( unsigned frame, Common::Array< 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;
    };


    /* Used to cache results of paths. Much, much faster
       to return an existing solution than to calculate
       a new one. A post on this is here:
       https://web.archive.org/web/20170714084918/http://grinninglizard.com/altera/programming/a-path-caching-2/
    */
    class PathCache
    {
    public:
        struct Item {
            // The key:
            void* start;
            void* end;

            bool KeyEqual( const Item& item ) const { return start == item.start && end == item.end; }
            bool Empty() const                      { return start == 0 && end == 0; }

            // Data:
            void*   next;
            float   cost;   // from 'start' to 'next'. FLT_MAX if unsolveable.

            unsigned Hash() const {
                const unsigned char *p = (const unsigned char *)(&start);
                uint h = 2166136261U;

                for( unsigned i=0; i<sizeof(void*)*2; ++i, ++p ) {
                    h ^= *p;
                    h *= 16777619;
                }
                return h;
            }
        };

        PathCache( int itemsToAllocate );
        ~PathCache();

        void Reset();
        void Add( const Common::Array< void* >& path, const Common::Array< float >& cost );
        void AddNoSolution( void* end, void* states[], int count );
        int Solve( void* startState, void* endState, Common::Array< void* >* path, float* totalCost );

        int AllocatedBytes() const { return allocated * sizeof(Item); }
        int UsedBytes() const { return nItems * sizeof(Item); }

        int hit;
        int miss;

    private:
        void AddItem( const Item& item );
        const Item* Find( void* start, void* end );

        Item*   mem;
        int     allocated;
        int     nItems;
    };

    struct CacheData {
        CacheData() { reset(); }

        int nBytesAllocated;
        int nBytesUsed;
        float memoryFraction;

        int hit;
        int miss;
        float hitFraction;

        void reset() {
            nBytesAllocated = 0;
            nBytesUsed = 0;
            memoryFraction = 0;

            hit = 0;
            miss = 0;
            hitFraction = 0;
        }
    };

    class MicroPather
    {
        friend class micropather::PathNode;

    public:
        enum
        {
            SOLVED,
            NO_SOLUTION,
            START_END_SAME,

            // internal
            NOT_CACHED
        };

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

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

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

        void Reset();

        // Debugging function to return all states that were used by the last "solve"
        void StatesInPool( Common::Array< void* >* stateVec );
        void GetCacheData( CacheData* data );

    private:
        MicroPather( const MicroPather& );  // undefined and unsupported
        void operator=( const MicroPather ); // undefined and unsupported

        void GoalReached( PathNode* node, void* start, void* end, Common::Array< void* > *path );

        void GetNodeNeighbors(  PathNode* node, Common::Array< NodeCost >* neighborNode );

#ifdef TETRAEDGE_MICROPATHER_DEBUG
        void DumpStats();
#endif

        PathNodePool            pathNodePool;
        Common::Array< StateCost >  stateCostVec;   // local to Solve, but put here to reduce memory allocation
        Common::Array< NodeCost >   nodeCostVec;    // local to Solve, but put here to reduce memory allocation
        Common::Array< float >      costVec;

        Graph* graph;
        unsigned frame;                     // incremented with every solve, used to determine if cached data needs to be refreshed
        PathCache* pathCache;
    };

}   // namespace micropather

}   // namespace Tetraedge

#endif // TETRAEDGE_TE_MICROPATHER