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