PF_API 0.52
|
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