PFAStar.h

Go to the documentation of this file.

//    Copyright (C) 2004-2011 Dylan Blair
//
//    email: dblair@alumni.cs.utexas.edu
//
//    This library is free software; you can redistribute it and/or
//    modify it under the terms of the GNU Lesser General Public
//    License as published by the Free Software Foundation; either
//    version 2.1 of the License, or (at your option) any later version.
//
//    This library 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
//    Lesser General Public License for more details.
//
//    You should have received a copy of the GNU Lesser General Public
//    License along with this library; if not, write to the Free Software
//    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#ifndef PF_A_STAR_H
#define PF_A_STAR_H

#include "../CD_LIB/CDGrid.h"

#include "PFConsts.h"

namespace OpenSkyNet {
    namespace PF {
        struct DirOrPath {
            bool _isDir;
            CD::DIRECTION _dir;
            Math::Path _path;
            DirOrPath() : _isDir(true), _dir(CD::NONE) {}
            DirOrPath(CD::DIRECTION dir_) : _isDir(true), _dir(dir_) {}
            DirOrPath(const Math::Path& path_) : _isDir(false), _dir(CD::NONE), _path(path_) {}
        };

        struct AStarNode {
            AStarNode* _parent;
            bool _isFromWPC;
            CD::DIRECTION _prevDir;
#ifdef PF_USES_WPC
            Math::Path _prevPath;
#endif
            const Math::Point<int>& _bin;
            float _f,_g,_h;

            AStarNode(const Math::Point<int>& bin_) : _parent(0), _isFromWPC(false), _prevDir(CD::NONE), _bin(bin_), 
                _f((CD::MAX_X_DIVISIONS_FOR_ALL_GRIDS*CD::MAX_Y_DIVISIONS_FOR_ALL_GRIDS*CD::MAX_Z_DIVISIONS_FOR_ALL_GRIDS)+1), 
                _g(0), _h(0) {}

            inline bool operator<(const AStarNode& rhs_) const { return _f < rhs_._f; }
        private:
#ifdef _WIN32
#pragma warning(push)
#pragma warning(disable:4100) //unreferenced formal parameter
#endif
            AStarNode& operator=(const AStarNode& rhs_) { return *this; }
#ifdef _WIN32
#pragma warning(pop)
#endif
        };

        class PriorityQueue {
        public:
            Utils::HashTableUIntKeys<AStarNode*> _nodes;
        private:
            std::vector<AStarNode*> _heap;
            Utils::uint _currSize;
            Utils::HashTableUIntKeys<float> _minGCosts;

            inline void percolateDown(Utils::uint hole_) {
                Utils::uint child;
                AStarNode* tmp = _heap[hole_];
                for ( ; (hole_ << 1) <= _currSize; hole_ = child) {
                    child = hole_ << 1;
                    if ((child != _currSize) && (*(_heap[child+1]) < *(_heap[child])))
                        child++;
                    if (*(_heap[child]) < *tmp)
                        _heap[hole_] = _heap[child];
                    else
                        break;
                }
                _heap[hole_] = tmp;
            }
        public:
            PriorityQueue();
            ~PriorityQueue();

            inline void insert(AStarNode* node_) {
                if (++_currSize == _heap.size())
                    _heap.resize(_currSize << 1,0);
                int hole = 0;
                for (hole = _currSize; (hole > 1) && (*node_ < *(_heap[hole >> 1])); hole = hole >> 1)
                    _heap[hole] = _heap[hole >> 1];
                _heap[hole] = node_;
                _minGCosts.add(Utils::getCompositeKey(node_->_bin)) = node_->_g;
            }

            inline void removeMin(AStarNode*& min_) {
                min_ = _heap[1];
                _heap[1] = _heap[_currSize--];
                percolateDown(1);
                //NOTE: If I somehow flagged min_ in _minGCosts as final, which is one way 
                //      to add it to a "closed set," then I could replace the addition and 
                //      comparison check in processSuccessor() with this flag check.
            }

            void init(const Math::Point<int>& bin_);

            inline bool isEmpty() const { return !_currSize; }

            inline float getMinGCost(Math::Point<int> bin_) const {
                return _minGCosts[Utils::getCompositeKey(bin_)];
            }

            inline void processSuccessor(AStarNode* curr_, AStarNode* successor_, const Math::Point<int>& targBin_, CD::DIRECTION prevDir_) {
                //see note in removeMin()

                if (prevDir_ > CD::POS_Z) {
                    if (prevDir_ > CD::POS_Y_POS_Z) {
                        if ((curr_->_g + 1.73205f) < getMinGCost(successor_->_bin)) {
                            successor_->_g = curr_->_g + 1.73205f;
                            successor_->_h = sqrtf(static_cast<float>((targBin_ - successor_->_bin).getLenSqrd()));
                            successor_->_f = successor_->_g + successor_->_h;
                            successor_->_parent = curr_;
                            successor_->_prevDir = prevDir_;
                            successor_->_isFromWPC = false;
                            insert(successor_);
                        }
                    }
                    else {
                        if ((curr_->_g + 1.41421f) < getMinGCost(successor_->_bin)) {
                            successor_->_g = curr_->_g + 1.41421f;
                            successor_->_h = sqrtf(static_cast<float>((targBin_ - successor_->_bin).getLenSqrd()));
                            successor_->_f = successor_->_g + successor_->_h;
                            successor_->_parent = curr_;
                            successor_->_prevDir = prevDir_;
                            successor_->_isFromWPC = false;
                            insert(successor_);
                        }
                    }
                }
                else {
                    if ((curr_->_g + 1.0f) < getMinGCost(successor_->_bin)) {
                        successor_->_g = curr_->_g + 1.0f;
                        successor_->_h = sqrtf(static_cast<float>((targBin_ - successor_->_bin).getLenSqrd()));
                        successor_->_f = successor_->_g + successor_->_h;
                        successor_->_parent = curr_;
                        successor_->_prevDir = prevDir_;
                        successor_->_isFromWPC = false;
                        insert(successor_);
                    }
                }
            }

#ifdef PF_USES_WPC
            inline void processSuccessor(AStarNode* curr_, AStarNode* successor_, const Math::Point<int>& targBin_, float prevPathBinDist_) {
                //see note in removeMin()

                float successorG = curr_->_g + prevPathBinDist_;
                if (successorG < getMinGCost(successor_->_bin)) {
                    successor_->_g = successorG;
                    successor_->_h = sqrtf(static_cast<float>((targBin_ - successor_->_bin).getLenSqrd()));
                    successor_->_f = successor_->_g + successor_->_h;
                    successor_->_parent = curr_;
                    successor_->_isFromWPC = true;
                    insert(successor_);
                }
            }
#endif

            void getDirsOrPaths(AStarNode* curr_, std::vector<DirOrPath>& dirsOrPaths_) const;
        };
    }
}

#endif //PF_A_STAR_H