rewrite a* search
- removing the pooled a_star_nodes and making a few optimizations - runs in as little as 40% of the time of the old implementation - remove astarnode.hpp and astarnode.cpp
This commit is contained in:
Chris Hopman
parent
7532730389
commit
336121d42b
6 changed files with 128 additions and 422 deletions
|
|
@ -138,7 +138,6 @@ add_library(wesnoth-core STATIC EXCLUDE_FROM_ALL ${libwesnoth-core_STAT_SRC})
|
|||
# rename libwesnoth.a to libwesnoth-game.a to have clearer targets
|
||||
|
||||
SET(libwesnoth-game_STAT_SRC
|
||||
astarnode.cpp
|
||||
astarsearch.cpp
|
||||
builder.cpp
|
||||
cavegen.cpp
|
||||
|
|
|
|||
|
|
@ -351,7 +351,6 @@ libwesnoth_core_a_SOURCES = \
|
|||
serialization/tokenizer.cpp
|
||||
|
||||
libwesnoth_a_SOURCES = \
|
||||
astarnode.cpp \
|
||||
astarsearch.cpp \
|
||||
builder.cpp \
|
||||
cavegen.cpp \
|
||||
|
|
|
|||
|
|
@ -61,7 +61,6 @@ libwesnoth_core_sources.extend([
|
|||
libwesnoth_core = env.Library("wesnoth_core", libwesnoth_core_sources)
|
||||
|
||||
libwesnoth_sources = Split("""
|
||||
astarnode.cpp
|
||||
astarsearch.cpp
|
||||
builder.cpp
|
||||
cavegen.cpp
|
||||
|
|
|
|||
|
|
@ -1,166 +0,0 @@
|
|||
/* $Id$ */
|
||||
/*
|
||||
Copyright (C) 2003 by David White <dave@whitevine.net>
|
||||
2005 - 2009 by Guillaume Melquiond <guillaume.melquiond@gmail.com>
|
||||
Part of the Battle for Wesnoth Project http://www.wesnoth.org/
|
||||
|
||||
This program is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License version 2
|
||||
or at your option any later version.
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY.
|
||||
|
||||
See the COPYING file for more details.
|
||||
*/
|
||||
|
||||
#include "global.hpp"
|
||||
#include "astarnode.hpp"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
void a_star_node::initNode(map_location const &pos, map_location const &dst,
|
||||
double cost, a_star_node *parent, std::set<map_location> const *teleports)
|
||||
{
|
||||
isInCloseList = false;
|
||||
loc = pos;
|
||||
nodeParent = parent;
|
||||
g = cost;
|
||||
h = heuristic(pos, dst);
|
||||
//if there are teleport locations, correct the heuristic to take them into account
|
||||
if (teleports != NULL) {
|
||||
double srch = h, dsth = h;
|
||||
std::set<map_location>::const_iterator i;
|
||||
for(i = teleports->begin(); i != teleports->end(); ++i) {
|
||||
const double new_srch = heuristic(pos, *i);
|
||||
const double new_dsth = heuristic(*i, dst);
|
||||
if(new_srch < srch) {
|
||||
srch = new_srch;
|
||||
}
|
||||
if(new_dsth < dsth) {
|
||||
dsth = new_dsth;
|
||||
}
|
||||
}
|
||||
if(srch + dsth + 1.0 < h) {
|
||||
h = srch + dsth + 1.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
class a_star_world::poss_a_star_node
|
||||
{
|
||||
private:
|
||||
typedef std::vector<a_star_node*> vect_page_a_star_node;
|
||||
vect_page_a_star_node vectPageAStarNode_;
|
||||
size_t nbElemByPage_;
|
||||
size_t capacity_;
|
||||
size_t curIndex_;
|
||||
void addPage();
|
||||
|
||||
public:
|
||||
poss_a_star_node();
|
||||
~poss_a_star_node();
|
||||
a_star_node *getAStarNode();
|
||||
void clear();
|
||||
};
|
||||
|
||||
void a_star_world::poss_a_star_node::addPage()
|
||||
{
|
||||
vectPageAStarNode_.push_back(new a_star_node[nbElemByPage_]);
|
||||
capacity_ += nbElemByPage_;
|
||||
}
|
||||
|
||||
a_star_world::poss_a_star_node::poss_a_star_node() :
|
||||
vectPageAStarNode_(),
|
||||
nbElemByPage_(size_t((4096 - 24) / sizeof(a_star_node))),
|
||||
capacity_(0),
|
||||
curIndex_(0)
|
||||
{
|
||||
assert(nbElemByPage_ > 0);
|
||||
addPage();
|
||||
}
|
||||
|
||||
a_star_world::poss_a_star_node::~poss_a_star_node()
|
||||
{
|
||||
for(vect_page_a_star_node::iterator iter = vectPageAStarNode_.begin(),
|
||||
iend = vectPageAStarNode_.end();
|
||||
iter != iend; ++iter)
|
||||
delete[] *iter;
|
||||
}
|
||||
|
||||
a_star_node *a_star_world::poss_a_star_node::getAStarNode()
|
||||
{
|
||||
//----------------- PRE_CONDITIONS ------------------
|
||||
assert(capacity_ > 0);
|
||||
assert(curIndex_ <= capacity_);
|
||||
//---------------------------------------------------
|
||||
if (curIndex_ == capacity_)
|
||||
addPage();
|
||||
size_t i = curIndex_++;
|
||||
return &vectPageAStarNode_[i / nbElemByPage_][i % nbElemByPage_];
|
||||
}
|
||||
|
||||
void a_star_world::poss_a_star_node::clear()
|
||||
{
|
||||
if (capacity_ > nbElemByPage_) {
|
||||
for(vect_page_a_star_node::iterator iter = vectPageAStarNode_.begin() + 1,
|
||||
iend = vectPageAStarNode_.end();
|
||||
iter != iend; ++iter)
|
||||
delete[] *iter;
|
||||
vectPageAStarNode_.resize(1);
|
||||
capacity_ = nbElemByPage_;
|
||||
}
|
||||
curIndex_ = 0;
|
||||
//----------------- POST_CONDITIONS -----------------
|
||||
assert(capacity_ == nbElemByPage_);
|
||||
assert(vectPageAStarNode_.size() == 1);
|
||||
//---------------------------------------------------
|
||||
}
|
||||
|
||||
a_star_world::a_star_world() :
|
||||
pool_(new poss_a_star_node),
|
||||
vectAStarNode_(),
|
||||
width_(0),
|
||||
nbNode_(0)
|
||||
{
|
||||
}
|
||||
|
||||
a_star_world::~a_star_world()
|
||||
{
|
||||
delete pool_;
|
||||
}
|
||||
|
||||
void a_star_world::resize_IFN(size_t parWidth, size_t parHeight)
|
||||
{
|
||||
//----------------- PRE_CONDITIONS ------------------
|
||||
assert(nbNode_ == 0);
|
||||
//---------------------------------------------------
|
||||
width_ = parWidth;
|
||||
size_t sz = parWidth * parHeight;
|
||||
if (vectAStarNode_.size() == sz)
|
||||
return;
|
||||
vectAStarNode_.reserve(sz);
|
||||
vectAStarNode_.resize(sz);
|
||||
}
|
||||
|
||||
void a_star_world::clear()
|
||||
{
|
||||
a_star_node *locNode = NULL;
|
||||
std::fill(vectAStarNode_.begin(), vectAStarNode_.end(), locNode);
|
||||
nbNode_ = 0;
|
||||
pool_->clear();
|
||||
}
|
||||
|
||||
a_star_node *a_star_world::getNodeFromLocation(map_location const &loc, bool &isCreated)
|
||||
{
|
||||
//----------------- PRE_CONDITIONS ------------------
|
||||
assert(loc.valid());
|
||||
assert(loc.x + loc.y * width_ < vectAStarNode_.size());
|
||||
//---------------------------------------------------
|
||||
|
||||
a_star_node *&node = vectAStarNode_[loc.x + loc.y * width_];
|
||||
if ((isCreated = (node == NULL))) {
|
||||
node = pool_->getAStarNode();
|
||||
++nbNode_;
|
||||
}
|
||||
return node;
|
||||
}
|
||||
|
|
@ -1,84 +0,0 @@
|
|||
/* $Id$ */
|
||||
/*
|
||||
Copyright (C) 2003 by David White <dave@whitevine.net>
|
||||
2005 - 2009 by Guillaume Melquiond <guillaume.melquiond@gmail.com>
|
||||
Part of the Battle for Wesnoth Project http://www.wesnoth.org/
|
||||
|
||||
This program is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License version 2
|
||||
or at your option any later version.
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY.
|
||||
|
||||
See the COPYING file for more details.
|
||||
*/
|
||||
|
||||
#ifndef ASTARNODE_H_INCLUDED
|
||||
#define ASTARNODE_H_INCLUDED
|
||||
|
||||
#include "pathutils.hpp"
|
||||
#include <set>
|
||||
|
||||
struct a_star_node
|
||||
{
|
||||
public:
|
||||
|
||||
a_star_node() :
|
||||
g(0.0),
|
||||
h(0.0),
|
||||
loc(),
|
||||
nodeParent(0),
|
||||
isInCloseList(false)
|
||||
{
|
||||
}
|
||||
|
||||
double g, h; // g: already traveled time, h: estimated time still to travel
|
||||
map_location loc;
|
||||
a_star_node* nodeParent;
|
||||
bool isInCloseList;
|
||||
|
||||
void initNode(map_location const &pos, map_location const &dst,
|
||||
double cost, a_star_node *parent, std::set<map_location> const *teleports);
|
||||
|
||||
inline double heuristic(const map_location& src, const map_location& dst)
|
||||
{
|
||||
// We will mainly use the distances in hexes
|
||||
// but we substract a tiny bonus for shorter Euclidean distance
|
||||
// based on how the path looks on the screen.
|
||||
// We must substract (and not add) to keep the heuristic 'admissible'.
|
||||
|
||||
// 0.75 comes frome the horizontal hex imbrication
|
||||
double xdiff = (src.x - dst.x) * 0.75;
|
||||
// we must add 0.5 to the y coordinate when x is odd
|
||||
double ydiff = (src.y - dst.y) + ((src.x & 1) - (dst.x & 1)) * 0.5;
|
||||
|
||||
// we assume a map with a maximum diagonal of 300 (bigger than a 200x200)
|
||||
// and we divide by 90000 * 10000 to avoid interfering with the defense subcost
|
||||
// (see shortest_path_calculator::cost)
|
||||
return distance_between(src, dst) -
|
||||
(90000.0 - ( xdiff*xdiff + ydiff*ydiff)) / 900000000.0;
|
||||
|
||||
// TODO: move the heuristic function into the cost_calculator
|
||||
// so we can use case-specific heuristic
|
||||
// and clean the definition of these numbers
|
||||
}
|
||||
};
|
||||
|
||||
class a_star_world
|
||||
{
|
||||
class poss_a_star_node;
|
||||
poss_a_star_node *pool_;
|
||||
typedef std::vector<a_star_node*> vect_a_star_node;
|
||||
vect_a_star_node vectAStarNode_;
|
||||
size_t width_, nbNode_;
|
||||
|
||||
public:
|
||||
void resize_IFN(size_t parWidth, size_t parHeight);
|
||||
void clear();
|
||||
a_star_node* getNodeFromLocation(map_location const &loc, bool& isCreated);
|
||||
a_star_world();
|
||||
~a_star_world();
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -15,202 +15,161 @@
|
|||
|
||||
#include "global.hpp"
|
||||
|
||||
#include "astarnode.hpp"
|
||||
#include "log.hpp"
|
||||
#include "map.hpp"
|
||||
#include "pathfind.hpp"
|
||||
#include "foreach.hpp"
|
||||
|
||||
#include <queue>
|
||||
#include <map>
|
||||
|
||||
|
||||
#define LOG_PF LOG_STREAM(info, engine)
|
||||
#define DBG_PF LOG_STREAM(debug, engine)
|
||||
#define ERR_PF LOG_STREAM(err, engine)
|
||||
|
||||
typedef std::vector<map_location> vector_location;
|
||||
typedef std::vector<a_star_node*> vector_a_star_node;
|
||||
typedef std::set<map_location> set_location;
|
||||
double heuristic(const map_location& src, const map_location& dst)
|
||||
{
|
||||
// We will mainly use the distances in hexes
|
||||
// but we substract a tiny bonus for shorter Euclidean distance
|
||||
// based on how the path looks on the screen.
|
||||
// We must substract (and not add) to keep the heuristic 'admissible'.
|
||||
|
||||
// heaps give the biggest element for free, so we want the biggest element to
|
||||
// have the smallest cost
|
||||
static bool compare_lt_a_star_node(const a_star_node* node1, const a_star_node* node2) {
|
||||
return node1->g + node1->h > node2->g + node2->h;
|
||||
// 0.75 comes frome the horizontal hex imbrication
|
||||
double xdiff = (src.x - dst.x) * 0.75;
|
||||
// we must add 0.5 to the y coordinate when x is odd
|
||||
double ydiff = (src.y - dst.y) + ((src.x & 1) - (dst.x & 1)) * 0.5;
|
||||
|
||||
// we assume a map with a maximum diagonal of 300 (bigger than a 200x200)
|
||||
// and we divide by 90000 * 10000 to avoid interfering with the defense subcost
|
||||
// (see shortest_path_calculator::cost)
|
||||
return distance_between(src, dst) -
|
||||
(90000.0 - ( xdiff*xdiff + ydiff*ydiff)) / 900000000.0;
|
||||
|
||||
// TODO: move the heuristic function into the cost_calculator
|
||||
// so we can use case-specific heuristic
|
||||
// and clean the definition of these numbers
|
||||
}
|
||||
|
||||
static void a_star_init(map_location const &src, map_location const &dst,
|
||||
vector_a_star_node &openList, a_star_world &aStarGameWorld,
|
||||
const size_t parWidth, const size_t parHeight,
|
||||
vector_location &vectLocation, std::set<map_location> const *teleports,
|
||||
size_t &parNbTeleport)
|
||||
{
|
||||
bool locIsCreated;
|
||||
|
||||
aStarGameWorld.resize_IFN(parWidth, parHeight);
|
||||
a_star_node *locStartNode = aStarGameWorld.getNodeFromLocation(src, locIsCreated);
|
||||
assert(locIsCreated);
|
||||
locStartNode->initNode(src, dst, 0.0, NULL, teleports);
|
||||
|
||||
const size_t locValueH = size_t(locStartNode->h);
|
||||
size_t locAllocSize;
|
||||
|
||||
if (locValueH < 16)
|
||||
locAllocSize = 16;
|
||||
else if (locValueH > 128)
|
||||
locAllocSize = 128;
|
||||
else
|
||||
locAllocSize = locValueH;
|
||||
openList.reserve(locAllocSize);
|
||||
openList.push_back(locStartNode);
|
||||
|
||||
if (teleports != NULL)
|
||||
parNbTeleport = teleports->size();
|
||||
else
|
||||
parNbTeleport = 0;
|
||||
|
||||
vectLocation.reserve(parNbTeleport + 6);
|
||||
vectLocation.resize(parNbTeleport + 6);
|
||||
|
||||
if (parNbTeleport > 0)
|
||||
std::copy(teleports->begin(), teleports->end(), &vectLocation[6]);
|
||||
}
|
||||
|
||||
static void a_star_explore_neighbours(map_location const &dst, const double stop_at,
|
||||
cost_calculator const *costCalculator,
|
||||
const size_t parWidth, const size_t parHeight,
|
||||
std::set<map_location> const *teleports,
|
||||
vector_location &vectLocation, vector_a_star_node &openList,
|
||||
a_star_world &aStarGameWorld,
|
||||
a_star_node *parCurNode, const size_t parNbTeleport)
|
||||
{
|
||||
typedef std::pair<vector_a_star_node::iterator, vector_a_star_node::iterator> pair_node_iter;
|
||||
|
||||
a_star_node *locNextNode;
|
||||
double locCost;
|
||||
pair_node_iter locPlace;
|
||||
size_t locSize;
|
||||
bool locIsCreated;
|
||||
const double locCostFather = parCurNode->g;
|
||||
|
||||
get_adjacent_tiles(parCurNode->loc, &vectLocation[0]);
|
||||
|
||||
if (parNbTeleport > 0 && teleports->count(parCurNode->loc) > 0)
|
||||
locSize = parNbTeleport + 6;
|
||||
else
|
||||
locSize = 6;
|
||||
|
||||
bool broken_heap = false;
|
||||
int locNbAdded = 0;
|
||||
|
||||
for (size_t i = 0; i != locSize; ++i)
|
||||
{
|
||||
const map_location& locLocation = vectLocation[i];
|
||||
|
||||
if (locLocation.valid(int(parWidth), int(parHeight)) == false)
|
||||
continue;
|
||||
locNextNode = aStarGameWorld.getNodeFromLocation(locLocation, locIsCreated);
|
||||
locCost = locCostFather + costCalculator->cost(parCurNode->loc,locLocation, locCostFather);
|
||||
if (locIsCreated) {
|
||||
locNextNode->initNode(locLocation, dst, locCost, parCurNode, teleports);
|
||||
if (locNextNode->g + locNextNode->h < stop_at) {
|
||||
openList.push_back(locNextNode);
|
||||
++locNbAdded;
|
||||
} else
|
||||
locNextNode->isInCloseList = true;
|
||||
|
||||
} else if (locCost < locNextNode->g) {
|
||||
|
||||
if (locNextNode->isInCloseList) {
|
||||
locNextNode->isInCloseList = false;
|
||||
openList.push_back(locNextNode);
|
||||
++locNbAdded;
|
||||
} else
|
||||
broken_heap = true;
|
||||
|
||||
locNextNode->g = locCost;
|
||||
locNextNode->nodeParent = parCurNode;
|
||||
}
|
||||
struct node {
|
||||
double g, h, t;
|
||||
map_location curr, prev;
|
||||
bool in;
|
||||
node() : g(1e25), t(1e25), in(false) { }
|
||||
node(double s, const map_location& c, const map_location& p, const map_location& dst, bool i) : g(s),
|
||||
h(heuristic(c, dst)), t(g + h), curr(c), prev(p), in(i) { }
|
||||
bool operator<(const node& o) const {
|
||||
return t < o.t;
|
||||
}
|
||||
bool operator>(const node& o) const {
|
||||
return o < *this;
|
||||
}
|
||||
};
|
||||
|
||||
vector_a_star_node::iterator openList_begin = openList.begin(),
|
||||
openList_end = openList.end();
|
||||
if (broken_heap)
|
||||
std::make_heap(openList_begin, openList_end, compare_lt_a_star_node);
|
||||
else
|
||||
for(; locNbAdded > 0; --locNbAdded)
|
||||
std::push_heap(openList_begin, openList_end - (locNbAdded - 1), compare_lt_a_star_node);
|
||||
}
|
||||
struct comp {
|
||||
const std::vector<node>& nodes;
|
||||
comp(const std::vector<node>& n) : nodes(n) { }
|
||||
bool operator()(int a, int b) {
|
||||
return nodes[b] < nodes[a];
|
||||
}
|
||||
};
|
||||
|
||||
paths::route a_star_search(map_location const &src, map_location const &dst,
|
||||
double stop_at, cost_calculator const *costCalculator, const size_t parWidth,
|
||||
const size_t parHeight, std::set<map_location> const *teleports)
|
||||
{
|
||||
struct indexer {
|
||||
size_t h, w;
|
||||
indexer(size_t a, size_t b) : h(a), w(b) { }
|
||||
size_t operator()(const map_location& loc) {
|
||||
return loc.y * h + loc.x;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
paths::route a_star_search(const map_location& src, const map_location& dst,
|
||||
double stop_at, const cost_calculator *calc, const size_t width,
|
||||
const size_t height, const std::set<map_location>* teleports_ptr) {
|
||||
//----------------- PRE_CONDITIONS ------------------
|
||||
assert(src.valid(parWidth, parHeight));
|
||||
assert(dst.valid(parWidth, parHeight));
|
||||
assert(costCalculator != NULL);
|
||||
assert(stop_at <= costCalculator->getNoPathValue());
|
||||
assert(src.valid(width, height));
|
||||
assert(dst.valid(width, height));
|
||||
assert(calc != NULL);
|
||||
assert(stop_at <= calc->getNoPathValue());
|
||||
//---------------------------------------------------
|
||||
static a_star_world aStarGameWorld;
|
||||
|
||||
vector_a_star_node openList;
|
||||
vector_location vectLocation;
|
||||
paths::route locRoute;
|
||||
size_t locNbTeleport;
|
||||
a_star_node *locDestNode = NULL;
|
||||
a_star_node *locCurNode = NULL;
|
||||
|
||||
|
||||
|
||||
DBG_PF << "A* search: " << src << " -> " << dst << '\n';
|
||||
|
||||
if (costCalculator->cost(src,dst, 0) >= stop_at) {
|
||||
if (calc->cost(src,dst, 0) >= stop_at) {
|
||||
LOG_PF << "aborted A* search because Start or Dest is invalid\n";
|
||||
locRoute.move_left = int(costCalculator->getNoPathValue());
|
||||
paths::route locRoute;
|
||||
locRoute.move_left = int(calc->getNoPathValue());
|
||||
return locRoute;
|
||||
}
|
||||
|
||||
a_star_init(src, dst, openList, aStarGameWorld, parWidth, parHeight, vectLocation, teleports, locNbTeleport);
|
||||
|
||||
bool routeSolved = false;
|
||||
while (!routeSolved && !openList.empty())
|
||||
{
|
||||
locCurNode = openList.front();
|
||||
assert(locCurNode != NULL);
|
||||
|
||||
//if we have found a solution
|
||||
if (locCurNode->loc == dst)
|
||||
{
|
||||
routeSolved = true;
|
||||
} else {
|
||||
std::pop_heap(openList.begin(), openList.end(), compare_lt_a_star_node);
|
||||
openList.pop_back();
|
||||
|
||||
assert(locCurNode->isInCloseList == false);
|
||||
locCurNode->isInCloseList = true;
|
||||
|
||||
a_star_explore_neighbours(dst, stop_at, costCalculator, parWidth, parHeight,
|
||||
teleports, vectLocation, openList, aStarGameWorld, locCurNode, locNbTeleport);
|
||||
const std::set<map_location>& teleports = teleports_ptr ? *teleports_ptr : std::set<map_location>();
|
||||
|
||||
|
||||
std::vector<map_location> locs(6 + teleports.size());
|
||||
std::copy(teleports.begin(), teleports.end(), locs.begin() + 6);
|
||||
|
||||
static std::vector<node> nodes;
|
||||
nodes.clear();
|
||||
nodes.resize(width * height);
|
||||
|
||||
indexer index(width, height);
|
||||
comp node_comp(nodes);
|
||||
nodes[index(dst)].g = stop_at;
|
||||
nodes[index(src)] = node(0, src, map_location::null_location, dst, true);
|
||||
|
||||
std::vector<int> pq;
|
||||
pq.push_back(index(src));
|
||||
std::push_heap(pq.begin(), pq.end(), node_comp);
|
||||
int c = 0;
|
||||
while (!pq.empty()) {
|
||||
node& n = nodes[pq.front()];
|
||||
n.in = false;
|
||||
std::pop_heap(pq.begin(), pq.end(), node_comp);
|
||||
pq.pop_back();
|
||||
|
||||
if (n.t >= nodes[index(dst)].g) break;
|
||||
if (n.curr == dst) break;
|
||||
|
||||
get_adjacent_tiles(n.curr, &locs[0]);
|
||||
|
||||
for (int i = teleports.count(n.curr) ? locs.size() : 6; i-- > 0;) {
|
||||
if (!locs[i].valid(width, height)) continue;
|
||||
double thresh = nodes[index(locs[i])].g;
|
||||
if (n.g >= thresh) continue;
|
||||
double cost = n.g + calc->cost(n.curr, locs[i], n.g);
|
||||
if (cost >= thresh) continue;
|
||||
|
||||
node& next = nodes[index(locs[i])];
|
||||
bool in_list = next.in;
|
||||
next = node(cost, locs[i], n.curr, dst, true);
|
||||
if (in_list) {
|
||||
std::push_heap(pq.begin(), std::find(pq.begin(), pq.end(), index(locs[i])) + 1, node_comp);
|
||||
} else {
|
||||
pq.push_back(index(locs[i]));
|
||||
std::push_heap(pq.begin(), pq.end(), node_comp);
|
||||
}
|
||||
}
|
||||
}
|
||||
if(routeSolved) {
|
||||
locDestNode = locCurNode;
|
||||
|
||||
|
||||
|
||||
paths::route route;
|
||||
if (nodes[index(dst)].g < stop_at) {
|
||||
DBG_PF << "found solution; calculating it...\n";
|
||||
while (locCurNode != NULL)
|
||||
{
|
||||
locRoute.steps.push_back(locCurNode->loc);
|
||||
locCurNode = locCurNode->nodeParent;
|
||||
route.move_left = nodes[index(dst)].g;
|
||||
for (node curr = nodes[index(dst)]; curr.prev != map_location::null_location; curr = nodes[index(curr.prev)]) {
|
||||
route.steps.push_back(curr.curr);
|
||||
}
|
||||
std::reverse(locRoute.steps.begin(), locRoute.steps.end());
|
||||
locRoute.move_left = int(locDestNode->g);
|
||||
|
||||
assert(locRoute.steps.front() == src);
|
||||
assert(locRoute.steps.back() == dst);
|
||||
|
||||
DBG_PF << "exiting a* search (solved)\n";
|
||||
} else {
|
||||
//route not solved
|
||||
LOG_PF << "aborted a* search\n";
|
||||
locRoute.move_left = int(costCalculator->getNoPathValue());
|
||||
route.steps.push_back(src);
|
||||
std::reverse(route.steps.begin(), route.steps.end());
|
||||
} else {
|
||||
LOG_PF << "aborted a* search " << c << "\n";
|
||||
route.move_left = (int)calc->getNoPathValue();
|
||||
}
|
||||
openList.clear();
|
||||
aStarGameWorld.clear();
|
||||
return locRoute;
|
||||
|
||||
return route;
|
||||
|
||||
}
|
||||
|
||||
static void get_tiles_radius_internal(const map_location& a, size_t radius,
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue