| 1 | #include "trippath.h" | ||
| 2 | #include <math.h> | ||
| 3 | |||
| 4 | #if 0 | ||
| 5 | #define LOG(...) fprintf(stderr, __VA_ARGS__) | ||
| 6 | #else | ||
| 7 | #define LOG(...) | ||
| 8 | #endif | ||
| 9 | |||
| 10 | using namespace std; | ||
| 11 | using namespace tr1; | ||
| 12 | |||
| 13 | static inline double radians(double degrees) | ||
| 14 | { | ||
| 15 | return degrees/180.0f*M_PI; | ||
| 16 | } | ||
| 17 | |||
| 18 | static inline double degrees(double radians) | ||
| 19 | { | ||
| 20 | return radians*180.0f/M_PI; | ||
| 21 | } | ||
| 22 | |||
| 23 | static double distance(double src_lat, double src_lng, double dest_lat, double dest_lng) | ||
| 24 | { | ||
| 25 | if (src_lat == dest_lat && src_lng == dest_lng) | ||
| 26 | return 0.0f; | ||
| 27 | |||
| 28 | double theta = src_lng - dest_lng; | ||
| 29 | double src_lat_radians = radians(src_lat); | ||
| 30 | double dest_lat_radians = radians(dest_lat); | ||
| 31 | double dist = sin(src_lat_radians) * sin(dest_lat_radians) + | ||
| 32 | cos(src_lat_radians) * cos(dest_lat_radians) * | ||
| 33 | cos(radians(theta)); | ||
| 34 | dist = acos(dist); | ||
| 35 | dist = degrees(dist); | ||
| 36 | dist *= (60.0f * 1.1515 * 1.609344 * 1000.0f); | ||
| 37 | return dist; | ||
| 38 | } | ||
| 39 | |||
| 40 | |||
| 41 | TripAction::TripAction(int32_t _src_id, int32_t _dest_id, | ||
| 42 | int _route_id, double _start_time, double _end_time) : | ||
| 43 | src_id(_src_id), | ||
| 44 | dest_id(_dest_id), | ||
| 45 | route_id(_route_id), | ||
| 46 | start_time(_start_time), | ||
| 47 | end_time(_end_time), | ||
| 48 | parent() | ||
| 49 | { | ||
| 50 | } | ||
| 51 | |||
| 52 | |||
| 53 | TripAction::TripAction(const TripAction &other): | ||
| 54 | src_id(other.src_id), | ||
| 55 | dest_id(other.dest_id), | ||
| 56 | route_id(other.route_id), | ||
| 57 | start_time(other.start_time), | ||
| 58 | end_time(other.end_time), | ||
| 59 | parent(other.parent) | ||
| 60 | { | ||
| 61 | } | ||
| 62 | |||
| 63 | |||
| 64 | TripAction& TripAction::operator=(const TripAction &other) | ||
| 65 | { | ||
| 66 | src_id = other.src_id; | ||
| 67 | dest_id = other.dest_id; | ||
| 68 | route_id = other.route_id; | ||
| 69 | start_time = other.start_time; | ||
| 70 | end_time = other.end_time; | ||
| 71 | parent = other.parent; | ||
| 72 | } | ||
| 73 | |||
| 74 | TripPath::TripPath(double _time, double _fastest_speed, | ||
| 75 | shared_ptr<TripStop> &_dest_stop, | ||
| 76 | shared_ptr<TripStop> &_last_stop) | ||
| 77 | { | ||
| 78 | fastest_speed = _fastest_speed; | ||
| 79 | dest_stop = _dest_stop; | ||
| 80 | last_stop = _last_stop; | ||
| 81 | time = _time; | ||
| 82 | |||
| 83 | walking_time = 0.0f; | ||
| 84 | weight = _time; | ||
| 85 | traversed_route_ids = 0; | ||
| 86 | last_route_id = -1; | ||
| 87 | route_time = 0.0f; | ||
| 88 | _get_heuristic_weight(); | ||
| 89 | } | ||
| 90 | |||
| 91 | #if 0 | ||
| 92 | python::object TripPath::get_last_action() | ||
| 93 | { | ||
| 94 | if (last_action) | ||
| 95 | return python::object(*last_action); | ||
| 96 | |||
| 97 | return python::object(); | ||
| 98 | } | ||
| 99 | #endif | ||
| 100 | |||
| 101 | void TripPath::_get_heuristic_weight() | ||
| 102 | { | ||
| 103 | // start off with heuristic weight being equivalent to its real weight | ||
| 104 | heuristic_weight = weight; | ||
| 105 | |||
| 106 | // then, calculate the time remaining based on going directly | ||
| 107 | // from the last vertex to the destination vertex at the fastest | ||
| 108 | // possible speed in the graph | ||
| 109 | double remaining_distance = distance(last_stop->lat, last_stop->lng, | ||
| 110 | dest_stop->lat, dest_stop->lng); | ||
| 111 | heuristic_weight += remaining_distance / 5; //(fastest_speed / 3); | ||
| 112 | |||
| 113 | // now, add 5 minutes per each transfer, multiplied to the power of 2 | ||
| 114 | // (to make transfers exponentially more painful) | ||
| 115 | if (traversed_route_ids > 1) | ||
| 116 | heuristic_weight += (pow(2.0f, (int)(traversed_route_ids-2)) * 5.0f * 60.0f); | ||
| 117 | |||
| 118 | // double the cost of walking after 5 mins, quadruple after 10 mins, | ||
| 119 | // octuple after 15, etc. (up to a maximum of 20 iterations of this, to | ||
| 120 | // make sure we don't freeze for particularly long walking times-- mostly | ||
| 121 | // useful for obscure test cases) | ||
| 122 | double excess_walking_time = walking_time - 300.0f; | ||
| 123 | int iter = 0; | ||
| 124 | while (excess_walking_time > 0 && iter < 20) | ||
| 125 | { | ||
| 126 | double iter_walking_time = 0; | ||
| 127 | if (excess_walking_time > 300.0f) | ||
| 128 | iter_walking_time = 300.0f; | ||
| 129 | else | ||
| 130 | iter_walking_time = excess_walking_time; | ||
| 131 | heuristic_weight += (iter_walking_time * pow(2.0f, iter)); | ||
| 132 | excess_walking_time -= 300.0f; | ||
| 133 | iter++; | ||
| 134 | } | ||
| 135 | |||
| 136 | // add 5 mins to our weight if we were walking and remaining distance | ||
| 137 | // >1000m, to account for the fact that we're probably going to | ||
| 138 | // want to wait for another bus. this prevents us from repeatedly | ||
| 139 | // getting out of the bus and walking around | ||
| 140 | if (last_route_id == -1 && remaining_distance > 1000) | ||
| 141 | heuristic_weight += (5*60); | ||
| 142 | } | ||
| 143 | |||
| 144 | static void _add_actions_to_list(deque<TripAction> &l, | ||
| 145 | shared_ptr<TripAction> &action) | ||
| 146 | { | ||
| 147 | if (action) | ||
| 148 | { | ||
| 149 | if (action->parent) | ||
| 150 | _add_actions_to_list(l, action->parent); | ||
| 151 | l.push_back(TripAction(*action)); | ||
| 152 | } | ||
| 153 | } | ||
| 154 | |||
| 155 | deque<TripAction> TripPath::get_actions() | ||
| 156 | { | ||
| 157 | deque<TripAction> l; | ||
| 158 | |||
| 159 | // recursively add actions to list, so we get them back in the | ||
| 160 | // correct order | ||
| 161 | _add_actions_to_list(l, last_action); | ||
| 162 | |||
| 163 | return l; | ||
| 164 | } | ||
| 165 | |||
| 166 | shared_ptr<TripPath> TripPath::add_action(shared_ptr<TripAction> &action, | ||
| 167 | deque<int> &_possible_route_ids, | ||
| 168 | shared_ptr<TripStop> &_last_stop) | ||
| 169 | { | ||
| 170 | shared_ptr<TripPath> new_trippath(new TripPath(*this)); | ||
| 171 | |||
| 172 | float departure_delay = 0.0f; | ||
| 173 | |||
| 174 | if (action->route_id == -1) | ||
| 175 | { | ||
| 176 | new_trippath->walking_time += (action->end_time - action->start_time); | ||
| 177 | new_trippath->route_time = 0; | ||
| 178 | } | ||
| 179 | else if (new_trippath->last_action) | ||
| 180 | { | ||
| 181 | // Starting first bus route, adjust the start time to match. | ||
| 182 | if (new_trippath->traversed_route_ids == 0) | ||
| 183 | { | ||
| 184 | departure_delay = | ||
| 185 | action->start_time - new_trippath->last_action->end_time; | ||
| 186 | // Aim to be at the bus stop 3 minutes early. | ||
| 187 | departure_delay -= 3*60; | ||
| 188 | } | ||
| 189 | |||
| 190 | if (action->route_id != new_trippath->last_action->route_id) | ||
| 191 | { | ||
| 192 | new_trippath->traversed_route_ids++; | ||
| 193 | new_trippath->route_time = 0; | ||
| 194 | } | ||
| 195 | } | ||
| 196 | |||
| 197 | for (deque<int>::iterator i = _possible_route_ids.begin(); | ||
| 198 | i != _possible_route_ids.end(); i++) | ||
| 199 | { | ||
| 200 | new_trippath->possible_route_ids.insert(*i); | ||
| 201 | } | ||
| 202 | |||
| 203 | new_trippath->route_time += (action->end_time - action->start_time); | ||
| 204 | new_trippath->weight += (action->end_time - action->start_time); | ||
| 205 | new_trippath->weight += (action->start_time - time); | ||
| 206 | |||
| 207 | if (new_trippath->last_action) | ||
| 208 | action->parent = new_trippath->last_action; | ||
| 209 | new_trippath->last_action = shared_ptr<TripAction>(new TripAction(*action)); | ||
| 210 | new_trippath->last_stop = _last_stop; | ||
| 211 | new_trippath->last_route_id = action->route_id; | ||
| 212 | new_trippath->_get_heuristic_weight(); | ||
| 213 | new_trippath->time = action->end_time; | ||
| 214 | |||
| 215 | if (departure_delay > 0.0f) | ||
| 216 | { | ||
| 217 | LOG("Delaying start by %f seconds\n", departure_delay); | ||
| 218 | new_trippath->delay_walk(new_trippath->last_action, departure_delay); | ||
| 219 | } | ||
| 220 | |||
| 221 | return new_trippath; | ||
| 222 | } | ||
| 223 | |||
| 224 | |||
| 225 | void TripPath::delay_walk(shared_ptr<TripAction> walk, float secs) | ||
| 226 | { | ||
| 227 | if (!walk) | ||
| 228 | return; | ||
| 229 | |||
| 230 | // Don't delay partial walks; we need to be given the element *after* | ||
| 231 | // the final walk. | ||
| 232 | if (walk->route_id == -1) | ||
| 233 | return; | ||
| 234 | |||
| 235 | // Only delay actual walks. | ||
| 236 | if (!walk->parent || walk->parent->route_id != -1) | ||
| 237 | return; | ||
| 238 | |||
| 239 | shared_ptr<TripAction> w(walk); | ||
| 240 | while (w && w->parent && w->parent->route_id == -1) | ||
| 241 | { | ||
| 242 | // We need to clone the actions, as they're no longer safe to share | ||
| 243 | // (for instance, they could be shared by another bus trip that leaves | ||
| 244 | // earlier). | ||
| 245 | w->parent = shared_ptr<TripAction>(new TripAction(*(w->parent))); | ||
| 246 | w = w->parent; | ||
| 247 | |||
| 248 | w->start_time += secs; | ||
| 249 | w->end_time += secs; | ||
| 250 | } | ||
| 251 | |||
| 252 | // If we delayed the initial walk, then we've reduced the total trip time. | ||
| 253 | if (!w) | ||
| 254 | { | ||
| 255 | weight -= secs; | ||
| 256 | _get_heuristic_weight(); | ||
| 257 | } | ||
| 258 | } | ||
| 259 | |||
| 260 |