openstreetmap — C ++ двунаправленная реализация dijkstra очень медленная

Я делаю свою собственную реализацию двунаправленного алгоритма Дейкстры. Я использую его для исследования маршрутизации в среде Openstreetmaps (OSM), поэтому я хотел создать свою собственную реализацию. График, на который я направляюсь, представляет собой двумерную карту ребер, содержащую отрезки дороги из среды OSM. Это выглядит так: graph[ource_id][target_id] = Edgeinfo,

Я пробовал его на небольшом демонстрационном графике, и он, кажется, работает нормально, однако, когда я пробую его на (небольшом) подмножестве данных OSM, вычислительное время, кажется, взорвалось, и оно даже не может найти маленькие пути. Мне было интересно, если моя реализация просто очень медленная (часы для небольших путей) и может быть выполнена намного быстрее, или я допустил ошибку, которую пропустил в своем коде.

Это основные функции моего кода:

main.cc

#include "graph.h"#include "bidij.h"
int main(int argc, char* argv[])
{
graph g;
long long source_osm_id, target_osm_id;

g.load();
dijkstra d(g.myGraph);

source_osm_id = 1;
target_osm_id = 5;

cout << "calculate route: " << endl;
d.bidirectionaldijkstra(source_osm_id, target_osm_id);

return 1;
}

graph.h

#ifndef ____graph__
#define ____graph__

#include <iostream>
#include <string>
#include <map>
#include <cmath>
using namespace std;

struct Edge
{
double distance;
};

class graph {
public:
void load();
map< long long, map <long long, Edge> > myGraph;
};

#endif

graph.cc

#include "graph.h"
void graph::load()
{
Edge e;

// edges from node 1
e.distance = 7;
this->myGraph[1][2] = e;

e.distance = 9;
this->myGraph[1][3] = e;

e.distance = 14;
this->myGraph[1][6] = e;

// edges from node 2
e.distance = 7;
this->myGraph[2][1] = e;

e.distance = 10;
this->myGraph[2][3] = e;

e.distance = 15;
this->myGraph[2][4] = e;

// edges from node 3
e.distance = 2;
this->myGraph[3][6] = e;

e.distance = 9;
this->myGraph[3][1] = e;

e.distance = 10;
this->myGraph[3][2] = e;

e.distance = 11;
this->myGraph[3][4] = e;

// edges from node 4
e.distance = 6;
this->myGraph[4][5] = e;

e.distance = 11;
this->myGraph[4][3] = e;

e.distance = 15;
this->myGraph[4][2] = e;

// edges from node 5
e.distance = 9;
this->myGraph[5][6] = e;

e.distance = 6;
this->myGraph[5][4] = e;

// edges from node 6
e.distance = 9;
this->myGraph[6][5] = e;

e.distance = 2;
this->myGraph[6][3] = e;

e.distance = 14;
this->myGraph[6][1] = e;
}

bidij.h

#ifndef ____bidij__
#define ____bidij__

#include "graph.h"#include <iostream>
#include <queue>
#include <string>
using namespace std;

struct queueitem
{
long long distance, osm_source_id, osm_target_id;
queueitem * parentnode;
queue<long long> * route;
};

class CompareQueueItems {
public:
bool operator()(queueitem *t1, queueitem *t2){
return (t1->distance > t2->distance);
}
};

class dijkstra {
public:
dijkstra(map< long long, map <long long, Edge> > graph)
{
mu = numeric_limits<double>::max();
myGraph = graph;
}
void bidirectionaldijkstra(long long, long long);
private:
double mu;
long long v,w;
map< long long, int> visited;
map <long long, double> distancetable;
map <long long, queueitem*> relaxednode;
void exploration(queueitem*, int,priority_queue< queueitem*, vector< queueitem* >, CompareQueueItems> &);
void print_route(queueitem * q, int direction);
map< long long, map <long long, Edge> > myGraph;
};

#endif

bidij.cc

