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(TreeNode* root)
{
if (!root)
{
return;
}
typedef std::queue<TreeNode*> NodeQueue;
NodeQueue nqueue;
nqueue.push(root);
while (!nqueue.empty())
{
NodeQueue::size_type qsize = nqueue.size();
for (NodeQueue::size_type i = 0; i < qsize; ++i)
{
TreeNode* cnode = nqueue.front();
nqueue.pop();
std::cout<<cnode->val<<" ";
if (cnode->left)
{
//std::cout<<"left child: "<<cnode->left->val<<"\n";
nqueue.push(cnode->left);
}
if (cnode->right)
{
//std::cout<<"right child: "<<cnode->right->val<<"\n";
nqueue.push(cnode->right);
}
}
std::cout<<"\n";
}
}
void Callback::constructNonZeroComponents( Graph const & g
, GraphVariables const & vars
, IloNumArray x_values
, NodeSetVector & nonZeroNodesComponents
) const {
using NodeQueue = std::queue< Graph::Node >;
NodeQueue queue;
// -3 queued
// -2 unvisited
// -1 visited and zero
// >= 0 visited and non-zero
auto compMap = g.createNodeMap< int >( -2 );
int compIdx = 0;
for ( Graph::Node n : g.nodes() ) {
if ( compMap[n] == -2 && intIsNonZero( x_values[vars.nodeToIndex[n]] ) ) {
queue.push( n );
compMap[n] = -3;
nonZeroNodesComponents.emplace_back();
// perform bfs
while ( !queue.empty() ) {
Graph::Node n = queue.front();
assert( compMap[n] == -3 );
queue.pop();
compMap[n] = compIdx;
nonZeroNodesComponents.back().insert( n );
for ( Graph::Edge e : g.incEdges( n ) ) {
Graph::Node nn = g.oppositeNode( n, e );
if ( compMap[nn] == -2 ) {
if ( intIsNonZero( x_values[vars.nodeToIndex[nn]] ) ) {
queue.push( nn );
compMap[nn] = -3;
} else {
compMap[nn] = -1;
}
}
}
}
++compIdx;
}
}
}
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);
}
}