GISCup 2015
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Macros
giscup.hpp
Go to the documentation of this file.
1 /* Copyright 2015 Martin Werner - <martin.werner@ifi.lmu.de>
2  *
3  * This program is free software: you can redistribute it and/or modify
4  * it under the terms of the GNU General Public License as published by
5  * the Free Software Foundation, either version 3 of the License, or
6  * (at your option) any later version.
7  *
8  * This program is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
17 #ifndef GISCUP_HPP_INC
18 #define GISCUP_HPP_INC
19 
20 /*
21  * This is a header-only library containing all algortihms
22  * and mechanisms for my GISCup 2015 contribution
23  *
24  * It is kept as a header-only library in order not to have problems
25  * with different modules compiled using different compiler parameters
26  * and similar problems. Furthermore, this opens up the algorithms
27  * to quick integration in GUI and console versions
28  *
29  * Dependencies:
30  * I tried to have as few dependencies as possible. However, the following
31  * libraries are being used:
32  *
33  * shapelib-1.3
34  * used to load the dataset
35  * download from http://shapelib.maptools.org/, my version: 1.3.0
36  * unpack somewhere, ./configure, make, sudo make install...
37  * Boost 1.58.0
38  * used to model the graph and the geometry
39  * download from http://www.boost.org/users/download/
40  * unpack to your home (or somewhere else)
41  * update Makefile -I../boost_1_58 to match your location
42  * the used boost libraries need not be compiled nor installed...
43  *
44  * If you want to compile the GUI, you also need a working:
45  *
46  * - wxWidgets with OpenGL
47  * - OpenGL
48  * - dslab
49  * get from https://github.com/mwernerds/dslab
50  * contribute to it
51  *
52  * Additionally, the compiler must support modern C++ (C++11). I am
53  * compiling always with g++ -std=std++11...
54  *
55  *
56  * This source code is structured as follows
57  *
58  * Preamble: Includes and global types
59  * Section 0: Configuration and Utilities
60  * Section 1: DataHandling
61  * Section 1.1: Geometry
62  * Section 1.2: Shape Data Types
63  * Section 1.3: Data Loading
64  * Section 1.4: The Document (ShapeCollection)
65  * Section 2: Graph Search
66  * Section 2.1: Graph Types
67  * Section 2.2: Implementation and Evaluation of Searches
68  * Section 2.2.1: Reference A Search
69  * Section 2.2.2: Explicit A Search
70  * Section 2.2.3: Explicit Dijkstra
71  * Section 2.2.4: Boost Dijkstra
72  * Section 2.2.5: ALT
73  * Section 2.3: OpenGL Rendering (Optional)
74  *
75  * The actual algorithms have been put in header files in the algorithm
76  * subfolder.
77  *
78  * If you have any questions, don't hesitate to contact me at
79  * <martin.werner@ifi.lmu.de>
80  *
81  * If you have any contributions, just mail me a patch or put a pull request
82  * on GitHub. It would be great, if we can join forces to cleanly implement
83  * various other search algorithms in this setting...
84  */
85 
86 
87  /*************************************************
88  * Preamble: Includes and global types *
89  *************************************************/
90 
91 #include<iostream>
92 #include<iomanip>
93 #include<sstream>
94 #include<stdexcept>
95 #include<limits>
96 #include<cmath>
97 
98 #include<vector>
99 #include<queue>
100 #include<map>
101 
102 
103 #include <boost/pending/queue.hpp>
104 #include <boost/graph/astar_search.hpp>
105 #include <boost/graph/dijkstra_shortest_paths.hpp>
106 #include <boost/graph/adjacency_list.hpp>
107 #include <boost/graph/random.hpp>
108 #include <boost/graph/reverse_graph.hpp>
109 #include <boost/graph/graph_utility.hpp>
110 
111 #include <boost/random.hpp>
112 
113 
114 #include <boost/geometry.hpp>
115 #include <boost/geometry/geometries/point.hpp>
116 #include <boost/geometry/geometries/box.hpp>
117 #include <boost/geometry/geometries/polygon.hpp>
118 #include <boost/geometry/index/rtree.hpp>
119 #include <boost/geometry/algorithms/distance.hpp>
120 
121 #include <boost/foreach.hpp>
122 
123 #include <boost/property_tree/ptree.hpp>
124 #include <boost/property_tree/json_parser.hpp>
125 
126 #include <shapefil.h>
127 
128 namespace bg = boost::geometry;
129 namespace bgi = boost::geometry::index;
130 
131 typedef bg::model::point<double, 2, bg::cs::cartesian> point;
132 typedef bg::model::box<point> box;
133 typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon
134 typedef bg::model::linestring<point> linestring;
135 typedef std::pair<box, unsigned> value;
136 
137 using namespace std;
138 
139 
140 
141 
142  /**************************************************
143  * Section 0: Configuration and Utilities *
144  *************************************************/
145 
155 double satof(std::string s)
156 {
157  std::istringstream istr(s);
158  istr.imbue(std::locale("C"));
159  double ret;
160  istr >> ret;
161  return ret;
162 }
163 
171 double _DBFReadDoubleAttribute(DBFHandle hDBF, int id, int iField)
172 {
173  const char *s = DBFReadStringAttribute(hDBF,id,iField);
174  return satof(std::string(s));
175 }
176 
183 std::string removeExt( std::string const& filename )
184 {
185  std::string::const_reverse_iterator pivot
186  = std::find( filename.rbegin(), filename.rend(), '.' );
187  return pivot == filename.rend()
188  ? filename
189  : std::string( filename.begin(), pivot.base() - 1 );
190 }
191 
199 bool less_second (const value& lhs, const value& rhs)
200 {
201  return lhs.second<rhs.second;
202 }
203 
204 
205  /**************************************************
206  * Section 1: DataHandling *
207  *************************************************/
208 
209  /**************************************************
210  * Section 1.1: Geometry *
211  *************************************************/
212 
218 std::pair<double,double> WebMercator2WGS84(double mercatorY_lat, double mercatorX_lon)
219 {
220  // hacky way of doing it: Convert to WGS84 using some constants
221  // and use haversine after that. This can be made faster and
222  // much much more reliable ;-)
223 
224  if (abs(mercatorX_lon) < 180 && abs(mercatorY_lat) < 90)
225  return make_pair(std::numeric_limits<double>::infinity(),std::numeric_limits<double>::infinity());
226 
227  if ((abs(mercatorX_lon) > 20037508.3427892) || (abs(mercatorY_lat) > 20037508.3427892))
228  return make_pair(std::numeric_limits<double>::infinity(),std::numeric_limits<double>::infinity());
229 
230  double x = mercatorX_lon;
231  double y = mercatorY_lat;
232  double num3 = x / 6378137.0;
233  double num4 = num3 * 57.295779513082323;
234  double num5 = floor((num4 + 180.0) / 360.0);
235  double num6 = num4 - (num5 * 360.0);
236  double num7 = 1.5707963267948966 - (2.0 * atan(exp((-1.0 * y) / 6378137.0)));
237  mercatorX_lon = num6;
238  mercatorY_lat = num7 * 57.295779513082323;
239 // cout << "WGS84: "<< mercatorX_lon << ";" << mercatorY_lat << endl;
240  return (make_pair(mercatorY_lat,mercatorX_lon));
241 }
242 
243 
251 template<typename coord>
252 double WebMercatorDistance(coord p1, coord p2, coord p3, coord p4 )
253 {
254  double lon1, lon2, lat1, lat2;
255  tie(lat1,lon1) = WebMercator2WGS84(p1,p2);
256  tie(lat2,lon2) = WebMercator2WGS84(p3,p4);
257  // convert to radians
258  lat1 *= (M_PI / 180.0);
259  lon1 *= (M_PI / 180.0);
260  lat2 *= (M_PI / 180.0);
261  lon2 *= (M_PI / 180.0);
262 
263  double dlon = lon2 - lon1;
264  double dlat = lat2 - lat1;
265  double a = sin(dlat/2)*sin(dlat/2) + cos(lat1) * cos(lat2) * sin(dlon/2)*sin(dlon/2);
266  double c = 2 * atan2(sqrt(a), sqrt(1-a));
267  return 6371000.0 * c;
268 }
269 
270 
271  /*************************************************
272  * Section 1.2: Shape Data Types *
273  *************************************************/
274 
275 typedef std::vector<std::vector<double>> polyline;
276 typedef std::vector<polyline> polycollection;
277 typedef std::vector<std::vector<double>> pointcollection;
278 
279 #include "codegen/NewRoadAttrib.hpp"
280 #include "codegen/NewNodeAttrib.hpp"
281 
282 typedef std::vector<NewRoadAttrib> newroadattribcollection;
283 typedef std::vector<NewNodeAttrib> newnodeattribcollection;
284 
285 
286  /*************************************************
287  * Section 1.3: Data Loading *
288  *************************************************/
295 std::string shapename(size_t type)
296 {
297  switch(type){
298  case 0: return std::string("NULL");
299  case 1: return std::string("POINT");
300  case 3: return std::string("ARC");
301  case 5: return std::string("POLYGON");
302  case 8: return std::string("MULTIPOINT");
303  default: return std::string("undefined");
304  }
305 }
306 
314 std::string dbftype(size_t type)
315 {
316  switch(type){
317  case 0: return std::string("FTString");
318  case 1: return std::string("FTInteger");
319  case 2: return std::string("FTDouble");
320  case 3: return std::string("FTLogical");
321  default: return std::string("FTundefined");
322  }
323 }
324 
332 inline void handlePolyline(polycollection &coll, SHPObject *shape)
333 {
334  polycollection::value_type thispline;
335  for (size_t i=0; i < shape->nVertices; i++)
336  {
337  double x=0,y=0,z=0;
338  if(shape->padfX != NULL) x = shape->padfX[i];
339  if(shape->padfY != NULL) y = shape->padfY[i];
340  if(shape->padfZ != NULL) z = shape->padfZ[i];
341  thispline.push_back({x,y,z});
342  }
343  coll.push_back(thispline);
344 }
345 
352 inline void handlePoint(pointcollection &coll, SHPObject *shape)
353 {
354  pointcollection::value_type thispoint;
355  if (shape->nVertices != 1)
356  {
357  cout << "Point object with more than one vertex. Only adding first" << endl;
358  }
359 
360  if (shape->nVertices == 0)
361  {
362  cout << "Barrier object without point skipped" << endl;
363  return;
364  }
365 
366  double x=0,y=0,z=0;
367  if(shape->padfX != NULL) x = shape->padfX[0];
368  if(shape->padfY != NULL) y = shape->padfY[0];
369  if(shape->padfZ != NULL) z = shape->padfZ[0];
370  coll.push_back({x,y,z});
371 }
372 
373 
374 
386 template<typename polylinecollection, typename attribcollection, typename outstream,
387  typename shapehandler>
388 void importSHP(std::string filename, polylinecollection &coll,
389  attribcollection &attribs,
390  shapehandler handler,
391  outstream &out = std::cerr, bool verbose=true
392  )
393 {
394  SHPHandle hSHP;
395  DBFHandle hDBF;
396  int nShapeType, i, nEntities, nShpInFile;
397  SHPObject *shape;
398 
399  hSHP = SHPOpen( (filename+std::string(".shp")).c_str(), "rb" );
400  hDBF = DBFOpen ((filename+std::string(".dbf")).c_str(), "rb");
401  if(hSHP == NULL) throw(std::runtime_error("Unable to open Shapefile"));
402  if(hDBF == NULL) throw(std::runtime_error("Unable to open DBF "));
403 
404  SHPGetInfo( hSHP, &nEntities, &nShapeType, NULL, NULL );
405  if (verbose)
406  out << "Importing file with " << nEntities << " entities"<<endl;
407 
408  size_t nDBFRecords = DBFGetRecordCount(hDBF );
409  if (verbose)
410  out << "Found a DBF with " << nDBFRecords << " entries" << endl;
411 
412  if (nEntities != nDBFRecords)
413  {
414  out << "Using DBF and SHP pair of different size: " << filename << endl;
415  }
416 
417  /*And the associated DBF information*/
418  size_t fields = DBFGetFieldCount(hDBF);
419  if (verbose){
420  for (size_t i=0; i < fields; i++)
421  {
422  char name[20];
423  size_t type = DBFGetFieldInfo(hDBF,i,name, NULL, NULL);
424  cout << dbftype(type) << ":" << name << "\n";
425  }
426  cout << endl << endl;
427  }
428 
429  // File Type Checking
430  if (!attribcollection::value_type::assertCorrectFormat(hDBF)){
431  cout << "Error: Wrong DBF format" << endl;
432  }
433 
434  // File Loading loop
435  for( i = 0; i < nEntities; i++ )
436  {
437  shape = SHPReadObject( hSHP, i );
438  if (shape == NULL) throw(std::runtime_error("unable to read some shape"));
439 #if VERY_VERBOSE
440  cout << "Found a shape" << shapename(shape->nSHPType) << "(" << shape->nSHPType <<")"<< endl;
441  cout << "ID:" << shape->nShapeId << endl;
442  cout << "numParts: " << shape->nParts << endl;
443  cout << "numVertices:" << shape->nVertices << endl;
444 #endif
445  handler(coll,shape);
446  SHPDestroyObject ( shape );
447  /*Read all attributes*/
448  typename attribcollection::value_type attrib;
449  attrib.readFromID(hDBF, i);
450  if (verbose)
451  attrib.dump(cout);
452  attribs.push_back(attrib);
453  }
454  SHPClose( hSHP );
455  DBFClose( hDBF);
456  out << "Completed " << nEntities << " entities."<<endl;
457 }
458 
467 template<typename polylinecollection, typename outstream,
468  typename shapehandler>
469 void importSHPOnlyGeometry(std::string filename, polylinecollection &coll,
470  shapehandler handler,
471  outstream &out = std::cerr, bool verbose=false
472  )
473 {
474  SHPHandle hSHP;
475  DBFHandle hDBF;
476  int nShapeType, i, nEntities, nShpInFile;
477  SHPObject *shape;
478 
479  hSHP = SHPOpen( (filename+std::string(".shp")).c_str(), "rb" );
480  if(hSHP == NULL) throw(std::runtime_error("Unable to open Shapefile"));
481 
482  SHPGetInfo( hSHP, &nEntities, &nShapeType, NULL, NULL );
483  if (verbose)
484  out << "Importing file with " << nEntities << " entities"<<endl;
485 
486 
487  // File Loading loop
488  for( i = 0; i < nEntities; i++ )
489  {
490  shape = SHPReadObject( hSHP, i );
491  if (shape == NULL) throw(std::runtime_error("unable to read some shape"));
492 #if VERY_VERBOSE
493  cout << "Found a shape" << shapename(shape->nSHPType) << "(" << shape->nSHPType <<")"<< endl;
494  cout << "ID:" << shape->nShapeId << endl;
495  cout << "numParts: " << shape->nParts << endl;
496  cout << "numVertices:" << shape->nVertices << endl;
497 #endif
498  handler(coll,shape);
499  SHPDestroyObject ( shape );
500  }
501  SHPClose( hSHP );
502  out << "Completed " << nEntities << " entities."<<endl;
503 }
504 
511 template<typename polylinecollection, typename attribcollection, typename outstream>
512 void importSHPPolylines(std::string filename, polylinecollection &coll,
513  attribcollection &attribs,
514  outstream &out = std::cerr, bool verbose=false
515  )
516 {
517 
518  importSHP(filename,coll,attribs, handlePolyline ,out,verbose);
519 }
520 
526 template<typename polylinecollection, typename outstream>
527 void importSHPPolylinesOnlyGeometry(std::string filename, polylinecollection &coll,
528  outstream &out = std::cerr, bool verbose=false
529  )
530 {
531 
532  importSHPOnlyGeometry(filename,coll, handlePolyline ,out,verbose);
533 }
534 
540 template<typename collection, typename attribcollection, typename outstream>
541 void importSHPPoints(std::string filename, collection &coll,
542  attribcollection &attribs,
543  outstream &out = std::cerr, bool verbose=false
544  )
545 {
546 
547  importSHP(filename,coll,attribs, handlePoint ,out,verbose);
548 }
549 
550 
551 
552 /*************************************************
553  * Section 1.4: The Document (ShapeCollection) *
554  *************************************************/
556  {
557  public:
558 
561  bgi::rtree< value, bgi::rstar<16, 4> >
563 
566  bgi::rtree< value, bgi::rstar<16, 4> >
568 
570  std::vector<polygon> obstacles;
571 
572 
573  public:
574 
585  void addPolygonalObstacle(polycollection::value_type &in)
586  {
587  polygon p;
588  for (auto &x: in)
589  {
590  p.outer().push_back(point(x[0],x[1]));
591  }
592  obstacles.push_back(p);
593  /* BOOST_FOREACH(polygon const& x, obstacles)
594  std::cout << bg::wkt<polygon>(x) << std::endl;*/
595  }
596 
606  void getMBR(double &l, double &r, double &b, double &t)
607  {
608  l = b = std::numeric_limits<double>::infinity();
609  r = t = -std::numeric_limits<double>::infinity();
610  for (auto &j:junctions)
611  {
612  double x = j[0][0];
613  double y = j[0][1];
614  if (x < l) l = x;
615  if (x > r) r = x;
616  if (y < b) b = y;
617  if (y > t) t = y;
618  }
619  }
620 
633  void fillSpatialIndizes()
634  {
635  /*Fill roads by creating temporary polygons*/
636  for (size_t i=0; i < roads.size(); i++)
637  {
638  polygon poly;
639  for (auto p:roads[i])
640  {
641  box b(point(p[0],p[1]),point(p[0],p[1]));
642  roads_rtree.insert(std::make_pair(b,i));
643  }
644  }
645 
646  for (size_t i=0; i < junctions.size(); i++)
647  {
648  box b(point(junctions[i][0][0],junctions[i][0][1]),
649  point(junctions[i][0][0],junctions[i][0][1]));
650 
651  junction_rtree.insert(std::make_pair(b, i));
652  }
653  }
654 
655 
656 
664  size_t nearestJunction(double x, double y)
665  {
666  std::vector<value> result_n;
667  junction_rtree.query(bgi::nearest(point(x, y), 1), std::back_inserter(result_n));
668  return result_n[0].second;
669  }
670 
671 
672 
678  std::vector<size_t> nearestRoads(double x, double y, size_t k)
679  {
680  std::vector<value> result_n;
681  roads_rtree.query(bgi::nearest(point(x, y), k), std::back_inserter(result_n));
682  std::vector<size_t> ret;
683  for (auto a : result_n) {
684  ret.push_back(a.second);
685  }
686  return ret;
687  }
688 
689  /*std::vector<size_t> junctionsOnRoad(size_t road)
690  {
691  std::vector<size_t> ret;
692  for (size_t i=0; i < roads[road].size(); i++)
693  {
694  auto junction=nearestJunction(roads[road][i][0],roads[road][i][1]);
695  if (d(roads[road][i], junctions[junction][0]) < 0.1)
696  {
697  ret.push_back(junction);
698  }
699  }
700  return ret;
701  }
702  bool is_junction(size_t road, size_t idx)
703  {
704  auto junction=nearestJunction(roads[road][idx][0],roads[road][idx][1]);
705  return (d(roads[road][idx], junctions[junction][0]) < 0.1);
706  }
707  std::pair<size_t,size_t> find_next_junction(size_t road, size_t idx)
708  {
709  for (size_t i=idx; i < roads[road].size(); i++)
710  {
711  auto junction=nearestJunction(roads[road][i][0],roads[road][i][1]);
712  if (d(roads[road][i], junctions[junction][0]) < 0.1)
713  {
714  return make_pair(junction,i); // junction index and road index
715  }
716  }
717  return make_pair(junctions.size(),roads[road].size());
718  }
719 
720 
721  double roadWeight(size_t road, size_t start, size_t end, size_t valuation=VALUATION_DISTANCE)
722  {
723  double ret = 0;
724  for (size_t i=start; i < end; i++)
725  {
726  switch(valuation){
727  case VALUATION_DISTANCE:
728  ret += d(roads[road][i],roads[road][i+1]);
729 
730  break;
731  case VALUATION_TIME:
732 
733  break;
734  default:
735  cout << "Unknown valuation" << endl;
736  }
737 
738  }
739  //cout << "#?" << d(roads[road][start],roads[road][end]) << endl;
740  //cout << road <<"," << start<< "," << end <<"==" << ret << endl;
741  return ret;
742  }
743  */
744 
752  void LoadFiles(std::string roadfile, std::string junctionfile)
753  {
754 
755  cout << "Step 1: Load roads \t";
756  importSHPPolylines(removeExt(roadfile), roads,aroads,cout);
757 
758  cout << "Step 2: Load junctions\t";
759  importSHPPolylines(removeExt(junctionfile), junctions,ajunctions,cout);
760 
761  fillSpatialIndizes();
762  }
763 
764 
765 
773  void LoadDirectory(std::string filebase)
774  {
775  cout << "Loading from "<<filebase << endl;
776  cout << "Step 1: Load roads \t";
777  importSHPPolylines((filebase + "/sfo_roads"), roads,aroads,cout);
778 
779  cout << "Step 2: Load junctions\t";
780  importSHPPolylines((filebase + "/sfo_nodes"), junctions,ajunctions,cout);
781  //cout << "Step 5: Spatial Indizes"<< endl;
782  fillSpatialIndizes();
783  }
784 
785 
798  void LoadRestrictions(std::string fname)
799  {
800  polygons.clear();
801  importSHPPolylinesOnlyGeometry(removeExt(fname), polygons,cout);
802  cout << "Found " << polygons.size() << "polygons" << endl;
803  for (auto &p: polygons)
804  addPolygonalObstacle(p);
805  }
806 
807 
808 
809 
810  };
811 
812 /**************************************************
813  * Section 2: Graph Search *
814  *************************************************/
815 /**************************************************
816  * Section 2.1: Graph Types *
817  *************************************************/
818 
819 using namespace boost;
820 
821 struct location
822 {
823  double x,y;
824 };
825 typedef double cost;
826 
827  typedef adjacency_list<vecS, vecS, bidirectionalS, property<vertex_color_t,default_color_type>,
828  property<edge_weight_t, cost, property<edge_index_t,size_t>> > mygraph_t;
829 
830  typedef adjacency_list<vecS, vecS, undirectedS, property<vertex_color_t,default_color_type>,
831  property<edge_weight_t, cost, property<edge_index_t,size_t>> > mygraph_t_undirected;
832 
833  typedef property_map<mygraph_t, edge_weight_t>::type WeightMap;
834  typedef property_map<mygraph_t, vertex_color_t>::type ColorMap;
835  typedef color_traits<property_traits<ColorMap>::value_type> Color;
836  typedef property_map<mygraph_t, edge_index_t>::type IndexMap;
837  typedef mygraph_t::vertex_descriptor vertex;
840 
841 
842 
843 
844  /***********************************************************
845  * Section 2.2: Implementation and Evaluation of Searches *
846  **********************************************************/
847 
858  template<class Graph, class Colors, class PathDescriptor, class Distance>
859 std::string stats(std::string name, Graph &g, Colors &colors, PathDescriptor &shortest_path, Distance &d,
860  size_t lastStart, size_t lastGoal, double _d, size_t examines)
861  {
862  size_t white=0, gray=0, black=0;
863  for (auto vp = vertices(g); vp.first != vp.second; ++vp.first)
864  {
865  if(colors[*vp.first] == Color::white()) white++;
866  if(colors[*vp.first] == Color::gray()) gray++;
867  if(colors[*vp.first] == Color::black()) black++;
868  }
869 
870 
871  boost::property_tree::ptree pt;
872  pt.put ("algorithm", name);
873  pt.put("examines",examines);
874  pt.put ("white", white);
875  pt.put ("gray", gray);
876  pt.put ("black", black);
877  pt.put("path_vertices",shortest_path.size());
878  pt.put("distance", _d);
879  pt.put("from", lastStart);
880  pt.put("to", lastGoal);
881 #ifdef STAT_OUTPUT_VERTICES
882  boost::property_tree::ptree vertex_list;
883  auto it = shortest_path.begin();
884  for (size_t i=0; i < shortest_path.size(); i++){
885  stringstream s(""); s << i;
886  vertex_list.put(s.str(),*it);
887  it = std::next(it);
888  }
889  //pt.put("vertices",vertex_list);
890  pt.push_back(std::make_pair("vertices",vertex_list));
891 #endif
892 
893  std::ostringstream buf;
894  write_json (buf, pt, false);
895  return buf.str();
896 }
897 
898 
907 #define DEFAULT_STATS(x) {cout << stats (x,g, colors, shortest_path, d, lastStart, lastGoal,getDistance(),stat_examines) << endl;}
908 
909 
918 {
925 };
933 const std::vector<std::string> SearchAlgorithmNames=
934 {
935  "Boost A* ",
936  "Boost Dijkstra",
937  "Basic A* ",
938  "Basic Dijkstra ",
939  "ALT "
940 };
941 
948 #define STAT_EXAMINE() {stat_examines ++;}
949 
950 
958 {
959  public:
960  /*Types*/
961  typedef std::pair<double,mygraph_t::vertex_descriptor> queue_element;
963  bool operator()( queue_element const& lhs, queue_element const& rhs){
964  // we want a smallest queue
965  return lhs.first >= rhs.first;
966  }
967  };
968  typedef std::priority_queue<queue_element,
969  std::vector<queue_element>,
972 
973 
975  std::vector<double> f;
977  vector<mygraph_t::vertex_descriptor> p;
978  vector<cost> d;
979  vector<default_color_type> colors;
980 
981  // for twosided search
982  vector<mygraph_t::vertex_descriptor> p1,p2;
983  vector<cost> f1,f2,d1,d2;
984  vector<default_color_type> colors1,colors2;
985 
986 
987  std::vector<double> Time;
988  std::vector<double> Length;
989 
991 
992 
993 
994  vector<unsigned long> vertex_ids;
995 
996  std::vector<location> locations;
997 
998  std::map<mygraph_t::edge_descriptor,linestring> ls_roads;
999 
1000  list<mygraph_t::vertex_descriptor> shortest_path;
1001  mygraph_t::vertex_descriptor lastStart, lastGoal;
1002 
1003  bgi::rtree< value, bgi::rstar<16, 4> > vertex_rtree;
1004 
1005  size_t stat_examines;
1006 
1007 
1008 
1015  size_t nearestVertex(double x, double y)
1016  {
1017  std::vector<value> result_n;
1018  vertex_rtree.query(bgi::nearest(point(x, y), 1), std::back_inserter(result_n));
1019  return result_n[0].second;
1020  }
1021 
1028 std::string default_stats()
1029 {
1030  return stats (SearchAlgorithmNames[search_algorithm],g, colors, shortest_path, d, lastStart, lastGoal,getDistance(),stat_examines);
1031 }
1032 
1039 
1040 
1048  double getDistance()
1049  {
1050  // Some searches don't return the distance, e.g., two-sided. So we recalculate
1051  // in this case
1052  double d = 0;
1053  list<mygraph_t::vertex_descriptor>::iterator it,nx, it_end;
1054  it_end = shortest_path.end();
1055  for (it = shortest_path.begin(); it != it_end; it++)
1056  {
1057  nx = std::next(it);
1058  if (nx == it_end) break;
1059  edge_descriptor e; bool found;
1060  tie(e,found) = boost::edge( *it,*nx,g );
1061  if (!found){
1062  cout << "Edge of SP not found!" << *it <<"=>" << *nx << endl;
1063  return 0;
1064  }
1065  d += weightmap[e];
1066  }
1067  return d;
1068  }
1069 
1076  double getTime()
1077  {
1078  double d = 0;
1079  list<mygraph_t::vertex_descriptor>::iterator it,nx, it_end;
1080  it_end = shortest_path.end();
1081  for (it = shortest_path.begin(); it != it_end; it++)
1082  {
1083  nx = std::next(it);
1084  if (nx == it_end) break;
1085  edge_descriptor e; bool found;
1086  tie(e,found) = boost::edge( *it,*nx,g );
1087  if (!found){
1088  cout << "Edge of SP not found!" << *it <<"=>" << *nx << endl;
1089  return 0;
1090  }
1091  d += Time[edgeidmap[e]];//weightmap[e];
1092  }
1093  return d;
1094  }
1095 
1096 
1097  size_t g_edge_index;
1098 
1105  edge_descriptor myadd_edge(mygraph_t::vertex_descriptor s,mygraph_t::vertex_descriptor e,double w )
1106  {
1107  edge_descriptor ed;
1108 
1109  bool inserted;
1110  tie(ed, inserted) = add_edge(s,e, g);
1111  if (!inserted)
1112  {
1113  throw(std::runtime_error("unable to insert edge"));
1114  }
1115  edgeidmap[ed] = g_edge_index ++;
1116  weightmap[ed] = w;
1117  return ed;
1118  }
1119 
1120 
1131  mygraph_t::vertex_descriptor getVertexIndexFromLocation(cost x, cost y, double eps=0.01)
1132  {
1133  // check, if the vertex is already modelled, otherwise add a vertex
1135  // Query R tree for nearest
1136  std::vector<value> result_n;
1137  vertex_rtree.query(bgi::nearest(point(x, y), 1), std::back_inserter(result_n));
1138  if (result_n.size() != 0)
1139  if (bg::distance(result_n[0].first, point(x,y)) < eps)
1140  {
1141  return result_n[0].second;
1142  }
1143 
1144 
1145  auto w = add_vertex(g);
1146  locations.push_back({x,y});
1147  box b(point(x,y),
1148  point(x,y));
1149  vertex_rtree.insert(std::make_pair(b,w));
1150  return w;
1151  }
1152 
1153 
1164  void createFromSimplifiedShapeCollection (NewShapeCollection &shapes)
1165  {
1166  g.clear();
1167 
1168 
1169  std::vector<location> tmp_locations;
1170 
1171  vertex_ids.resize(shapes.ajunctions.size());
1172  for (size_t i=0; i < shapes.ajunctions.size(); i++)
1173  {
1174  vertex_ids[i] = shapes.ajunctions[i].ID;
1175  }
1176 
1177  // Add the location
1178  tmp_locations.resize(shapes.junctions.size());
1179  for (size_t i=0; i < shapes.junctions.size(); i++)
1180  tmp_locations[i] = {shapes.junctions[i][0][0],shapes.junctions[i][0][1]};
1181 
1182  // now vertex_ids can be used to find the location in tmp_locations
1183 
1184 
1185  weightmap = get(edge_weight, g);
1186  edgeidmap = get(edge_index, g);
1187 
1188  Time.resize(shapes.aroads.size());
1189  Length.resize(shapes.aroads.size());
1190 
1191  g_edge_index = 0;
1192  for (size_t i=0; i < shapes.aroads.size(); i++)
1193  {
1194  auto &r = shapes.aroads[i];
1195  //cout << r.EDGEID << ":" << r.STARTID << "=>" << r.ENDID << " (" << r.LENGTH <<";" << r.SPD << ")"<< endl;
1196  auto start = std::find(vertex_ids.begin(),vertex_ids.end(),r.STARTID);
1197  auto end = std::find(vertex_ids.begin(),vertex_ids.end(),r.ENDID);
1198 
1199  if (start == vertex_ids.end() || end == vertex_ids.end())
1200  {
1201  cerr << "Dataset contains edge without a vertex" << endl;
1202  throw(std::runtime_error("Dataset contains edge without a vertex\n"));
1203  }
1204 
1205  location l1,l2;
1206  l1 = tmp_locations[std::distance(vertex_ids.begin(),start)];
1207  l2 = tmp_locations[std::distance(vertex_ids.begin(),end)];
1208  auto v1 = getVertexIndexFromLocation(l1.x,l1.y);
1209  auto v2 = getVertexIndexFromLocation(l2.x,l2.y);
1210 
1211  auto id = g_edge_index;
1212  auto e = myadd_edge(v1,v2,r.LENGTH);
1213 
1214  // store the complete road
1215  linestring road;
1216  for (auto it = shapes.roads[i].begin(), it_end = shapes.roads[i].end();
1217  it != it_end; ++it )
1218  {
1219  road.push_back(point((*it)[0],(*it)[1]));
1220  }
1221  ls_roads[e] = road;
1222 
1223 
1224  //auto id = num_edges(g)-1;
1225  // check that this was unique
1226  start ++;
1227  end ++;
1228 
1229  Length[id] = r.LENGTH;
1230  Time[id] = r.LENGTH / r.SPD;
1231  }
1232 
1233 
1234 
1235  /*removed_edges.resize(num_edges(g));
1236  for (size_t i=0; i < removed_edges.size(); i++)
1237  remove_map[i] = false;*/
1238 
1239  // add all vertices
1240  cout << "Built a graph." << endl;
1241  cout << "Vertices: " << num_vertices(g) <<", Edges: " << num_edges(g) << endl;
1242  /*Allocate all properties*/
1243  f.resize(num_vertices(g));
1244  p.resize(num_vertices(g));
1245  d.resize(num_vertices(g));
1246  colors.resize(num_vertices(g));;
1247 
1248  }
1249 
1257  void activateTime()
1258  {
1259  mygraph_t::edge_iterator oe, oe_end;
1260  for(tie(oe,oe_end) = edges(g); oe != oe_end; ++oe)
1261  weightmap[*oe] = Time[edgeidmap[*oe]];
1262  }
1263 
1264 
1265 
1273  void activateDistance()
1274  {
1275  mygraph_t::edge_iterator oe, oe_end;
1276  for(tie(oe,oe_end) = edges(g); oe != oe_end; ++oe)
1277  weightmap[*oe] = Length[edgeidmap[*oe]];
1278  }
1279 
1290  void applyPolygonalRestrictions(std::vector<polygon> &restrictions)
1291  {
1292 
1293  mygraph_t::edge_iterator oe, oe_end;
1294  for(tie(oe,oe_end) = edges(g); oe != oe_end; oe ++)
1295  {
1296  for (auto &r: restrictions)
1297  {
1298  if (bg::intersects(r,ls_roads[*oe]))
1299  {
1300  weightmap [*oe] = std::numeric_limits<cost>::infinity();
1301  }
1302  }
1303 
1304  }
1305  }
1306 
1307 
1315  mygraph_t::vertex_descriptor random_v()
1316  {
1317  static boost::mt19937 gen(time(0));
1318  return random_vertex(g, gen);
1319  }
1320 
1330  std::pair<bool, double> search(size_t start, size_t end)
1331  {
1332  std::pair<bool,double> ret;
1333  stat_examines = 0;
1334  shortest_path.clear();
1335 
1336  switch(search_algorithm)
1337  {
1339  return search_reference(start,end);
1341  return search_boostdijkstra(start,end);
1343  return search_explicit(start,end);
1345  return search_dijkstra(start,end);
1346  case SEARCH_ALGORITHM_ALT:
1347  return search_ALT(start,end);
1348 
1349  };
1350  cout << "Invalid Search Algorithm" << endl;
1351  return make_pair(false,std::numeric_limits<double>::infinity());
1352  }
1353 
1360  std::pair<bool,double> search()
1361  {
1362  return search(lastStart,lastGoal);
1363  }
1364 
1365  /**************************************************
1366  * Section 2.2.1: Reference A* Search *
1367  *************************************************/
1368 
1369 #include "algorithm/boostastar.hpp"
1370 
1371  /**************************************************
1372  * Section 2.2.2: Explicit A* Search *
1373  *************************************************/
1374 #include "algorithm/astar.hpp"
1375 
1376  /**************************************************
1377  * Section 2.2.3: Explicit Dijkstra *
1378  *************************************************/
1379 #include "algorithm/dijkstra.hpp"
1380 
1381  /**************************************************
1382  * Section 2.2.4: Boost Dijkstra *
1383  *************************************************/
1384 #include "algorithm/boostdijkstra.hpp"
1385 
1386  /**************************************************
1387  * Section 2.2.5: ALT *
1388  *************************************************/
1389 #include "algorithm/ALT.hpp"
1390 
1391 
1392 
1393  /**************************************************
1394  * Section 2.3: OpenGL Rendering (Optional) *
1395  *************************************************/
1396 
1397 
1398 
1406  void renderGL(bool showDirected=true)
1407  {
1408 #ifndef GISCUP_NO_OPENGL
1409  //colors = get(vertex_color,g);
1410  glPointSize(2);
1411  glBegin(GL_POINTS);
1412  for (auto vp = vertices(g); vp.first != vp.second; ++vp.first)
1413  {
1414  glColor3f(0.5,0.5,0.5);
1415 
1416  //if(colors[*vp.first] == Color::white()) glColor3f(0.2,0.8,0.2);
1417  if(colors[*vp.first] == Color::white()) continue;
1418  if(colors[*vp.first] == Color::gray()) glColor3f(0.8,0.8,0.8);
1419  if(colors[*vp.first] == Color::black()) glColor3f(0,0,0);
1420 
1421 
1422  double x = locations[*vp.first].x;
1423  double y = locations[*vp.first].y;
1424  glVertex2f(x,y);
1425  //cout << "Putting a point" << x << "/" << y << endl;
1426  }
1427  glEnd();
1428 
1429 
1430 
1431  glColor4f(0,0,1,0.2);
1432  glLineWidth(2);
1433  glBegin(GL_LINES);
1434  for (auto ed = edges(g); ed.first != ed.second; ++ed.first)
1435  {
1436  edge_descriptor e = *ed.first;
1437  double x = locations[source(e,g)].x;
1438  double y = locations[source(e,g)].y;
1439  double x2 = locations[target(e,g)].x;
1440  double y2 = locations[target(e,g)].y;
1441  glVertex2f(x,y);
1442  glVertex2f(x2,y2);
1443  if (showDirected){
1444  double dx = (x2-x);
1445  double dy = (y2-y);
1446  double len = sqrt (dx*dx+dy*dy);
1447  double glmpx = (x+x2) / 2;
1448  double glmpy = (y+y2) / 2;
1449 
1450  double glrpx = x + 0.4 * dx;
1451  double glrpy = y + 0.4 * dy;
1452  double glpfeil1_x = glrpx + 0.1 *(-dy);
1453  double glpfeil1_y = glrpy + 0.1 *(dx);
1454  double glpfeil2_x = glrpx - 0.1 *(-dy);
1455  double glpfeil2_y = glrpy - 0.1 *(dx);
1456  // cout << glrpx << "," << glrpy<<"--"<<glpfeil1_x << "," << glpfeil1_y << endl;
1457  glVertex2f(glmpx,glmpy); glVertex2f(glpfeil1_x,glpfeil1_y);
1458  glVertex2f(glmpx,glmpy); glVertex2f(glpfeil2_x,glpfeil2_y);
1459  glVertex2f(glpfeil1_x,glpfeil1_y); glVertex2f(glpfeil2_x,glpfeil2_y);
1460  }
1461 
1462  //cout << "Putting a point" << x << "/" << y << endl;
1463  }
1464  glEnd();
1465  glPointSize(2);
1466  glBegin(GL_POINTS);
1467  for (auto vp = vertices(g); vp.first != vp.second; ++vp.first)
1468  {
1469  glColor3f(0.5,0.5,0.5);
1470 
1471  //if(colors[*vp.first] == Color::white()) glColor3f(0.2,0.8,0.2);
1472  if(colors[*vp.first] == Color::white()) continue;
1473  if(colors[*vp.first] == Color::gray()) glColor3f(0.8,0.8,0.8);
1474  if(colors[*vp.first] == Color::black()) glColor3f(0,0,0);
1475 
1476 
1477  double x = locations[*vp.first].x;
1478  double y = locations[*vp.first].y;
1479  glVertex2f(x,y);
1480  //cout << "Putting a point" << x << "/" << y << endl;
1481  }
1482  glEnd();
1483 
1484 #endif
1485  }
1486 
1494  void renderShortest()
1495  {
1496 #ifndef GISCUP_NO_OPENGL
1497  glDisable(GL_BLEND);
1498  glLineWidth(50);
1499  glColor4f(1,0.2,0.2,1);
1500  glBegin(GL_LINE_STRIP);
1501 
1502  for (auto &l : shortest_path)
1503  {
1504  double x = locations[l].x;
1505  double y = locations[l].y;
1506  glVertex2f(x,y);
1507  }
1508  glEnd();
1509  glEnable(GL_POINT_SMOOTH);
1510  glPointSize(16);
1511  glColor3f(0,0,1);
1512  glBegin(GL_POINTS);
1513  glVertex2f(locations[lastStart].x,locations[lastStart].y);
1514  glEnd();
1515  glColor3f(0,1,0);
1516  glBegin(GL_POINTS);
1517  glVertex2f(locations[lastGoal].x,locations[lastGoal].y);
1518  glEnd();
1519 
1520  glDisable(GL_POINT_SMOOTH);
1521 #endif
1522  }
1523 
1524 #ifndef GISCUP_NO_OPENGL
1525 
1531 void glJet(double v, double vmin, double vmax, double alpha=1)
1532 {
1533  double r=1, g =1, b = 1.0;
1534  double dv;
1535  if (v < 0)
1536  {
1537  r = g = b = 1;
1538  alpha = 0.1;
1539  }else{
1540 
1541  if (v < vmin)
1542  v = vmin;
1543  if (v > vmax)
1544  v = vmax;
1545  dv = vmax - vmin;
1546 
1547  if (v < (vmin + 0.25 * dv)) {
1548  r = 0;
1549  g = 4 * (v - vmin) / dv;
1550  } else if (v < (vmin + 0.5 * dv)) {
1551  r = 0;
1552  b = 1 + 4 * (vmin + 0.25 * dv - v) / dv;
1553  } else if (v < (vmin + 0.75 * dv)) {
1554  r = 4 * (v - vmin - 0.5 * dv) / dv;
1555  b = 0;
1556  } else {
1557  g = 1 + 4 * (vmin + 0.75 * dv - v) / dv;
1558  b = 0;
1559  }
1560  }
1561  glColor4f(r,g,b,alpha);
1562 }
1563 
1564 
1570  void renderLandmark(size_t which)
1571  {
1572  // first pass: max of landmark distance
1573  if (which >= landmarks.size() )
1574  return;
1575 
1576  double m = 0;
1577  for (size_t i=0; i < num_vertices(g); i++)
1578  if (landmarks[which].d_from[i] > m && landmarks[which].d_from[i] < 10E9)
1579  m = landmarks[which].d_from[i];
1580  glPointSize(4);
1581  glBegin(GL_POINTS);
1582  for (auto vp = vertices(g); vp.first != vp.second; ++vp.first)
1583  {
1584  glColor3f(0.5,0.5,0.5);
1585 
1586  if (landmarks[which].d_from[*vp.first] < std::numeric_limits<cost>::infinity())
1587  {
1588  glJet(landmarks[which].d_from[*vp.first],0,m);
1589  }else
1590  continue;
1591 
1592 
1593  double x = locations[*vp.first].x;
1594  double y = locations[*vp.first].y;
1595  glVertex2f(x,y);
1596 
1597  }
1598  glEnd();
1599 
1600 
1601  }
1602 
1603 
1604 #endif // OpenGL
1605 };
1606 
1607 #endif // header