|
|
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 |
|