+struct SPInfoComp{
+ bool operator()(const SPInfo* v1, const SPInfo* v2){
+ return v1->cost > v2->cost;
+ }
+};
+
+std::list<std::string> Bot::shortestPath(const std::string& from, PlayerType type, const SPGoal& goal){
+
+ //cerr << "New shortest path from: " << from << endl;
+
+ priority_queue<SPInfo*, vector<SPInfo* >, SPInfoComp > pq;
+ hash_map<string,SPInfo> nodes;
+
+ SPInfo* node = &nodes[from];
+ node->name = from;
+ node->settled = false;
+ node->parent = 0;
+ node->cost = 0;
+ pq.push(node);
+
+ int g = 0;
+ while(!pq.empty()){
+ node = pq.top();
+ pq.pop();
+ //cerr << "Vector with size: " << w.size() << endl;
+ //cerr << "Looking at: " << node->name << endl;
+ //copy(w.begin(), w.end(), ostream_iterator<string>(cerr, " : "));
+
+ g = goal(node);
+ if (g < 0)
+ break;
+ else if (g){
+ list<string> l;
+ front_insert_iterator<list<string> > ii(l);
+ do{
+ *ii++ = node->name;
+ node = node->parent;
+ }while (node != 0);
+ return l;
+ }
+ node->settled = true;
+
+ Intersection& inter = _intersections[node->name];
+ for (hash_map<string,StreetType>::const_iterator street = inter.connections.begin();
+ street != inter.connections.end(); ++street){
+ hash_map<string,SPInfo>::iterator newNode = nodes.find(street->first);
+ bool hascar = type == cop_car;
+ if ( (hascar && (street->second == car || street->second == both)) ||
+ (!hascar && (street->second == foot || street->second == both))){
+ //cerr << "Adding street: " << street->first << endl;
+ SPInfo* n = 0;
+ if (newNode == nodes.end()){
+ n = &nodes[street->first];
+ n->name = street->first;
+ n->settled = false;
+ n->parent = 0;
+ n->cost = 10000;
+ }else if (newNode->second.settled)
+ continue;
+ else
+ n = &newNode->second;
+
+ if (n->cost > node->cost + 1){
+ n->cost = node->cost + 1;
+ n->parent = node;
+ pq.push(n);
+ }
+ }
+
+ }
+
+ }
+
+ return list<string>();
+}