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