Tidy up unused source
[bus.git] / origin-src / tripgraph.cc
1 #include "tripgraph.h"
2 #include <assert.h>
3 #include <errno.h>
4 #include <map>
5 #include <math.h>
6 #include <stdlib.h>
7
8 using namespace std;
9 using namespace tr1;
10
11 // set to 1 to see what find_path is doing (VERY verbose)
12 #if 0
13 # define DEBUGPATH(fmt, args...) fprintf(stderr, fmt, ## args)
14 #else
15 # define DEBUGPATH
16 #endif
17
18 // Estimated walking speed in m/s
19 static const float EST_WALK_SPEED = 1.1f;
20 static int SECS_IN_DAY = (60*60*24);
21
22
23 static inline double radians(double degrees)
24 {
25 return degrees/180.0f*M_PI;
26 }
27
28 static inline double degrees(double radians)
29 {
30 return radians*180.0f/M_PI;
31 }
32
33 static double distance(double src_lat, double src_lng, double dest_lat,
34 double dest_lng)
35 {
36 // returns distance in meters
37 static const double EPSILON = 0.00005;
38
39 if (fabs(src_lat - dest_lat) < EPSILON && fabs(src_lng - dest_lng) < EPSILON) {
40 return 0.0f;
41 }
42
43 double theta = src_lng - dest_lng;
44 double src_lat_radians = radians(src_lat);
45 double dest_lat_radians = radians(dest_lat);
46 double dist = sin(src_lat_radians) * sin(dest_lat_radians) +
47 cos(src_lat_radians) * cos(dest_lat_radians) *
48 cos(radians(theta));
49 dist = acos(dist);
50 dist = degrees(dist);
51 dist *= (60.0f * 1.1515 * 1.609344 * 1000.0f);
52 return dist;
53 }
54
55
56 TripGraph::TripGraph()
57 {
58 set_timezone("UTC");
59 }
60
61
62 void TripGraph::load(string fname)
63 {
64 FILE *fp = fopen(fname.c_str(), "r");
65 if (!fp)
66 {
67 printf("Error: Couldn't open graph file %s: %s (%d).\n",
68 fname.c_str(), strerror(errno), errno);
69 return;
70 }
71
72 uint32_t timezone_len;
73 assert(fread(&timezone_len, sizeof(uint32_t), 1, fp) == 1);
74 char tz[timezone_len+1];
75 assert(fread(tz, sizeof(char), timezone_len, fp) == timezone_len);
76 tz[timezone_len] = '\0';
77 set_timezone(tz);
78
79 uint32_t num_service_periods;
80 if (fread(&num_service_periods, sizeof(uint32_t), 1, fp) != 1)
81 {
82 printf("Error: Couldn't read the number of service periods.\n");
83 return;
84 }
85 for (int i=0; i < num_service_periods; i++)
86 {
87 ServicePeriod s(fp);
88 add_service_period(s);
89 }
90
91 uint32_t num_tripstops;
92 if (fread(&num_tripstops, sizeof(uint32_t), 1, fp) != 1)
93 {
94 printf("Error: Couldn't read the number of tripstops.\n");
95 return;
96 }
97
98 tripstops.reserve(num_tripstops);
99 for (uint32_t i=0; i < num_tripstops; i++)
100 {
101 shared_ptr<TripStop> s(new TripStop(fp));
102 assert(tripstops.size() == s->id);
103 tripstops.push_back(s);
104 }
105
106 fclose(fp);
107 }
108
109
110 void TripGraph::save(string fname)
111 {
112 FILE *fp = fopen(fname.c_str(), "w");
113 if (!fp)
114 {
115 printf("Error: Couldn't open graph %s for writing: %s (%d).\n",
116 fname.c_str(), strerror(errno), errno);
117 return;
118 }
119
120 // write timezone
121 uint32_t timezone_len = timezone.size();
122 assert(fwrite(&timezone_len, sizeof(uint32_t), 1, fp) == 1);
123 assert(fwrite(timezone.c_str(), sizeof(char), timezone_len, fp) ==
124 timezone_len);
125
126 // write service periods
127 uint32_t num_service_periods = splist.size();
128 assert(fwrite(&num_service_periods, sizeof(uint32_t), 1, fp) == 1);
129 for (ServicePeriodList::iterator i = splist.begin(); i != splist.end();
130 i++)
131 i->write(fp);
132
133 // write tripstops
134 uint32_t num_tripstops = tripstops.size();
135 assert(fwrite(&num_tripstops, sizeof(uint32_t), 1, fp) == 1);
136 for (TripStopList::iterator i = tripstops.begin();
137 i != tripstops.end(); i++)
138 {
139 (*i)->write(fp);
140 }
141
142 fclose(fp);
143 }
144
145
146 void TripGraph::set_timezone(std::string _timezone)
147 {
148 timezone = _timezone;
149 setenv("TZ", timezone.c_str(), 1);
150 tzset();
151 }
152
153
154 void TripGraph::add_service_period(ServicePeriod &service_period)
155 {
156 assert(service_period.id == splist.size());
157 splist.push_back(service_period);
158 }
159
160
161 void TripGraph::add_triphop(int32_t start_time, int32_t end_time,
162 int32_t src_id, int32_t dest_id, int32_t route_id,
163 int32_t trip_id, int32_t service_id)
164 {
165 // will assert if src_id doesn't exist!!
166 _get_tripstop(src_id)->add_triphop(start_time, end_time, dest_id, route_id,
167 trip_id, service_id);
168 }
169
170
171 void TripGraph::add_tripstop(int32_t id, TripStop::Type type, float lat, float lng)
172 {
173 // id must equal size of tripstops
174 assert(id == tripstops.size());
175
176 tripstops.push_back(shared_ptr<TripStop>(new TripStop(id, type, lat, lng)));
177 }
178
179
180 void TripGraph::add_walkhop(int32_t src_id, int32_t dest_id)
181 {
182 // will assert if src_id or dest_id doesn't exist!!
183 shared_ptr<TripStop> ts_src = _get_tripstop(src_id);
184 shared_ptr<TripStop> ts_dest = _get_tripstop(dest_id);
185
186 double dist = distance(ts_src->lat, ts_src->lng,
187 ts_dest->lat, ts_dest->lng);
188
189 ts_src->add_walkhop(dest_id, dist / EST_WALK_SPEED);
190 }
191
192
193 struct Point
194 {
195 Point(double _lat, double _lng) { lat=_lat; lng=_lng; }
196 double lat;
197 double lng;
198 };
199
200 bool operator==(const Point &p1, const Point &p2)
201 {
202 // We say that anything within a distance of 1 meter is identical.
203 return (distance(p1.lat, p1.lng, p2.lat, p2.lng) < 1.0f);
204 }
205
206 Point get_closest_point(Point &a, Point &b, Point &c)
207 {
208 // Given a line made up of a and b, and a point c,
209 // return the point on the line closest to c (may be a or b).
210 double ab2 = pow((b.lat - a.lat), 2) + pow((b.lng - a.lng), 2);
211 double ap_ab = (c.lat - a.lat)*(b.lat-a.lat) + (c.lng-a.lng)*(b.lng-a.lng);
212 double t = ap_ab / ab2;
213
214 // Clamp t to be between a and b.
215 if (t < 0.0f)
216 t = 0.0f;
217 else if (t>1.0f)
218 t = 1.0f;
219
220 return Point(a.lat + (b.lat - a.lat)*t, a.lng + (b.lng - a.lng)*t);
221 }
222
223
224 // This complicated-looking method attempts to link gtfs stops to osm nodes.
225 // If a stop lies between two osm nodes on a polyline, we will link the gtfs
226 // stop to both of them.
227 void TripGraph::link_osm_gtfs()
228 {
229 map<int32_t, pair<int32_t, int32_t> > new_walkhops;
230
231 // do some counting of the actual number of gtfs
232 int gtfs_tripstop_count = 0;
233 int gtfs_tripstop_total = 0;
234 for (TripStopList::iterator i = tripstops.begin();
235 i != tripstops.end(); i++)
236 {
237 if ((*i)->type == TripStop::GTFS)
238 gtfs_tripstop_total++;
239 }
240
241 for (TripStopList::iterator i = tripstops.begin();
242 i != tripstops.end(); i++)
243 {
244 gtfs_tripstop_count++;
245 // For each GTFS stop...
246 if ((*i)->type == TripStop::GTFS)
247 {
248 Point gtfs_pt((*i)->lat, (*i)->lng);
249
250 pair<int32_t, int32_t> nearest_walkhop(-1, -1);
251 double min_dist;
252
253 // Check each other trip stop and all its walkhops...
254 // FIXME: This is begging to be optimized. We need some way to
255 // exclude the bulk of tripstops that are a million miles away.
256 // One idea is to do some sort of quadtree-like partitioning of
257 // the tripstops; then we'd mostly only have to check other stops
258 // within our partition.
259 // Another idea is to put a bounding box around each tripstop and
260 // its associated walkhops, saving us from having to examine each
261 // walkhop of some faraway triphop.
262 for (TripStopList::iterator j = tripstops.begin();
263 j != tripstops.end(); j++)
264 {
265 for (TripStop::WalkHopList::iterator k = (*j)->wlist.begin();
266 k != (*j)->wlist.end(); k++)
267 {
268 Point trip_pt((*j)->lat, (*j)->lng);
269
270 shared_ptr<TripStop> dest_stop = _get_tripstop(k->dest_id);
271 Point walk_pt(dest_stop->lat, dest_stop->lng);
272
273 Point p = get_closest_point(trip_pt, walk_pt, gtfs_pt);
274
275 // Find the closest OSM hop to the GTFS stop
276 double dist = distance(gtfs_pt.lat, gtfs_pt.lng,
277 p.lat, p.lng);
278 if ((nearest_walkhop.first == (-1) &&
279 nearest_walkhop.second == (-1)) || dist < min_dist)
280 {
281 nearest_walkhop = pair<int32_t,int32_t>(-1, -1);
282 // If the GTFS stop is on one of the OSM nodes, use
283 // that node. Otherwise remember both nodes.
284 if (trip_pt == p)
285 nearest_walkhop.first = (*j)->id;
286 else if (walk_pt == p)
287 nearest_walkhop.first = k->dest_id;
288 else
289 {
290 nearest_walkhop.first = (*j)->id;
291 nearest_walkhop.second = k->dest_id;
292 }
293
294 min_dist = dist;
295 }
296 }
297 }
298
299 new_walkhops[(*i)->id] = nearest_walkhop;
300 printf("%02.2f%% done: Linking %d -> %d, %d\n",
301 ((float)gtfs_tripstop_count * 100.0f) / ((float)gtfs_tripstop_total),
302 (*i)->id,
303 nearest_walkhop.first,
304 nearest_walkhop.second);
305 }
306 }
307
308 for (map<int32_t, pair<int32_t, int32_t> >::iterator i = new_walkhops.begin();
309 i != new_walkhops.end(); i++)
310 {
311 int32_t osmstop1 = i->second.first;
312 int32_t osmstop2 = i->second.second;
313
314 assert(osmstop1 >= 0);
315 add_walkhop(i->first, osmstop1);
316 add_walkhop(osmstop1, i->first);
317
318 if (osmstop2 >= 0)
319 {
320 add_walkhop(i->first, osmstop2);
321 add_walkhop(osmstop2, i->first);
322 }
323 }
324 }
325
326
327 shared_ptr<TripStop> TripGraph::get_nearest_stop(double lat, double lng)
328 {
329 // FIXME: use a quadtree to speed this up, see link_osm_gtfs() for
330 // more thoughts on this
331
332 shared_ptr<TripStop> closest_stop;
333 double min_dist = 0.0f;
334 for (TripStopList::iterator i = tripstops.begin();
335 i != tripstops.end(); i++)
336 {
337 double dist = pow(((*i)->lat - lat), 2) + pow(((*i)->lng - lng), 2);
338 if (!closest_stop || dist < min_dist)
339 {
340 closest_stop = (*i);
341 min_dist = dist;
342 }
343 }
344
345 return closest_stop;
346 }
347
348
349 TripStop TripGraph::get_tripstop(int32_t id)
350 {
351 shared_ptr<TripStop> ts = _get_tripstop(id);
352 return TripStop(*ts);
353 }
354
355
356 vector<pair<int, int> > TripGraph::get_service_period_ids_for_time(int secs)
357 {
358 vector<pair<int, int> > vsp;
359
360 for (ServicePeriodList::iterator i = splist.begin(); i != splist.end(); i++)
361 {
362 for (int offset = 0; offset < i->duration; offset += SECS_IN_DAY)
363 {
364 time_t mysecs = secs - offset;
365 struct tm * t = localtime(&mysecs);
366 if (i->start_time <= mysecs &&
367 i->end_time >= mysecs &&
368 (((t->tm_wday == 6 && i->saturday) ||
369 (t->tm_wday == 0 && i->sunday) ||
370 (t->tm_wday > 0 && t->tm_wday < 6 && i->weekday)) &&
371 !i->is_turned_off(t->tm_mday, t->tm_mon, t->tm_year)) ||
372 i->is_turned_on(t->tm_mday, t->tm_mon, t->tm_year))
373 {
374 vsp.push_back(pair<int, int>(i->id, offset));
375 }
376 }
377 }
378
379 return vsp;
380 }
381
382
383 TripPath * TripGraph::find_path(double start, bool walkonly,
384 double src_lat, double src_lng,
385 double dest_lat, double dest_lng)
386 {
387 PathQueue uncompleted_paths;
388 PathQueue completed_paths;
389
390 VisitedRouteMap visited_routes;
391 VisitedWalkMap visited_walks;
392
393 shared_ptr<TripStop> start_node = get_nearest_stop(src_lat, src_lng);
394 shared_ptr<TripStop> end_node = get_nearest_stop(dest_lat, dest_lng);
395 DEBUGPATH("Find path. Secs: %f walkonly: %d "
396 "src lat: %f src lng: %f dest_lat: %f dest_lng: %f\n",
397 start, walkonly, src_lat, src_lng, dest_lat, dest_lng);
398 DEBUGPATH("- Start: %d End: %d\n", start_node->id, end_node->id);
399
400 //DEBUGPATH("..service period determination..");
401
402 // Consider the distance required to reach the start node from the
403 // beginning, and add that to our start time.
404 double dist_from_start = distance(src_lat, src_lng,
405 start_node->lat, start_node->lng);
406 start += (dist_from_start / EST_WALK_SPEED);
407
408 DEBUGPATH("- Start time - %f (dist from start: %f)\n", start, dist_from_start);
409 shared_ptr<TripPath> start_path(new TripPath(start, EST_WALK_SPEED,
410 end_node, start_node));
411 if (start_node == end_node)
412 return new TripPath(*start_path);
413
414 uncompleted_paths.push(start_path);
415
416 int num_paths_considered = 0;
417
418 while (uncompleted_paths.size() > 0)
419 {
420 DEBUGPATH("Continuing\n");
421 shared_ptr<TripPath> path = uncompleted_paths.top();
422 uncompleted_paths.pop();
423 extend_path(path, walkonly, end_node->id, num_paths_considered,
424 visited_routes, visited_walks, uncompleted_paths,
425 completed_paths);
426
427 // If we've still got open paths, but their weight exceeds that
428 // of the weight of a completed path, break.
429 if (uncompleted_paths.size() > 0 && completed_paths.size() > 0 &&
430 uncompleted_paths.top()->heuristic_weight >
431 completed_paths.top()->heuristic_weight)
432 {
433 DEBUGPATH("Breaking with %d uncompleted paths (paths "
434 "considered: %d).\n", uncompleted_paths.size(),
435 num_paths_considered);
436 return new TripPath(*(completed_paths.top()));
437 }
438
439 //if len(completed_paths) > 0 and len(uncompleted_paths) > 0:
440 // print "Weight of best completed path: %s, uncompleted: %s" % \
441 // (completed_paths[0].heuristic_weight, uncompleted_paths[0].heuristic_weight)
442 }
443
444 if (completed_paths.size())
445 return new TripPath(*(completed_paths.top()));
446
447 return NULL;
448 }
449
450
451 shared_ptr<TripStop> TripGraph::_get_tripstop(int32_t id)
452 {
453 assert(id < tripstops.size());
454
455 return tripstops[id];
456 }
457
458
459 void TripGraph::extend_path(shared_ptr<TripPath> &path,
460 bool walkonly,
461 int32_t goal_id,
462 int &num_paths_considered,
463 VisitedRouteMap &visited_routes,
464 VisitedWalkMap &visited_walks,
465 PathQueue &uncompleted_paths,
466 PathQueue &completed_paths)
467 {
468 TripPathList newpaths;
469 int32_t src_id = path->last_stop->id;
470 int last_route_id = path->last_route_id;
471
472 #if 0
473 if (path->last_action)
474 {
475 string last_src_id = path->last_action->src_id;
476 if (cb)
477 python::call<void>(cb, tripstops[last_src_id]->lat,
478 tripstops[last_src_id]->lng,
479 tripstops[src_id]->lat,
480 tripstops[src_id]->lng,
481 last_route_id);
482 }
483 #endif
484 time_t mysecs = (time_t)path->time;
485 struct tm * tm = localtime(&mysecs);
486 double elapsed_daysecs = tm->tm_sec + (60*tm->tm_min) + (60*60*tm->tm_hour);
487 double daystart = path->time - elapsed_daysecs;
488
489 // Figure out service period based on start time, then figure out
490 // seconds since midnight on our particular day
491 vector<pair<int, int> > vsp = get_service_period_ids_for_time(path->time);
492
493 DEBUGPATH("Extending path at vertex %d (on %d) @ %f (walktime: %f, "
494 "routetime: %f elapsed_daysecs: %f)\n", src_id, last_route_id, path->time,
495 path->walking_time, path->route_time, elapsed_daysecs);
496 shared_ptr<TripStop> src_stop = _get_tripstop(src_id);
497
498 // Keep track of outgoing route ids at this node: make sure that we
499 // don't get on a route later when we could have gotten on here.
500 deque<int> outgoing_route_ids;
501 if (!walkonly)
502 {
503 for (vector<pair<int, int> >::iterator i = vsp.begin(); i != vsp.end(); i++)
504 {
505 deque<int> route_ids = src_stop->get_routes(i->first);
506 for (deque<int>::iterator j = route_ids.begin(); j != route_ids.end(); j++)
507 outgoing_route_ids.push_back(*j);
508 }
509 }
510
511 // Explore walkhops that are better than the ones we've already visited.
512 // If we're on a bus, don't allow a transfer if we've been on for
513 // less than 5 minutes (FIXME: probably better to measure distance
514 // travelled?)
515 if (last_route_id == -1 || path->route_time > (2 * 60))
516 {
517 for (TripStop::WalkHopList::iterator i = src_stop->wlist.begin();
518 i != src_stop->wlist.end(); i++)
519 {
520 int32_t dest_id = i->dest_id;
521 double walktime = i->walktime;
522
523 // Do a quick test to make sure that the potential basis for a
524 // new path isn't worse than what we have already, before
525 // incurring the cost of creating a new path and evaluating it.
526 unordered_map<int32_t, shared_ptr<TripPath> > vsrc = visited_walks[src_id];
527 unordered_map<int32_t, shared_ptr<TripPath> >::iterator v1 = vsrc.find(dest_id);
528 if (v1 != vsrc.end() && path->heuristic_weight > v1->second->heuristic_weight)
529 continue;
530
531 shared_ptr<TripAction> action(
532 new TripAction(src_id, dest_id, -1, path->time,
533 (path->time + walktime)));
534 shared_ptr<TripStop> ds = _get_tripstop(dest_id);
535 shared_ptr<TripPath> path2 = path->add_action(
536 action, outgoing_route_ids, ds);
537
538 DEBUGPATH("- Considering walkpath to %d\n", dest_id);
539
540 if (v1 == vsrc.end() ||
541 v1->second->heuristic_weight > path2->heuristic_weight ||
542 ((v1->second->heuristic_weight - path2->heuristic_weight) < 1.0f &&
543 v1->second->walking_time > path2->walking_time))
544 {
545 DEBUGPATH("-- Adding walkpath to %d (walktime: %f (%f, %f))\n", dest_id, walktime, action->start_time, action->end_time);
546 if (dest_id == goal_id)
547 completed_paths.push(path2);
548 else
549 uncompleted_paths.push(path2);
550
551 num_paths_considered++;
552 visited_walks[src_id][dest_id] = path2;
553 }
554 }
555 }
556
557
558 // If we're doing a walkonly path (mostly for generating shapes?), stop
559 // and return here.
560 if (walkonly)
561 return;
562
563 // Find outgoing triphops from the source and get a list of paths to them.
564 for (vector<pair<int, int> >::iterator sp = vsp.begin(); sp != vsp.end();
565 sp++)
566 {
567 deque<int> route_ids = src_stop->get_routes(sp->first);
568 for (deque<int>::iterator j = route_ids.begin(); j != route_ids.end(); j++)
569 {
570 int LEEWAY = 0;
571 if ((*j) != last_route_id)
572 LEEWAY = (5*60); // give 5 mins to make a transfer
573
574 const TripHop * t = src_stop->find_triphop(
575 elapsed_daysecs + sp->second + LEEWAY, (*j), sp->first);
576 if (t)
577 {
578 // If we've been on the route before (or could have been),
579 // don't get on again.
580 if ((*j) != last_route_id && path->possible_route_ids.count(*j))
581 {
582 // pass
583 }
584 // Disallow more than three transfers.
585 else if ((*j) != last_route_id &&
586 path->traversed_route_ids > 3)
587 {
588 // pass
589 }
590 else
591 {
592 // Do a quick test to make sure that the potential basis for a
593 // new path isn't worse than what we have already, before
594 // incurring the cost of creating a new path and evaluating it.
595 unordered_map<int, shared_ptr<TripPath> >::iterator v = visited_routes[src_id].find(*j);
596 if (v != visited_routes[src_id].end() && path->heuristic_weight > v->second->heuristic_weight)
597 continue;
598
599 shared_ptr<TripAction> action = shared_ptr<TripAction>(
600 new TripAction(src_id, t->dest_id, (*j), daystart + t->start_time,
601 daystart + t->end_time));
602 shared_ptr<TripStop> ds = _get_tripstop(t->dest_id);
603 shared_ptr<TripPath> path2 = path->add_action(
604 action, outgoing_route_ids, ds);
605
606
607 if (v == visited_routes[src_id].end() ||
608 v->second->heuristic_weight > path2->heuristic_weight ||
609 ((v->second->heuristic_weight - path2->heuristic_weight) < 1.0f &&
610 v->second->walking_time > path2->walking_time))
611 {
612 if (t->dest_id == goal_id)
613 completed_paths.push(path2);
614 else
615 uncompleted_paths.push(path2);
616
617 num_paths_considered++;
618 visited_routes[src_id][(*j)] = path2;
619 }
620 }
621 }
622 }
623 }
624 }
625
626
627 vector<TripStop> TripGraph::find_tripstops_in_range(double lat, double lng,
628 TripStop::Type type,
629 double range)
630 {
631 vector<TripStop> tripstops_in_range;
632
633 for (TripStopList::iterator i = tripstops.begin();
634 i != tripstops.end(); i++)
635 {
636 if ((*i)->type != type)
637 continue;
638
639 double dist = distance((*i)->lat, (*i)->lng, lat, lng);
640 if (dist <= range)
641 tripstops_in_range.push_back(*(*i));
642 }
643
644 return tripstops_in_range;
645 }
646