#include "bidij.h"
void dijkstra::exploration(queueitem *q, int direction ,priority_queue< queueitem*, vector< queueitem* >, CompareQueueItems> &pq)
{
// all edges connected to v
map <long long, Edge> edgev;
map<long long, Edge>::const_iterator iv;
queueitem *qtemp;

edgev = myGraph[q->osm_source_id]; // edge v
// scan each edge w around v (v,w)
for(iv = edgev.begin(); iv != edgev.end();iv++)
{
if(visited[iv->first] == 0)
{
qtemp = new queueitem;
qtemp->osm_source_id = iv->first;
qtemp->parentnode = q;
qtemp->distance = q->distance + iv->second.distance;
qtemp->route = new queue<long long>(*q->route);
pq.push(qtemp);

// update shortest distance to w and add the node to a map so we can retrieve it later for printing the route
if(distancetable[iv->first] > qtemp->distance){
distancetable[iv->first] = qtemp->distance;
relaxednode[iv->first] = qtemp;
}
} else if(visited[iv->first] != direction) {
if((q->distance + iv->second.distance + distancetable[iv->first]) < mu)
{
// mu is the best path so far
mu = (q->distance + iv->second.distance + distancetable[iv->first]);

v = (direction == 1) ? q->osm_source_id : iv->first;
w = (direction == 1) ? iv->first : q->osm_source_id;
}
}
}
visited[q->osm_source_id] = direction;

}

void dijkstra::print_route(queueitem * q, int direction)
{
queueitem * qtemp = q, * new_root = NULL;

if(direction == -1)
{
while(qtemp)
{
cout << qtemp->osm_source_id << ',';
qtemp = qtemp->parentnode;
}
} else { // inverse
while (qtemp) {
queueitem* next = qtemp->parentnode;
qtemp->parentnode = new_root;
new_root = qtemp;
qtemp = next;
}
while(new_root)
{
cout << new_root->osm_source_id << ',';
new_root = new_root->parentnode;
}
}
}void dijkstra::bidirectionaldijkstra(long long source, long long target)
{
if(source == target)
{
cout << "Total distance: " << 0 << endl;
cout << source << ',' << target << endl;
return;
}
priority_queue< queueitem*, vector< queueitem* >, CompareQueueItems> frontq, reverseq;
queueitem *qs, *qt;

typedef map<long long, map <long long, Edge> >::iterator it_type;
for(it_type iterator = myGraph.begin(); iterator != myGraph.end(); iterator++)
{
// set initial distances to infinity (or close to it)
distancetable[iterator->first] = numeric_limits<double>::max();
visited[iterator->first] = 0; // unvisited
}

int direction; // direction of the search

// create queueitem from source node
qs = new queueitem;
qs->distance = 0.0f;
qs->osm_source_id = source;
qs->parentnode = NULL;
qs->route = new queue<long long>;
frontq.push(qs); // put source node on front priority queue
distancetable[source] = 0.0f;
relaxednode[source] = qs;

// create queueitem from target node
qt = new queueitem;
qt->distance = 0.0f;
qt->osm_source_id = target;
qt->parentnode = NULL;
qt->route = new queue<long long>;
reverseq.push(qt); // put target node on reverse priority queue
distancetable[target] = 0.0f;
relaxednode[target] = qt;while(!frontq.empty() && !reverseq.empty())
{
qs = frontq.top();
qt = reverseq.top();

if(qs->distance + qt->distance >= mu)
{
cout << "Total distance: " << mu << endl;
print_route(relaxednode[v],1);
print_route(relaxednode[w],-1);
return;
} else {
// explore from reverse queue
if(qt->distance < qs->distance)
{
direction = -1;
qt->route->push(qt->osm_source_id);
exploration(qt,direction,reverseq);
reverseq.pop();
} else { // explore from front queue
direction = 1;
qs->route->push(qs->osm_source_id);
exploration(qs,direction,frontq);
frontq.pop();
}
}

}
cout << "notfound";
}

РЕДАКТИРОВАТЬ: добавил полный код, чтобы прояснить ситуацию.

График: