astarsearch.cpp

Go to the documentation of this file.
00001 /* $Id: astarsearch.cpp 24925 2008-03-21 04:54:52Z alink $ */
00002 /*
00003 Copyright (C) 2003 by David White <dave@whitevine.net>
00004 Copyright (C) 2005 - 2008 by Guillaume Melquiond <guillaume.melquiond@gmail.com>
00005 Part of the Battle for Wesnoth Project http://www.wesnoth.org/
00006 
00007 This program is free software; you can redistribute it and/or modify
00008 it under the terms of the GNU General Public License version 2
00009 or at your option any later version.
00010 This program is distributed in the hope that it will be useful,
00011 but WITHOUT ANY WARRANTY.
00012 
00013 See the COPYING file for more details.
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 // heaps give the biggest element for free, so we want the biggest element to
00035 // have the smallest cost
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     //----------------- PRE_CONDITIONS ------------------
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         //if we have found a solution
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         //route not solved
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 

Generated by doxygen 1.5.5 on 23 May 2008 for The Battle for Wesnoth
Gna! | Forum | Wiki | CIA | devdocs