00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "global.hpp"
00017
00018 #include "astarnode.hpp"
00019 #include "log.hpp"
00020 #include "pathfind.hpp"
00021 #include "util.hpp"
00022
00023 #include <cassert>
00024 #include <cmath>
00025 #include <iostream>
00026
00027 #define LOG_PF LOG_STREAM(info, engine)
00028 #define DBG_PF LOG_STREAM(debug, engine)
00029
00030 typedef std::vector<gamemap::location> vector_location;
00031 typedef std::vector<a_star_node*> vector_a_star_node;
00032 typedef std::set<gamemap::location> set_location;
00033
00034
00035
00036 static bool compare_lt_a_star_node(const a_star_node* node1, const a_star_node* node2) {
00037 return node1->g + node1->h > node2->g + node2->h;
00038 }
00039
00040 static void a_star_init(gamemap::location const &src, gamemap::location const &dst,
00041 vector_a_star_node &openList, a_star_world &aStarGameWorld,
00042 const size_t parWidth, const size_t parHeight,
00043 vector_location &vectLocation, std::set<gamemap::location> const *teleports,
00044 size_t &parNbTeleport)
00045 {
00046 bool locIsCreated;
00047
00048 aStarGameWorld.resize_IFN(parWidth, parHeight);
00049 a_star_node *locStartNode = aStarGameWorld.getNodeFromLocation(src, locIsCreated);
00050 assert(locIsCreated);
00051 locStartNode->initNode(src, dst, 0.0, NULL, teleports);
00052
00053 const size_t locValueH = size_t(locStartNode->h);
00054 size_t locAllocSize;
00055
00056 if (locValueH < 16)
00057 locAllocSize = 16;
00058 else if (locValueH > 128)
00059 locAllocSize = 128;
00060 else
00061 locAllocSize = locValueH;
00062 openList.reserve(locAllocSize);
00063 openList.push_back(locStartNode);
00064
00065 if (teleports != NULL)
00066 parNbTeleport = teleports->size();
00067 else
00068 parNbTeleport = 0;
00069
00070 vectLocation.reserve(parNbTeleport + 6);
00071 vectLocation.resize(parNbTeleport + 6);
00072
00073 if (parNbTeleport > 0)
00074 std::copy(teleports->begin(), teleports->end(), &vectLocation[6]);
00075 }
00076
00077 static void a_star_explore_neighbours(gamemap::location const &dst, const double stop_at,
00078 cost_calculator const *costCalculator,
00079 const size_t parWidth, const size_t parHeight,
00080 std::set<gamemap::location> const *teleports,
00081 vector_location &vectLocation, vector_a_star_node &openList,
00082 a_star_world &aStarGameWorld,
00083 a_star_node *parCurNode, const size_t parNbTeleport)
00084 {
00085 typedef std::pair<vector_a_star_node::iterator, vector_a_star_node::iterator> pair_node_iter;
00086
00087 a_star_node *locNextNode;
00088 double locCost;
00089 pair_node_iter locPlace;
00090 size_t locSize;
00091 bool locIsCreated;
00092 const double locCostFather = parCurNode->g;
00093
00094 get_adjacent_tiles(parCurNode->loc, &vectLocation[0]);
00095
00096 if (parNbTeleport > 0 && teleports->count(parCurNode->loc) > 0)
00097 locSize = parNbTeleport + 6;
00098 else
00099 locSize = 6;
00100
00101 bool broken_heap = false;
00102 int locNbAdded = 0;
00103
00104 for (size_t i = 0; i != locSize; ++i)
00105 {
00106 const gamemap::location& locLocation = vectLocation[i];
00107
00108 if (locLocation.valid(int(parWidth), int(parHeight)) == false)
00109 continue;
00110 locNextNode = aStarGameWorld.getNodeFromLocation(locLocation, locIsCreated);
00111 locCost = locCostFather + costCalculator->cost(parCurNode->loc,locLocation, locCostFather);
00112 if (locIsCreated) {
00113 locNextNode->initNode(locLocation, dst, locCost, parCurNode, teleports);
00114 if (locNextNode->g + locNextNode->h < stop_at) {
00115 openList.push_back(locNextNode);
00116 ++locNbAdded;
00117 } else
00118 locNextNode->isInCloseList = true;
00119
00120 } else if (locCost < locNextNode->g) {
00121
00122 if (locNextNode->isInCloseList) {
00123 locNextNode->isInCloseList = false;
00124 openList.push_back(locNextNode);
00125 ++locNbAdded;
00126 } else
00127 broken_heap = true;
00128
00129 locNextNode->g = locCost;
00130 locNextNode->nodeParent = parCurNode;
00131 }
00132 }
00133
00134 vector_a_star_node::iterator openList_begin = openList.begin(),
00135 openList_end = openList.end();
00136 if (broken_heap)
00137 std::make_heap(openList_begin, openList_end, compare_lt_a_star_node);
00138 else
00139 for(; locNbAdded > 0; --locNbAdded)
00140 std::push_heap(openList_begin, openList_end - (locNbAdded - 1), compare_lt_a_star_node);
00141 }
00142
00143 paths::route a_star_search(gamemap::location const &src, gamemap::location const &dst,
00144 double stop_at, cost_calculator const *costCalculator, const size_t parWidth,
00145 const size_t parHeight, std::set<gamemap::location> const *teleports)
00146 {
00147
00148 assert(src.valid(parWidth, parHeight));
00149 assert(dst.valid(parWidth, parHeight));
00150 assert(costCalculator != NULL);
00151 assert(stop_at <= costCalculator->getNoPathValue());
00152
00153 static a_star_world aStarGameWorld;
00154
00155 vector_a_star_node openList;
00156 vector_location vectLocation;
00157 paths::route locRoute;
00158 size_t locNbTeleport;
00159 a_star_node *locDestNode = NULL;
00160 a_star_node *locCurNode = NULL;
00161
00162 DBG_PF << "A* search: " << src << " -> " << dst << '\n';
00163
00164 if (costCalculator->cost(src,dst, 0) >= stop_at) {
00165 LOG_PF << "aborted A* search because Start or Dest is invalid\n";
00166 locRoute.move_left = int(costCalculator->getNoPathValue());
00167 return locRoute;
00168 }
00169
00170 a_star_init(src, dst, openList, aStarGameWorld, parWidth, parHeight, vectLocation, teleports, locNbTeleport);
00171
00172 bool routeSolved = false;
00173 while (!routeSolved && !openList.empty())
00174 {
00175 locCurNode = openList.front();
00176 assert(locCurNode != NULL);
00177
00178
00179 if (locCurNode->loc == dst)
00180 {
00181 routeSolved = true;
00182 } else {
00183 std::pop_heap(openList.begin(), openList.end(), compare_lt_a_star_node);
00184 openList.pop_back();
00185
00186 assert(locCurNode->isInCloseList == false);
00187 locCurNode->isInCloseList = true;
00188
00189 a_star_explore_neighbours(dst, stop_at, costCalculator, parWidth, parHeight,
00190 teleports, vectLocation, openList, aStarGameWorld, locCurNode, locNbTeleport);
00191 }
00192 }
00193 if(routeSolved) {
00194 locDestNode = locCurNode;
00195
00196 DBG_PF << "found solution; calculating it...\n";
00197 while (locCurNode != NULL)
00198 {
00199 locRoute.steps.push_back(locCurNode->loc);
00200 locCurNode = locCurNode->nodeParent;
00201 }
00202 std::reverse(locRoute.steps.begin(), locRoute.steps.end());
00203 locRoute.move_left = int(locDestNode->g);
00204
00205 assert(locRoute.steps.front() == src);
00206 assert(locRoute.steps.back() == dst);
00207
00208 DBG_PF << "exiting a* search (solved)\n";
00209 } else {
00210
00211 LOG_PF << "aborted a* search\n";
00212 locRoute.move_left = int(costCalculator->getNoPathValue());
00213 }
00214 openList.clear();
00215 aStarGameWorld.clear();
00216 return locRoute;
00217 }
00218
00219 static void get_tiles_radius_internal(const gamemap::location& a, size_t radius,
00220 std::set<gamemap::location>& res, std::map<gamemap::location,int>& visited)
00221 {
00222 visited[a] = radius;
00223 res.insert(a);
00224
00225 if(radius == 0) {
00226 return;
00227 }
00228
00229 gamemap::location adj[6];
00230 get_adjacent_tiles(a,adj);
00231 for(size_t i = 0; i != 6; ++i) {
00232 if(visited.count(adj[i]) == 0 || visited[adj[i]] < int(radius)-1) {
00233 get_tiles_radius_internal(adj[i],radius-1,res,visited);
00234 }
00235 }
00236 }
00237
00238 void get_tiles_radius(const gamemap::location& a, size_t radius,
00239 std::set<gamemap::location>& res)
00240 {
00241 std::map<gamemap::location,int> visited;
00242 get_tiles_radius_internal(a,radius,res,visited);
00243 }
00244
00245 void get_tiles_radius(gamemap const &map, std::vector<gamemap::location> const &locs,
00246 size_t radius, std::set<gamemap::location> &res, xy_pred *pred)
00247 {
00248 typedef std::set<gamemap::location> location_set;
00249 location_set not_visited(locs.begin(), locs.end()), must_visit, filtered_out;
00250 ++radius;
00251
00252 for(;;) {
00253 location_set::const_iterator it = not_visited.begin(), it_end = not_visited.end();
00254 std::copy(it,it_end,std::inserter(res,res.end()));
00255 for(; it != it_end; ++it) {
00256 gamemap::location adj[6];
00257 get_adjacent_tiles(*it, adj);
00258 for(size_t i = 0; i != 6; ++i) {
00259 gamemap::location const &loc = adj[i];
00260 if(map.on_board(loc) && !res.count(loc) && !filtered_out.count(loc)) {
00261 if(!pred || (*pred)(loc)) {
00262 must_visit.insert(loc);
00263 } else {
00264 filtered_out.insert(loc);
00265 }
00266 }
00267 }
00268 }
00269
00270 if(--radius == 0 || must_visit.empty()) {
00271 break;
00272 }
00273
00274 not_visited.swap(must_visit);
00275 must_visit.clear();
00276 }
00277 }
00278