PF_API 0.52

Android/jni/Libs/PF_LIB/PFAStar.h

Go to the documentation of this file.
00001 
00002 //    Copyright (C) 2004-2011 Dylan Blair
00003 //
00004 //    email: dblair@alumni.cs.utexas.edu
00005 //
00006 //    This library is free software; you can redistribute it and/or
00007 //    modify it under the terms of the GNU Lesser General Public
00008 //    License as published by the Free Software Foundation; either
00009 //    version 2.1 of the License, or (at your option) any later version.
00010 //
00011 //    This library is distributed in the hope that it will be useful,
00012 //    but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 //    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014 //    Lesser General Public License for more details.
00015 //
00016 //    You should have received a copy of the GNU Lesser General Public
00017 //    License along with this library; if not, write to the Free Software
00018 //    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020 
00021 #ifndef PF_A_STAR_H
00022 #define PF_A_STAR_H
00023 
00024 #include "../CD_LIB/CDGrid.h"
00025 
00026 #include "PFConsts.h"
00027 
00028 namespace OpenSkyNet {
00029     namespace PF {
00033         struct DirOrPath {
00034             bool _isDir;
00035             CD::DIRECTION _dir;
00036             Math::Path _path;
00037             DirOrPath() : _isDir(true), _dir(CD::NONE) {}
00038             DirOrPath(CD::DIRECTION dir_) : _isDir(true), _dir(dir_) {}
00039             DirOrPath(const Math::Path& path_) : _isDir(false), _dir(CD::NONE), _path(path_) {}
00040         };
00041 
00043         struct AStarNode {
00044             AStarNode* _parent;
00045             bool _isFromWPC;
00046             CD::DIRECTION _prevDir;
00047 #ifdef PF_USES_WPC
00048             Math::Path _prevPath;
00049 #endif
00050             const Math::Point<int>& _bin;
00051             float _f,_g,_h;
00052 
00053             AStarNode(const Math::Point<int>& bin_) : _parent(0), _isFromWPC(false), _prevDir(CD::NONE), _bin(bin_), 
00054                 _f((CD::MAX_X_DIVISIONS_FOR_ALL_GRIDS*CD::MAX_Y_DIVISIONS_FOR_ALL_GRIDS*CD::MAX_Z_DIVISIONS_FOR_ALL_GRIDS)+1), 
00055                 _g(0), _h(0) {}
00056 
00057             inline bool operator<(const AStarNode& rhs_) const { return _f < rhs_._f; }
00058         private:
00059 #ifdef _WIN32
00060 #pragma warning(push)
00061 #pragma warning(disable:4100) //unreferenced formal parameter
00062 #endif
00063             AStarNode& operator=(const AStarNode& rhs_) { return *this; }
00064 #ifdef _WIN32
00065 #pragma warning(pop)
00066 #endif
00067         };
00068 
00070         class PriorityQueue {
00071         public:
00072             Utils::HashTableUIntKeys<AStarNode*> _nodes;
00073         private:
00074             std::vector<AStarNode*> _heap;
00075             Utils::uint _currSize;
00076             Utils::HashTableUIntKeys<float> _minGCosts;
00077 
00078             inline void percolateDown(Utils::uint hole_) {
00079                 Utils::uint child;
00080                 AStarNode* tmp = _heap[hole_];
00081                 for ( ; (hole_ << 1) <= _currSize; hole_ = child) {
00082                     child = hole_ << 1;
00083                     if ((child != _currSize) && (*(_heap[child+1]) < *(_heap[child])))
00084                         child++;
00085                     if (*(_heap[child]) < *tmp)
00086                         _heap[hole_] = _heap[child];
00087                     else
00088                         break;
00089                 }
00090                 _heap[hole_] = tmp;
00091             }
00092         public:
00093             PriorityQueue();
00094             ~PriorityQueue();
00095 
00096             inline void insert(AStarNode* node_) {
00097                 if (++_currSize == _heap.size())
00098                     _heap.resize(_currSize << 1,0);
00099                 int hole = 0;
00100                 for (hole = _currSize; (hole > 1) && (*node_ < *(_heap[hole >> 1])); hole = hole >> 1)
00101                     _heap[hole] = _heap[hole >> 1];
00102                 _heap[hole] = node_;
00103                 _minGCosts.add(Utils::getCompositeKey(node_->_bin)) = node_->_g;
00104             }
00105 
00106             inline void removeMin(AStarNode*& min_) {
00107                 min_ = _heap[1];
00108                 _heap[1] = _heap[_currSize--];
00109                 percolateDown(1);
00110             }
00111 
00112             void init(const Math::Point<int>& bin_);
00113 
00114             inline bool isEmpty() const { return !_currSize; }
00115 
00116             inline float getMinGCost(Math::Point<int> bin_) const {
00117                 return _minGCosts[Utils::getCompositeKey(bin_)];
00118             }
00119 
00120             inline void processSuccessor(AStarNode* curr_, AStarNode* successor_, const Math::Point<int>& targBin_, CD::DIRECTION prevDir_) {
00121                 if (prevDir_ > CD::POS_Z) {
00122                     if (prevDir_ > CD::POS_Y_POS_Z) {
00123                         if ((curr_->_g + 1.73205f) < getMinGCost(successor_->_bin)) {
00124                             successor_->_g = curr_->_g + 1.73205f;
00125                             successor_->_h = sqrtf(static_cast<float>((targBin_ - successor_->_bin).getLenSqrd()));
00126                             successor_->_f = successor_->_g + successor_->_h;
00127                             successor_->_parent = curr_;
00128                             successor_->_prevDir = prevDir_;
00129                             successor_->_isFromWPC = false;
00130                             insert(successor_);
00131                         }
00132                     }
00133                     else {
00134                         if ((curr_->_g + 1.41421f) < getMinGCost(successor_->_bin)) {
00135                             successor_->_g = curr_->_g + 1.41421f;
00136                             successor_->_h = sqrtf(static_cast<float>((targBin_ - successor_->_bin).getLenSqrd()));
00137                             successor_->_f = successor_->_g + successor_->_h;
00138                             successor_->_parent = curr_;
00139                             successor_->_prevDir = prevDir_;
00140                             successor_->_isFromWPC = false;
00141                             insert(successor_);
00142                         }
00143                     }
00144                 }
00145                 else {
00146                     if ((curr_->_g + 1.0f) < getMinGCost(successor_->_bin)) {
00147                         successor_->_g = curr_->_g + 1.0f;
00148                         successor_->_h = sqrtf(static_cast<float>((targBin_ - successor_->_bin).getLenSqrd()));
00149                         successor_->_f = successor_->_g + successor_->_h;
00150                         successor_->_parent = curr_;
00151                         successor_->_prevDir = prevDir_;
00152                         successor_->_isFromWPC = false;
00153                         insert(successor_);
00154                     }
00155                 }
00156             }
00157 
00158 #ifdef PF_USES_WPC
00159             inline void processSuccessor(AStarNode* curr_, AStarNode* successor_, const Math::Point<int>& targBin_, float prevPathBinDist_) {
00160                 float successorG = curr_->_g + prevPathBinDist_;
00161                 if (successorG < getMinGCost(successor_->_bin)) {
00162                     successor_->_g = successorG;
00163                     successor_->_h = sqrtf(static_cast<float>((targBin_ - successor_->_bin).getLenSqrd()));
00164                     successor_->_f = successor_->_g + successor_->_h;
00165                     successor_->_parent = curr_;
00166                     successor_->_isFromWPC = true;
00167                     insert(successor_);
00168                 }
00169             }
00170 #endif
00171 
00172             void getDirsOrPaths(AStarNode* curr_, std::vector<DirOrPath>& dirsOrPaths_) const;
00173         };
00174     }
00175 }
00176 
00177 #endif //PF_A_STAR_H
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines