void PathFinding::algo(NodeQueue &queue, PathFinding::PathMode mode) const
{
while (!queue.empty())
{
Node *currentNode = queue.top();
queue.pop();
if (currentNode == _end)
break;
Link *currentLink = currentNode->getPrevLink();
std::list<Link> &links = currentNode->getLinks();
for (std::list<Link>::iterator it = links.begin(); it != links.end(); ++it)
{
Link &curLink = *it;
double time;
if (mode == PathFinding::FASTEST)
time = currentNode->getWeigth() + curLink.distance / curLink.road->getSpeed();
else
time = currentNode->getWeigth() + curLink.distance;
if (curLink.node->getWeigth() < 0 || curLink.node->getWeigth() > time)
{
curLink.node->setWeigth(time);
curLink.node->setPrevLink(&curLink);
curLink.prevLink = currentLink;
queue.push(curLink.node);
}
}
}
}
void bfs(Node **array, int sx, int sy, int maxx, int maxy){
NodeQueue nodelist;
nodelist.enqueue(&array[sx][sy]);
array[sx][sy].if_source=1;
array[sx][sy].if_watered=1;
while (!nodelist.empty()){
Node* temp=nodelist.top();
select(temp->height, temp->x-1, temp->y, maxx, maxy, array, &nodelist);
select(temp->height, temp->x+1, temp->y, maxx, maxy, array, &nodelist);
select(temp->height, temp->x, temp->y-1, maxx, maxy, array, &nodelist);
select(temp->height, temp->x, temp->y+1, maxx, maxy, array, &nodelist);
nodelist.dequeue();
}
}
bool Path::find(const Grid &g, const Tile &start, const Tile &goal) {
Format f = "Finding path from cell {} to {}";
info(f.bind(start, goal));
Timer t;
t.start();
NodePool p;
Node *n = new (p) Node(start, 0.0f);
const Heuristic &h = m_heuristic;
n->remaining = h.estimate(n->tile, goal);
n->total = n->cost + n->remaining;
NodeQueue q;
q.push(n);
NodeSet frontier;
frontier.insert(n);
NodeSet interior;
Node *b;
Nodes v;
while (!q.empty()) {
n = q.top();
q.pop();
if (n->was_replaced()) {
p.discard(n);
continue;
}
if (n->tile == goal) {
trace(n, tiles);
report_stats(t, p);
return true;
}
frontier.erase(n);
interior.insert(n);
neighbors(n, v, p, g);
foreach (Node *a, v) {
a->cost += n->cost;
if ((b = lookup(a, frontier))) {
if (b->cost <= a->cost) {
p.discard(a);
continue;
}
frontier.erase(b);
b->mark_as_replaced();
}
if ((b = lookup(a, interior))) {
if (b->cost <= a->cost) {
p.discard(a);
continue;
}
interior.erase(b);
p.discard(b);
}
a->remaining = h.estimate(a->tile, goal);
a->total = a->cost + a->remaining;
a->next = n;
q.push(a);
frontier.insert(a);
}
}