--- /dev/null
+#include "bot.h"
+#include <queue>
+#include <string>
+#include <ext/hash_map>
+using namespace std;
+using namespace __gnu_cxx;
+
+struct SPCost{
+ inline int cost(std::string from, std::string to) const{
+ return 1;
+ }
+};
+
+template<class Goal>
+std::list<std::string> Bot::shortestPath(const std::string& from, PlayerType type, const Goal& goal, bool reverse) const{
+ return shortestPath(from, type, goal, SPCost(), reverse);
+}
+
+template<class Goal, class Cost>
+std::list<std::string> Bot::shortestPath(const std::string& from, PlayerType type, const Goal& goal, const Cost& cost,bool reverse) const{
+
+ //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;
+ bool hascar = type == cop_car;
+ ;
+ SPInfo* n = 0;
+ while(!pq.empty()){
+ node = pq.top();
+ pq.pop();
+
+ 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;
+ //cerr << "Settled " << node->name << endl;
+
+ hash_map<string,Intersection>::const_iterator intersection = _intersections.find(node->name);
+ if (intersection == _intersections.end()){ //Sanity check, should never be true..
+ cerr << "BUG: Could not find intersection: " << node->name << endl;
+ continue;
+ }
+ for (hash_map<string,StreetType>::const_iterator street = intersection->second.connections.begin();
+ street != intersection->second.connections.end(); ++street){
+ hash_map<string,SPInfo>::iterator newNode = nodes.find(street->first);
+ bool travelStreet = false;
+ if (hascar){
+ if (reverse){
+ if (street->second != foot){
+ hash_map<string,Intersection>::const_iterator newInter = _intersections.find(street->first);
+ if (newInter != _intersections.end()){
+ hash_map<string,StreetType>::const_iterator st = newInter->second.connections.find(node->name);
+ if (st != newInter->second.connections.end())
+ travelStreet = st->second != foot;
+ }
+ }else
+ travelStreet = true;
+ }else
+ travelStreet = street->second != foot;
+ }else{
+ travelStreet = street->second != car;
+ }
+ if (travelStreet){
+ //cerr << "Adding street: " << street->first << endl;
+ 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;
+
+ int c = cost.cost(node->name, n->name);
+ if (n->cost > node->cost + c){
+ n->cost = node->cost + c;
+ n->parent = node;
+ pq.push(n);
+ }
+ }
+
+ }
+
+ }
+
+ return list<string>();
+}