dgPathFinder.h

/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
*
* 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 __dgPathFinder__
#define __dgPathFinder__

#include "dgStdafx.h"
#include "dgTree.h"
#include "dgHeap.h"

template<class NODEID, class COST> class dgPathFinder;


#define DG_MAX_PATH_ENUMERATION_CHILDREN    128

template<class NODEID, class COST>
class dgPathNode {
    friend class dgPathFinder<NODEID, COST>;
    NODEID m_Id;
    COST m_NodeCostToGoal;
    COST m_NodeCostFromSource;
    dgPathNode *m_Next;

public:
    dgPathNode();
    NODEID GetId() const;
    const dgPathNode *GetNext() const;
    const dgPathNode *GetParent() const;
};

template<class NODEID, class COST>
class dgPathCloseList: public dgTree<dgPathNode<NODEID, COST>, NODEID> {
protected:
    dgPathCloseList(): dgTree<dgPathNode<NODEID, COST>, NODEID>() {}
};


template<class NODEID, class COST>
class dgPathOpenHeap: public dgUpHeap<typename dgPathCloseList<NODEID, COST>::dgTreeNode *, COST> {
protected:
    dgPathOpenHeap(dgInt32 maxElements)
        : dgUpHeap<typename dgPathCloseList<NODEID, COST>::dgTreeNode *, COST>(maxElements) {
    }

    friend class dgPathFinder<NODEID, COST>;
};


template<class NODEID, class COST>
class dgPathFinder: public dgPathCloseList<NODEID, COST> {
    dgInt32 maxChildren;

public:
    dgPathFinder(dgInt32 maxElementsInOpenList, dgInt32 nodeMaxChildren);
    virtual ~dgPathFinder();

    virtual const dgPathNode<NODEID, COST> *CalCulatePath(NODEID source, NODEID goal);

    // this funtions must be overloaded by the user
    virtual COST GetCostFromParent(const dgPathNode<NODEID, COST> &node) const;
    virtual COST GetEstimatedCostToGoal(NODEID id) const;
    virtual dgInt32 EnumerateChildren(NODEID parent, NODEID array[]) const;

    dgPathOpenHeap<NODEID, COST> m_openList;
};


template<class NODEID, class COST>
dgPathNode<NODEID, COST>::dgPathNode() {
}

template<class NODEID, class COST>
NODEID dgPathNode<NODEID, COST>::GetId() const {
    return m_Id;
}


template<class NODEID, class COST>
const dgPathNode<NODEID, COST> *dgPathNode<NODEID, COST>::GetNext() const {
    return m_Next;
}


template<class NODEID, class COST>
const dgPathNode<NODEID, COST> *dgPathNode<NODEID, COST>::GetParent() const {
    return m_Next;
}


template<class NODEID, class COST>
dgPathFinder<NODEID, COST>::dgPathFinder(
    dgInt32 maxElementsInOpenList,
    dgInt32 nodeMaxChildren)
    : dgPathCloseList<NODEID, COST>(),
      m_openList(maxElementsInOpenList) {
    maxChildren = nodeMaxChildren;
}


template<class NODEID, class COST>
dgPathFinder<NODEID, COST>::~dgPathFinder() {
}

template<class NODEID, class COST>
const dgPathNode<NODEID, COST> *dgPathFinder<NODEID, COST>::CalCulatePath(NODEID source, NODEID goal) {
    dgInt32 count;
    dgInt32 heapMaxCount;
    dgPathNode<NODEID, COST> cell;
    dgPathNode<NODEID, COST> *next;
    dgPathNode<NODEID, COST> *prev;
    dgPathNode<NODEID, COST> *link;
    typename dgPathCloseList<NODEID, COST>::dgTreeNode *node;
    NODEID idArray[DG_MAX_PATH_ENUMERATION_CHILDREN];

    dgPathCloseList<NODEID, COST> &close = *this;

    m_openList.Flush();
    close.RemoveAll();

    cell.m_Id = source;
    cell.m_Next = NULL;
    cell.m_NodeCostFromSource = COST(0);
    cell.m_NodeCostToGoal = GetEstimatedCostToGoal(cell.m_Id);

    node = close.Insert(cell, source);

    heapMaxCount = m_openList.GetMaxCount();
    m_openList.Push(node, cell.m_NodeCostFromSource + cell.m_NodeCostToGoal);
    while (m_openList.GetCount()) {
        node = m_openList[0];
        dgPathNode<NODEID, COST> &parent = node->GetInfo();
        if (parent.m_Id == goal) {
            link = &parent;
            next = NULL;
            do {
                prev = link->m_Next;
                link->m_Next = next;
                next = link;
                link = prev;
            } while (link);
            return next;
        }

        m_openList.Pop();
        count = EnumerateChildren(node->GetKey(), idArray);
        cell.m_Next = &parent;
        for (int i = 0; i < count; i ++) {
            bool state;
            cell.m_Id = idArray[i];

            COST newCostFromSource(GetCostFromParent(cell) + parent.m_NodeCostFromSource);
            node = close.Insert(cell, cell.m_Id, state);
            dgPathNode<NODEID, COST> &newCell = node->GetInfo();
            if (state) {
                if (newCell.m_NodeCostFromSource <= newCostFromSource) {
                    continue;
                }
                //newCell.m_Next = cell.m_Next;
            }

            newCell.m_NodeCostFromSource = newCostFromSource;
            newCell.m_NodeCostToGoal = GetEstimatedCostToGoal(newCell.m_Id);
            if (m_openList.GetCount() >= heapMaxCount) {
                m_openList.Remove(heapMaxCount);
            }
            m_openList.Push(node, newCell.m_NodeCostFromSource + newCell.m_NodeCostToGoal);
        }
    }
    return NULL;
}

template<class NODEID, class COST>
COST dgPathFinder<NODEID, COST>::GetCostFromParent(const dgPathNode<NODEID, COST> &node) const {
    return COST(1);
}

template<class NODEID, class COST>
COST dgPathFinder<NODEID, COST>::GetEstimatedCostToGoal(NODEID id) const {
    return COST(1);
}

template<class NODEID, class COST>
dgInt32 dgPathFinder<NODEID, COST>::EnumerateChildren(NODEID parent, NODEID array[]) const {
    return 0;
}




#endif