… | |
… | |
62 | struct street_data *street; |
62 | struct street_data *street; |
63 | struct tracking_line *next; |
63 | struct tracking_line *next; |
64 | int angle[0]; |
64 | int angle[0]; |
65 | }; |
65 | }; |
66 | |
66 | |
67 | |
|
|
68 | /** |
67 | /** |
69 | * @brief Conatins a list of previous speeds |
68 | * @brief Conatins a list of previous speeds |
70 | * |
69 | * |
71 | * This structure is used to hold a list of previously reported |
70 | * This structure is used to hold a list of previously reported |
72 | * speeds. This data is used by the CDF. |
71 | * speeds. This data is used by the CDF. |
73 | */ |
72 | */ |
74 | struct cdf_speed { |
73 | struct cdf_speed |
|
|
74 | { |
75 | struct cdf_speed *next; |
75 | struct cdf_speed *next; |
76 | int speed; |
76 | int speed; |
77 | time_t time; |
77 | time_t time; |
78 | }; |
78 | }; |
79 | |
79 | |
… | |
… | |
81 | * @brief Contains data for the CDF |
81 | * @brief Contains data for the CDF |
82 | * |
82 | * |
83 | * This structure holds all data needed by the |
83 | * This structure holds all data needed by the |
84 | * cumulative displacement filter. |
84 | * cumulative displacement filter. |
85 | */ |
85 | */ |
86 | struct cdf_data { |
86 | struct cdf_data |
|
|
87 | { |
87 | int extrapolating; |
88 | int extrapolating; |
88 | int available; |
89 | int available; |
89 | int first_pos; |
90 | int first_pos; |
90 | int poscount; |
91 | int poscount; |
91 | int hist_size; |
92 | int hist_size; |
92 | struct cdf_speed *speed_hist; |
93 | struct cdf_speed *speed_hist; |
93 | struct pcoord *pos_hist; |
94 | struct pcoord *pos_hist; |
94 | int *dir_hist; |
95 | int *dir_hist; |
95 | double last_dist; |
96 | double last_dist; |
96 | struct pcoord last_out; |
97 | struct pcoord last_out; |
97 | int last_dir; |
98 | int last_dir; |
98 | }; |
99 | }; |
99 | |
100 | |
100 | struct tracking { |
101 | struct tracking |
|
|
102 | { |
101 | struct mapset *ms; |
103 | struct mapset *ms; |
102 | struct route *rt; |
104 | struct route *rt; |
103 | struct map *map; |
105 | struct map *map; |
104 | struct vehicle *vehicle; |
106 | struct vehicle *vehicle; |
105 | struct vehicleprofile *vehicleprofile; |
107 | struct vehicleprofile *vehicleprofile; |
… | |
… | |
130 | int overspeed_pref; |
132 | int overspeed_pref; |
131 | int overspeed_percent_pref; |
133 | int overspeed_percent_pref; |
132 | int tunnel_extrapolation; |
134 | int tunnel_extrapolation; |
133 | }; |
135 | }; |
134 | |
136 | |
135 | |
|
|
136 | |
|
|
137 | |
|
|
138 | static void |
|
|
139 | tracking_init_cdf(struct cdf_data *cdf, int hist_size) |
137 | static void tracking_init_cdf(struct cdf_data *cdf, int hist_size) |
140 | { |
138 | { |
141 | cdf->extrapolating = 0; |
139 | cdf->extrapolating = 0; |
142 | cdf->available = 0; |
140 | cdf->available = 0; |
143 | cdf->poscount = 0; |
141 | cdf->poscount = 0; |
144 | cdf->last_dist = 0; |
142 | cdf->last_dist = 0; |
145 | cdf->hist_size = hist_size; |
143 | cdf->hist_size = hist_size; |
146 | |
144 | |
147 | cdf->pos_hist = g_new0(struct pcoord, hist_size); |
145 | cdf->pos_hist = g_new0(struct pcoord, hist_size); |
148 | cdf->dir_hist = g_new0(int, hist_size); |
146 | cdf->dir_hist = g_new0(int, hist_size); |
149 | } |
147 | } |
150 | |
148 | |
151 | // Variables for finetuning the CDF |
149 | // Variables for finetuning the CDF |
152 | |
150 | |
153 | // Minimum average speed |
151 | // Minimum average speed |
154 | #define CDF_MINAVG 1.f |
152 | #define CDF_MINAVG 1.f |
155 | // Maximum average speed |
153 | // Maximum average speed |
156 | #define CDF_MAXAVG 6.f // only ~ 20 km/h |
154 | #define CDF_MAXAVG 6.f // only ~ 20 km/h |
157 | // We need a low value here because otherwise we would extrapolate whenever we are not accelerating |
155 | // We need a low value here because otherwise we would extrapolate whenever we are not accelerating |
158 | |
156 | |
159 | // Mininum distance (square of it..), below which we ignore gps updates |
157 | // Mininum distance (square of it..), below which we ignore gps updates |
160 | #define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians. |
158 | #define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians. |
161 | |
|
|
162 | #if 0 |
159 | #if 0 |
163 | static void |
160 | static void |
164 | tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout, int cur_speed, time_t fixtime) |
161 | tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout, int cur_speed, time_t fixtime) |
165 | { |
162 | { |
166 | struct cdf_speed *speed,*sc,*sl; |
163 | struct cdf_speed *speed,*sc,*sl; |
167 | double speed_avg; |
164 | double speed_avg; |
168 | int speed_num,i; |
165 | int speed_num,i; |
169 | |
166 | |
170 | if (cdf->hist_size == 0) { |
167 | if (cdf->hist_size == 0) |
|
|
168 | { |
171 | dbg(1,"No CDF.\n"); |
169 | dbg(1,"No CDF.\n"); |
172 | *pout = *pin; |
170 | *pout = *pin; |
173 | *dirout = dirin; |
171 | *dirout = dirin; |
174 | return; |
172 | return; |
175 | } |
173 | } |
176 | |
174 | |
177 | speed = g_new0(struct cdf_speed, 1); |
175 | speed = g_new0(struct cdf_speed, 1); |
178 | speed->speed = cur_speed; |
176 | speed->speed = cur_speed; |
179 | speed->time = fixtime; |
177 | speed->time = fixtime; |
180 | |
178 | |
181 | speed->next = cdf->speed_hist; |
179 | speed->next = cdf->speed_hist; |
182 | cdf->speed_hist = speed; |
180 | cdf->speed_hist = speed; |
183 | |
181 | |
184 | sc = speed; |
182 | sc = speed; |
185 | sl = NULL; |
183 | sl = NULL; |
186 | speed_num = 0; |
184 | speed_num = 0; |
187 | speed_avg = 0; |
185 | speed_avg = 0; |
188 | while (sc && ((fixtime - speed->time) < 4)) { // FIXME static maxtime |
186 | while (sc && ((fixtime - speed->time) < 4)) |
|
|
187 | { // FIXME static maxtime |
189 | speed_num++; |
188 | speed_num++; |
190 | speed_avg += sc->speed; |
189 | speed_avg += sc->speed; |
191 | sl = sc; |
190 | sl = sc; |
192 | sc = sc->next; |
191 | sc = sc->next; |
193 | } |
192 | } |
194 | |
193 | |
195 | speed_avg /= (double)speed_num; |
194 | speed_avg /= (double)speed_num; |
196 | |
195 | |
197 | if (sl) { |
196 | if (sl) |
|
|
197 | { |
198 | sl->next = NULL; |
198 | sl->next = NULL; |
199 | } |
199 | } |
200 | |
200 | |
201 | while (sc) { |
201 | while (sc) |
|
|
202 | { |
202 | sl = sc->next; |
203 | sl = sc->next; |
203 | g_free(sc); |
204 | g_free(sc); |
204 | sc = sl; |
205 | sc = sl; |
205 | } |
206 | } |
206 | |
207 | |
207 | if (speed_avg < CDF_MINAVG) { |
208 | if (speed_avg < CDF_MINAVG) |
|
|
209 | { |
208 | speed_avg = CDF_MINAVG; |
210 | speed_avg = CDF_MINAVG; |
|
|
211 | } |
209 | } else if (speed_avg > CDF_MAXAVG) { |
212 | else if (speed_avg > CDF_MAXAVG) |
|
|
213 | { |
210 | speed_avg = CDF_MAXAVG; |
214 | speed_avg = CDF_MAXAVG; |
211 | } |
215 | } |
212 | |
216 | |
213 | |
|
|
214 | if (cur_speed >= speed_avg) { |
217 | if (cur_speed >= speed_avg) |
|
|
218 | { |
215 | if (cdf->extrapolating) { |
219 | if (cdf->extrapolating) |
|
|
220 | { |
216 | cdf->poscount = 0; |
221 | cdf->poscount = 0; |
217 | cdf->extrapolating = 0; |
222 | cdf->extrapolating = 0; |
|
|
223 | } |
|
|
224 | |
|
|
225 | cdf->first_pos--; |
|
|
226 | if (cdf->first_pos < 0) |
|
|
227 | { |
|
|
228 | cdf->first_pos = cdf->hist_size - 1; |
|
|
229 | } |
|
|
230 | |
|
|
231 | if (cdf->poscount < cdf->hist_size) |
|
|
232 | { |
|
|
233 | cdf->poscount++; |
|
|
234 | } |
|
|
235 | |
|
|
236 | cdf->pos_hist[cdf->first_pos] = *pin; |
|
|
237 | cdf->dir_hist[cdf->first_pos] = dirin; |
|
|
238 | |
|
|
239 | *pout = *pin; |
|
|
240 | *dirout = dirin; |
|
|
241 | } |
|
|
242 | else if (cdf->poscount > 0) |
|
|
243 | { |
|
|
244 | |
|
|
245 | double mx,my; // Average position's x and y values |
|
|
246 | double sx,sy; // Support vector |
|
|
247 | double dx,dy; // Difference between average and current position |
|
|
248 | double len; // Length of support vector |
|
|
249 | double dist; |
|
|
250 | |
|
|
251 | mx = my = 0; |
|
|
252 | sx = sy = 0; |
|
|
253 | |
|
|
254 | for (i = 0; i < cdf->poscount; i++) |
|
|
255 | { |
|
|
256 | mx += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x / cdf->poscount; |
|
|
257 | my += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y / cdf->poscount; |
|
|
258 | |
|
|
259 | if (i != 0) |
|
|
260 | { |
|
|
261 | sx += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].x; |
|
|
262 | sy += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].y; |
218 | } |
263 | } |
219 | |
264 | |
220 | cdf->first_pos--; |
|
|
221 | if (cdf->first_pos < 0) { |
|
|
222 | cdf->first_pos = cdf->hist_size - 1; |
|
|
223 | } |
265 | } |
224 | |
266 | |
225 | if (cdf->poscount < cdf->hist_size) { |
|
|
226 | cdf->poscount++; |
|
|
227 | } |
|
|
228 | |
|
|
229 | cdf->pos_hist[cdf->first_pos] = *pin; |
|
|
230 | cdf->dir_hist[cdf->first_pos] = dirin; |
|
|
231 | |
|
|
232 | *pout = *pin; |
|
|
233 | *dirout = dirin; |
|
|
234 | } else if (cdf->poscount > 0) { |
|
|
235 | |
|
|
236 | double mx,my; // Average position's x and y values |
|
|
237 | double sx,sy; // Support vector |
|
|
238 | double dx,dy; // Difference between average and current position |
|
|
239 | double len; // Length of support vector |
|
|
240 | double dist; |
|
|
241 | |
|
|
242 | mx = my = 0; |
|
|
243 | sx = sy = 0; |
|
|
244 | |
|
|
245 | for (i = 0; i < cdf->poscount; i++) { |
|
|
246 | mx += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x / cdf->poscount; |
|
|
247 | my += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y / cdf->poscount; |
|
|
248 | |
|
|
249 | |
|
|
250 | if (i != 0) { |
|
|
251 | sx += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].x; |
|
|
252 | sy += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].y; |
|
|
253 | } |
|
|
254 | |
|
|
255 | } |
|
|
256 | |
|
|
257 | if (cdf->poscount > 1) { |
267 | if (cdf->poscount > 1) |
|
|
268 | { |
258 | // Normalize the support vector |
269 | // Normalize the support vector |
259 | len = sqrt(sx * sx + sy * sy); |
270 | len = sqrt(sx * sx + sy * sy); |
260 | sx /= len; |
271 | sx /= len; |
261 | sy /= len; |
272 | sy /= len; |
262 | |
273 | |
263 | // Calculate the new direction |
274 | // Calculate the new direction |
264 | *dirout = (int)rint(atan(sx / sy) / M_PI * 180 + 180); |
275 | *dirout = (int)rint(atan(sx / sy) / M_PI * 180 + 180); |
265 | } else { |
276 | } |
|
|
277 | else |
|
|
278 | { |
266 | // If we only have one position, we can't use differences of positions, but we have to use the reported |
279 | // If we only have one position, we can't use differences of positions, but we have to use the reported |
267 | // direction of that position |
280 | // direction of that position |
268 | sx = sin((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI); |
281 | sx = sin((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI); |
269 | sy = cos((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI); |
282 | sy = cos((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI); |
270 | *dirout = cdf->dir_hist[cdf->first_pos]; |
283 | *dirout = cdf->dir_hist[cdf->first_pos]; |
271 | } |
284 | } |
272 | |
285 | |
273 | |
|
|
274 | dx = pin->x - mx; |
286 | dx = pin->x - mx; |
275 | dy = pin->y - my; |
287 | dy = pin->y - my; |
276 | dist = dx * sx + dy * sy; |
288 | dist = dx * sx + dy * sy; |
277 | |
289 | |
278 | if (cdf->extrapolating && (dist < cdf->last_dist)) { |
290 | if (cdf->extrapolating && (dist < cdf->last_dist)) |
|
|
291 | { |
279 | dist = cdf->last_dist; |
292 | dist = cdf->last_dist; |
280 | } |
293 | } |
281 | |
294 | |
282 | cdf->last_dist = dist; |
295 | cdf->last_dist = dist; |
283 | cdf->extrapolating = 1; |
296 | cdf->extrapolating = 1; |
284 | |
297 | |
285 | pout->x = (int)rint(mx + sx * dist); |
298 | pout->x = (int)rint(mx + sx * dist); |
286 | pout->y = (int)rint(my + sy * dist); |
299 | pout->y = (int)rint(my + sy * dist); |
287 | pout->pro = pin->pro; |
300 | pout->pro = pin->pro; |
288 | |
301 | |
289 | } else { |
302 | } |
|
|
303 | else |
|
|
304 | { |
290 | // We should extrapolate, but don't have an old position available |
305 | // We should extrapolate, but don't have an old position available |
291 | *pout = *pin; |
306 | *pout = *pin; |
292 | *dirout = dirin; |
307 | *dirout = dirin; |
293 | } |
308 | } |
294 | |
309 | |
295 | if (cdf->available) { |
310 | if (cdf->available) |
|
|
311 | { |
296 | int dx,dy; |
312 | int dx,dy; |
297 | |
313 | |
298 | dx = pout->x - cdf->last_out.x; |
314 | dx = pout->x - cdf->last_out.x; |
299 | dy = pout->y - cdf->last_out.y; |
315 | dy = pout->y - cdf->last_out.y; |
300 | |
316 | |
301 | if ((dx*dx + dy*dy) < CDF_MINDIST) { |
317 | if ((dx*dx + dy*dy) < CDF_MINDIST) |
|
|
318 | { |
302 | *pout = cdf->last_out; |
319 | *pout = cdf->last_out; |
303 | *dirout = cdf->last_dir; |
320 | *dirout = cdf->last_dir; |
304 | } |
321 | } |
305 | } |
322 | } |
306 | |
323 | |
307 | cdf->last_out = *pout; |
324 | cdf->last_out = *pout; |
308 | cdf->last_dir = *dirout; |
325 | cdf->last_dir = *dirout; |
309 | |
326 | |
310 | cdf->available = 1; |
327 | cdf->available = 1; |
311 | } |
328 | } |
312 | #endif |
329 | #endif |
313 | |
330 | |
314 | int |
|
|
315 | tracking_get_angle(struct tracking *tr) |
331 | int tracking_get_angle(struct tracking *tr) |
316 | { |
332 | { |
317 | return tr->curr_angle; |
333 | return tr->curr_angle; |
318 | } |
334 | } |
319 | |
335 | |
320 | struct coord * |
336 | struct coord * |
321 | tracking_get_pos(struct tracking *tr) |
337 | tracking_get_pos(struct tracking *tr) |
322 | { |
338 | { |
323 | return &tr->curr_out; |
339 | return &tr->curr_out; |
324 | } |
340 | } |
325 | |
341 | |
326 | int |
|
|
327 | tracking_get_street_direction(struct tracking *tr) |
342 | int tracking_get_street_direction(struct tracking *tr) |
328 | { |
343 | { |
329 | return tr->street_direction; |
344 | return tr->street_direction; |
330 | } |
345 | } |
331 | |
346 | |
332 | int |
|
|
333 | tracking_get_segment_pos(struct tracking *tr) |
347 | int tracking_get_segment_pos(struct tracking *tr) |
334 | { |
348 | { |
335 | return tr->pos; |
349 | return tr->pos; |
336 | } |
350 | } |
337 | |
351 | |
338 | struct street_data * |
352 | struct street_data * |
… | |
… | |
341 | if (tr->curr_line) |
355 | if (tr->curr_line) |
342 | return tr->curr_line->street; |
356 | return tr->curr_line->street; |
343 | return NULL; |
357 | return NULL; |
344 | } |
358 | } |
345 | |
359 | |
346 | int |
|
|
347 | tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter) |
360 | int tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter) |
348 | { |
361 | { |
349 | struct item *item; |
362 | struct item *item; |
350 | struct map_rect *mr; |
363 | struct map_rect *mr; |
351 | int result=0; |
364 | int result = 0; |
352 | // dbg(1,"enter %s\n",attr_to_name(type)); |
365 | // dbg(1,"enter %s\n",attr_to_name(type)); |
353 | |
366 | |
354 | if (_this->attr) |
367 | if (_this->attr) |
355 | { |
368 | { |
356 | attr_free(_this->attr); |
369 | attr_free(_this->attr); |
357 | _this->attr=NULL; |
370 | _this->attr = NULL; |
358 | } |
371 | } |
359 | |
372 | |
360 | switch (type) |
373 | switch (type) |
361 | { |
374 | { |
362 | case attr_position_valid: |
375 | case attr_position_valid: |
363 | attr->u.num=_this->valid; |
376 | attr->u.num = _this->valid; |
364 | return 1; |
377 | return 1; |
365 | case attr_position_direction: |
378 | case attr_position_direction: |
366 | attr->u.numd=&_this->direction; |
379 | attr->u.numd = &_this->direction; |
367 | return 1; |
380 | return 1; |
368 | case attr_position_speed: |
381 | case attr_position_speed: |
369 | attr->u.numd=&_this->speed; |
382 | attr->u.numd = &_this->speed; |
370 | return 1; |
383 | return 1; |
371 | case attr_directed: |
384 | case attr_directed: |
372 | attr->u.num=_this->street_direction; |
385 | attr->u.num = _this->street_direction; |
373 | return 1; |
386 | return 1; |
374 | case attr_position_coord_geo: |
387 | case attr_position_coord_geo: |
375 | if (!_this->coord_geo_valid) |
388 | if (!_this->coord_geo_valid) |
376 | { |
389 | { |
377 | struct coord c; |
390 | struct coord c; |
378 | c.x=_this->curr_out.x; |
391 | c.x = _this->curr_out.x; |
379 | c.y=_this->curr_out.y; |
392 | c.y = _this->curr_out.y; |
380 | transform_to_geo(_this->pro, &c, &_this->coord_geo); |
393 | transform_to_geo(_this->pro, &c, &_this->coord_geo); |
381 | _this->coord_geo_valid=1; |
394 | _this->coord_geo_valid = 1; |
382 | } |
395 | } |
383 | attr->u.coord_geo=&_this->coord_geo; |
396 | attr->u.coord_geo = &_this->coord_geo; |
384 | return 1; |
397 | return 1; |
385 | case attr_current_item: |
398 | case attr_current_item: |
386 | if (! _this->curr_line || ! _this->curr_line->street) |
399 | if (!_this->curr_line || !_this->curr_line->street) |
387 | return 0; |
400 | return 0; |
388 | attr->u.item=&_this->curr_line->street->item; |
401 | attr->u.item = &_this->curr_line->street->item; |
389 | return 1; |
402 | return 1; |
390 | default: |
403 | default: |
391 | if (! _this->curr_line || ! _this->curr_line->street) |
404 | if (!_this->curr_line || !_this->curr_line->street) |
392 | return 0; |
405 | return 0; |
393 | item=&_this->curr_line->street->item; |
406 | item = &_this->curr_line->street->item; |
394 | mr=map_rect_new(item->map,NULL); |
407 | mr = map_rect_new(item->map, NULL); |
395 | item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo); |
408 | item = map_rect_get_item_byid(mr, item->id_hi, item->id_lo); |
396 | if (item_attr_get(item, type, attr)) |
409 | if (item_attr_get(item, type, attr)) |
397 | { |
410 | { |
398 | _this->attr=attr_dup(attr); |
411 | _this->attr = attr_dup(attr); |
399 | *attr=*_this->attr; |
412 | *attr = *_this->attr; |
400 | result=1; |
413 | result = 1; |
401 | } |
414 | } |
402 | map_rect_destroy(mr); |
415 | map_rect_destroy(mr); |
403 | return result; |
416 | return result; |
404 | } |
417 | } |
405 | } |
418 | } |
406 | |
419 | |
407 | struct item * |
420 | struct item * |
408 | tracking_get_current_item(struct tracking *_this) |
421 | tracking_get_current_item(struct tracking *_this) |
409 | { |
422 | { |
410 | if (! _this->curr_line || ! _this->curr_line->street) |
423 | if (!_this->curr_line || !_this->curr_line->street) |
411 | return NULL; |
424 | return NULL; |
412 | return &_this->curr_line->street->item; |
425 | return &_this->curr_line->street->item; |
413 | } |
426 | } |
414 | |
427 | |
415 | int * |
428 | int * |
416 | tracking_get_current_flags(struct tracking *_this) |
429 | tracking_get_current_flags(struct tracking *_this) |
417 | { |
430 | { |
418 | if (! _this->curr_line || ! _this->curr_line->street) |
431 | if (!_this->curr_line || !_this->curr_line->street) |
419 | return NULL; |
432 | return NULL; |
420 | return &_this->curr_line->street->flags; |
433 | return &_this->curr_line->street->flags; |
421 | } |
434 | } |
422 | |
435 | |
423 | static void |
|
|
424 | tracking_get_angles(struct tracking_line *tl) |
436 | static void tracking_get_angles(struct tracking_line *tl) |
425 | { |
437 | { |
426 | int i; |
438 | int i; |
427 | struct street_data *sd=tl->street; |
439 | struct street_data *sd = tl->street; |
428 | for (i = 0 ; i < sd->count-1 ; i++) |
440 | for (i = 0; i < sd->count - 1; i++) |
429 | tl->angle[i]=transform_get_angle_delta(&sd->c[i], &sd->c[i+1], 0); |
441 | tl->angle[i] = transform_get_angle_delta(&sd->c[i], &sd->c[i + 1], 0); |
430 | } |
442 | } |
431 | |
443 | |
432 | static int |
|
|
433 | street_data_within_selection(struct street_data *sd, struct map_selection *sel) |
444 | static int street_data_within_selection(struct street_data *sd, struct map_selection *sel) |
434 | { |
445 | { |
435 | struct coord_rect r; |
446 | struct coord_rect r; |
436 | struct map_selection *curr; |
447 | struct map_selection *curr; |
437 | int i; |
448 | int i; |
438 | |
449 | |
439 | if (!sel) |
450 | if (!sel) |
440 | return 1; |
451 | return 1; |
441 | r.lu=sd->c[0]; |
452 | r.lu = sd->c[0]; |
442 | r.rl=sd->c[0]; |
453 | r.rl = sd->c[0]; |
443 | for (i = 1 ; i < sd->count ; i++) { |
454 | for (i = 1; i < sd->count; i++) |
|
|
455 | { |
444 | if (r.lu.x > sd->c[i].x) |
456 | if (r.lu.x > sd->c[i].x) |
445 | r.lu.x=sd->c[i].x; |
457 | r.lu.x = sd->c[i].x; |
446 | if (r.rl.x < sd->c[i].x) |
458 | if (r.rl.x < sd->c[i].x) |
447 | r.rl.x=sd->c[i].x; |
459 | r.rl.x = sd->c[i].x; |
448 | if (r.rl.y > sd->c[i].y) |
460 | if (r.rl.y > sd->c[i].y) |
449 | r.rl.y=sd->c[i].y; |
461 | r.rl.y = sd->c[i].y; |
450 | if (r.lu.y < sd->c[i].y) |
462 | if (r.lu.y < sd->c[i].y) |
451 | r.lu.y=sd->c[i].y; |
463 | r.lu.y = sd->c[i].y; |
452 | } |
464 | } |
453 | curr=sel; |
465 | curr = sel; |
454 | while (curr) { |
466 | while (curr) |
|
|
467 | { |
455 | struct coord_rect *sr=&curr->u.c_rect; |
468 | struct coord_rect *sr = &curr->u.c_rect; |
456 | if (r.lu.x <= sr->rl.x && r.rl.x >= sr->lu.x && |
469 | if (r.lu.x <= sr->rl.x && r.rl.x >= sr->lu.x && r.lu.y >= sr->rl.y && r.rl.y <= sr->lu.y) |
457 | r.lu.y >= sr->rl.y && r.rl.y <= sr->lu.y) |
|
|
458 | return 1; |
470 | return 1; |
459 | curr=curr->next; |
471 | curr = curr->next; |
460 | } |
472 | } |
461 | return 0; |
473 | return 0; |
462 | } |
474 | } |
463 | |
475 | |
464 | |
|
|
465 | static void |
|
|
466 | tracking_doupdate_lines(struct tracking *tr, struct coord *pc, enum projection pro) |
476 | static void tracking_doupdate_lines(struct tracking *tr, struct coord *pc, enum projection pro) |
467 | { |
477 | { |
468 | int max_dist=1000; |
478 | int max_dist = 1000; |
469 | struct map_selection *sel; |
479 | struct map_selection *sel; |
470 | struct mapset_handle *h; |
480 | struct mapset_handle *h; |
471 | struct map *m; |
481 | struct map *m; |
472 | struct map_rect *mr; |
482 | struct map_rect *mr; |
473 | struct item *item; |
483 | struct item *item; |
… | |
… | |
475 | struct tracking_line *tl; |
485 | struct tracking_line *tl; |
476 | struct coord_geo g; |
486 | struct coord_geo g; |
477 | struct coord cc; |
487 | struct coord cc; |
478 | |
488 | |
479 | //dbg(1,"enter\n"); |
489 | //dbg(1,"enter\n"); |
480 | h=mapset_open(tr->ms); |
490 | h = mapset_open(tr->ms); |
481 | while ((m=mapset_next(h,2))) { |
491 | while ((m = mapset_next(h, 2))) |
|
|
492 | { |
482 | cc.x = pc->x; |
493 | cc.x = pc->x; |
483 | cc.y = pc->y; |
494 | cc.y = pc->y; |
484 | if (map_projection(m) != pro) { |
495 | if (map_projection(m) != pro) |
|
|
496 | { |
485 | transform_to_geo(pro, &cc, &g); |
497 | transform_to_geo(pro, &cc, &g); |
486 | transform_from_geo(map_projection(m), &g, &cc); |
498 | transform_from_geo(map_projection(m), &g, &cc); |
487 | } |
499 | } |
488 | sel = route_rect(18, &cc, &cc, 0, max_dist); |
500 | sel = route_rect(18, &cc, &cc, 0, max_dist); |
489 | mr=map_rect_new(m, sel); |
501 | mr = map_rect_new(m, sel); |
490 | if (!mr) |
502 | if (!mr) |
491 | continue; |
503 | continue; |
492 | while ((item=map_rect_get_item(mr))) { |
504 | while ((item = map_rect_get_item(mr))) |
|
|
505 | { |
493 | if (item_get_default_flags(item->type)) { |
506 | if (item_get_default_flags(item->type)) |
|
|
507 | { |
494 | street=street_get_data(item); |
508 | street = street_get_data(item); |
495 | if (street_data_within_selection(street, sel)) { |
509 | if (street_data_within_selection(street, sel)) |
|
|
510 | { |
496 | tl=g_malloc(sizeof(struct tracking_line)+(street->count-1)*sizeof(int)); |
511 | tl = g_malloc(sizeof(struct tracking_line) + (street->count - 1) * sizeof(int)); |
497 | tl->street=street; |
512 | tl->street = street; |
498 | tracking_get_angles(tl); |
513 | tracking_get_angles(tl); |
499 | tl->next=tr->lines; |
514 | tl->next = tr->lines; |
500 | tr->lines=tl; |
515 | tr->lines = tl; |
|
|
516 | } |
501 | } else |
517 | else |
502 | street_data_free(street); |
518 | street_data_free(street); |
503 | } |
519 | } |
504 | } |
520 | } |
505 | map_selection_destroy(sel); |
521 | map_selection_destroy(sel); |
506 | map_rect_destroy(mr); |
522 | map_rect_destroy(mr); |
507 | } |
523 | } |
508 | mapset_close(h); |
524 | mapset_close(h); |
509 | //dbg(1, "exit\n"); |
525 | //dbg(1, "exit\n"); |
510 | } |
526 | } |
511 | |
527 | |
512 | |
|
|
513 | void |
|
|
514 | tracking_flush(struct tracking *tr) |
528 | void tracking_flush(struct tracking *tr) |
515 | { |
529 | { |
516 | struct tracking_line *tl=tr->lines,*next; |
530 | struct tracking_line *tl = tr->lines, *next; |
517 | dbg(1,"enter(tr=%p)\n", tr); |
531 | dbg(1, "enter(tr=%p)\n", tr); |
518 | |
532 | |
519 | while (tl) { |
533 | while (tl) |
|
|
534 | { |
520 | next=tl->next; |
535 | next = tl->next; |
521 | street_data_free(tl->street); |
536 | street_data_free(tl->street); |
522 | g_free(tl); |
537 | g_free(tl); |
523 | tl=next; |
538 | tl = next; |
524 | } |
539 | } |
525 | tr->lines=NULL; |
540 | tr->lines = NULL; |
526 | tr->curr_line = NULL; |
541 | tr->curr_line = NULL; |
527 | } |
542 | } |
528 | |
543 | |
529 | static int |
|
|
530 | tracking_angle_diff(int a1, int a2, int full) |
544 | static int tracking_angle_diff(int a1, int a2, int full) |
531 | { |
545 | { |
532 | int ret=(a1-a2)%full; |
546 | int ret = (a1 - a2) % full; |
533 | if (ret > full/2) |
547 | if (ret > full / 2) |
534 | ret-=full; |
548 | ret -= full; |
535 | if (ret < -full/2) |
549 | if (ret < -full / 2) |
536 | ret+=full; |
550 | ret += full; |
537 | return ret; |
551 | return ret; |
538 | } |
552 | } |
539 | |
553 | |
540 | static int |
|
|
541 | tracking_angle_abs_diff(int a1, int a2, int full) |
554 | static int tracking_angle_abs_diff(int a1, int a2, int full) |
542 | { |
555 | { |
543 | int ret=tracking_angle_diff(a1, a2, full); |
556 | int ret = tracking_angle_diff(a1, a2, full); |
544 | if (ret < 0) |
557 | if (ret < 0) |
545 | ret=-ret; |
558 | ret = -ret; |
546 | return ret; |
559 | return ret; |
547 | } |
560 | } |
548 | |
561 | |
549 | static int |
|
|
550 | tracking_angle_delta(struct tracking *tr, int vehicle_angle, int street_angle, int flags) |
562 | static int tracking_angle_delta(struct tracking *tr, int vehicle_angle, int street_angle, int flags) |
551 | { |
563 | { |
552 | int full=180,ret=360,fwd=0,rev=0; |
564 | int full = 180, ret = 360, fwd = 0, rev = 0; |
553 | struct vehicleprofile *profile=tr->vehicleprofile; |
565 | struct vehicleprofile *profile = tr->vehicleprofile; |
554 | |
566 | |
555 | if (profile) { |
567 | if (profile) |
|
|
568 | { |
556 | fwd=((flags & profile->flags_forward_mask) == profile->flags); |
569 | fwd = ((flags & profile->flags_forward_mask) == profile->flags); |
557 | rev=((flags & profile->flags_reverse_mask) == profile->flags); |
570 | rev = ((flags & profile->flags_reverse_mask) == profile->flags); |
558 | } |
571 | } |
559 | if (fwd || rev) { |
572 | if (fwd || rev) |
|
|
573 | { |
560 | if (!fwd || !rev) { |
574 | if (!fwd || !rev) |
|
|
575 | { |
561 | full=360; |
576 | full = 360; |
562 | if (rev) |
577 | if (rev) |
563 | street_angle=(street_angle+180)%360; |
578 | street_angle = (street_angle + 180) % 360; |
564 | } |
579 | } |
565 | ret=tracking_angle_abs_diff(vehicle_angle, street_angle, full); |
580 | ret = tracking_angle_abs_diff(vehicle_angle, street_angle, full); |
566 | } |
581 | } |
567 | return ret*ret; |
582 | return ret * ret; |
568 | } |
583 | } |
569 | |
584 | |
570 | static int |
|
|
571 | tracking_is_connected(struct tracking *tr, struct coord *c1, struct coord *c2) |
585 | static int tracking_is_connected(struct tracking *tr, struct coord *c1, struct coord *c2) |
572 | { |
586 | { |
573 | if (c1[0].x == c2[0].x && c1[0].y == c2[0].y) |
587 | if (c1[0].x == c2[0].x && c1[0].y == c2[0].y) |
574 | return 0; |
588 | return 0; |
575 | if (c1[0].x == c2[1].x && c1[0].y == c2[1].y) |
589 | if (c1[0].x == c2[1].x && c1[0].y == c2[1].y) |
576 | return 0; |
590 | return 0; |
… | |
… | |
579 | if (c1[1].x == c2[1].x && c1[1].y == c2[1].y) |
593 | if (c1[1].x == c2[1].x && c1[1].y == c2[1].y) |
580 | return 0; |
594 | return 0; |
581 | return tr->connected_pref; |
595 | return tr->connected_pref; |
582 | } |
596 | } |
583 | |
597 | |
584 | static int |
|
|
585 | tracking_is_no_stop(struct tracking *tr, struct coord *c1, struct coord *c2) |
598 | static int tracking_is_no_stop(struct tracking *tr, struct coord *c1, struct coord *c2) |
586 | { |
599 | { |
587 | if (c1->x == c2->x && c1->y == c2->y) |
600 | if (c1->x == c2->x && c1->y == c2->y) |
588 | return tr->nostop_pref; |
601 | return tr->nostop_pref; |
589 | return 0; |
602 | return 0; |
590 | } |
603 | } |
591 | |
604 | |
592 | static int |
|
|
593 | tracking_is_on_route(struct tracking *tr, struct route *rt, struct item *item) |
605 | static int tracking_is_on_route(struct tracking *tr, struct route *rt, struct item *item) |
594 | { |
606 | { |
595 | #ifdef USE_ROUTING |
607 | #ifdef USE_ROUTING |
596 | if (! rt) |
608 | if (! rt) |
597 | return 0; |
609 | return 0; |
598 | if (route_contains(rt, item)) |
610 | if (route_contains(rt, item)) |
599 | return 0; |
611 | return 0; |
600 | return tr->route_pref; |
612 | return tr->route_pref; |
601 | #else |
613 | #else |
602 | return 0; |
614 | return 0; |
603 | #endif |
615 | #endif |
604 | } |
616 | } |
605 | |
617 | |
606 | static int |
|
|
607 | tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags) |
618 | static int tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags) |
608 | { |
619 | { |
609 | int value=0; |
620 | int value = 0; |
610 | struct street_data *sd=t->street; |
621 | struct street_data *sd = t->street; |
611 | //dbg(2, "%d: (0x%x,0x%x)-(0x%x,0x%x)\n", offset, sd->c[offset].x, sd->c[offset].y, sd->c[offset+1].x, sd->c[offset+1].y); |
622 | //dbg(2, "%d: (0x%x,0x%x)-(0x%x,0x%x)\n", offset, sd->c[offset].x, sd->c[offset].y, sd->c[offset+1].x, sd->c[offset+1].y); |
612 | if (flags & 1) |
623 | if (flags & 1) |
613 | { |
624 | { |
614 | struct coord c1, c2, cp; |
625 | struct coord c1, c2, cp; |
615 | c1.x = sd->c[offset].x; |
626 | c1.x = sd->c[offset].x; |
616 | c1.y = sd->c[offset].y; |
627 | c1.y = sd->c[offset].y; |
617 | c2.x = sd->c[offset+1].x; |
628 | c2.x = sd->c[offset + 1].x; |
618 | c2.y = sd->c[offset+1].y; |
629 | c2.y = sd->c[offset + 1].y; |
619 | cp.x = tr->curr_in.x; |
630 | cp.x = tr->curr_in.x; |
620 | cp.y = tr->curr_in.y; |
631 | cp.y = tr->curr_in.y; |
621 | value+=transform_distance_line_sq(&c1, &c2, &cp, lpnt); |
632 | value += transform_distance_line_sq(&c1, &c2, &cp, lpnt); |
622 | } |
633 | } |
623 | if (value >= min) |
634 | if (value >= min) |
624 | return value; |
635 | return value; |
625 | if (flags & 2) |
636 | if (flags & 2) |
626 | value += tracking_angle_delta(tr, tr->curr_angle, t->angle[offset], sd->flags)*tr->angle_pref>>4; |
637 | value += tracking_angle_delta(tr, tr->curr_angle, t->angle[offset], sd->flags) * tr->angle_pref >> 4; |
627 | if (value >= min) |
638 | if (value >= min) |
628 | return value; |
639 | return value; |
629 | if ((flags & 4) && tr->connected_pref) |
640 | if ((flags & 4) && tr->connected_pref) |
630 | value += tracking_is_connected(tr, tr->last, &sd->c[offset]); |
641 | value += tracking_is_connected(tr, tr->last, &sd->c[offset]); |
631 | if ((flags & 8) && tr->nostop_pref) |
642 | if ((flags & 8) && tr->nostop_pref) |
632 | value += tracking_is_no_stop(tr, lpnt, &tr->last_out); |
643 | value += tracking_is_no_stop(tr, lpnt, &tr->last_out); |
633 | if (value >= min) |
644 | if (value >= min) |
634 | return value; |
645 | return value; |
635 | if ((flags & 16) && tr->route_pref) |
646 | if ((flags & 16) && tr->route_pref) |
636 | value += tracking_is_on_route(tr, tr->rt, &sd->item); |
647 | value += tracking_is_on_route(tr, tr->rt, &sd->item); |
637 | if ((flags & 32) && tr->overspeed_percent_pref && tr->overspeed_pref ) |
648 | if ((flags & 32) && tr->overspeed_percent_pref && tr->overspeed_pref) |
638 | { |
649 | { |
639 | struct roadprofile *roadprofile=g_hash_table_lookup(tr->vehicleprofile->roadprofile_hash, (void *)t->street->item.type); |
650 | struct roadprofile *roadprofile = g_hash_table_lookup(tr->vehicleprofile->roadprofile_hash, (void *) t->street->item.type); |
640 | if (roadprofile && tr->speed > roadprofile->speed * tr->overspeed_percent_pref/ 100) |
651 | if (roadprofile && tr->speed > roadprofile->speed * tr->overspeed_percent_pref / 100) |
641 | value += tr->overspeed_pref; |
652 | value += tr->overspeed_pref; |
642 | } |
653 | } |
643 | return value; |
654 | return value; |
644 | } |
655 | } |
645 | |
656 | |
646 | |
|
|
647 | void |
|
|
648 | tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro) |
657 | void tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro) |
649 | { |
658 | { |
650 | struct tracking_line *t; |
659 | struct tracking_line *t; |
651 | int i,value,min,time; |
660 | int i, value, min, time; |
652 | struct coord lpnt; |
661 | struct coord lpnt; |
653 | struct coord cin; |
662 | struct coord cin; |
654 | struct attr valid,speed_attr,direction_attr,coord_geo,lag,time_attr,static_speed,static_distance; |
663 | struct attr valid, speed_attr, direction_attr, coord_geo, lag, time_attr, static_speed, static_distance; |
655 | double speed, direction; |
664 | double speed, direction; |
656 | if (v) |
665 | if (v) |
657 | tr->vehicle=v; |
666 | tr->vehicle = v; |
658 | if (vehicleprofile) |
667 | if (vehicleprofile) |
659 | tr->vehicleprofile=vehicleprofile; |
668 | tr->vehicleprofile = vehicleprofile; |
660 | |
669 | |
661 | if (! tr->vehicle) |
670 | if (!tr->vehicle) |
662 | return; |
671 | return; |
663 | if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL)) |
672 | if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL)) |
664 | valid.u.num=attr_position_valid_valid; |
673 | valid.u.num = attr_position_valid_valid; |
665 | if (valid.u.num == attr_position_valid_invalid) |
674 | if (valid.u.num == attr_position_valid_invalid) |
666 | { |
675 | { |
667 | tr->valid=valid.u.num; |
676 | tr->valid = valid.u.num; |
668 | return; |
677 | return; |
669 | } |
678 | } |
670 | if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) || |
679 | if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) || !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) || !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) || !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)) |
671 | !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) || |
|
|
672 | !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) || |
|
|
673 | !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)) |
|
|
674 | { |
680 | { |
675 | //dbg(0,"failed to get position data %d %d %d %d\n", |
681 | //dbg(0,"failed to get position data %d %d %d %d\n", |
676 | //vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL), |
682 | //vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL), |
677 | //vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL), |
683 | //vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL), |
678 | //vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL), |
684 | //vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL), |
679 | //vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)); |
685 | //vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)); |
680 | return; |
686 | return; |
681 | } |
687 | } |
682 | if (!vehicleprofile_get_attr(vehicleprofile,attr_static_speed,&static_speed,NULL) || !vehicleprofile_get_attr(vehicleprofile,attr_static_distance,&static_distance,NULL)) |
688 | if (!vehicleprofile_get_attr(vehicleprofile, attr_static_speed, &static_speed, NULL) || !vehicleprofile_get_attr(vehicleprofile, attr_static_distance, &static_distance, NULL)) |
683 | { |
689 | { |
684 | static_speed.u.num=3; |
690 | static_speed.u.num = 3; |
685 | static_distance.u.num=10; |
691 | static_distance.u.num = 10; |
686 | //dbg(1,"Using defaults for static position detection\n"); |
692 | //dbg(1,"Using defaults for static position detection\n"); |
687 | } |
693 | } |
688 | //dbg(2,"Static speed: %u, static distance: %u\n",static_speed.u.num, static_distance.u.num); |
694 | //dbg(2,"Static speed: %u, static distance: %u\n",static_speed.u.num, static_distance.u.num); |
689 | time=iso8601_to_secs(time_attr.u.str); |
695 | time = iso8601_to_secs(time_attr.u.str); |
690 | speed=*speed_attr.u.numd; |
696 | speed = *speed_attr.u.numd; |
691 | direction=*direction_attr.u.numd; |
697 | direction = *direction_attr.u.numd; |
692 | tr->valid=attr_position_valid_valid; |
698 | tr->valid = attr_position_valid_valid; |
693 | transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in); |
699 | transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in); |
694 | if ((speed < static_speed.u.num && transform_distance(pro, &tr->last_in, &tr->curr_in) < static_distance.u.num )) |
700 | if ((speed < static_speed.u.num && transform_distance(pro, &tr->last_in, &tr->curr_in) < static_distance.u.num)) |
695 | { |
701 | { |
696 | //dbg(1,"static speed %f coord 0x%x,0x%x vs 0x%x,0x%x\n",speed,tr->last_in.x,tr->last_in.y, tr->curr_in.x, tr->curr_in.y); |
702 | //dbg(1,"static speed %f coord 0x%x,0x%x vs 0x%x,0x%x\n",speed,tr->last_in.x,tr->last_in.y, tr->curr_in.x, tr->curr_in.y); |
697 | tr->valid=attr_position_valid_static; |
703 | tr->valid = attr_position_valid_static; |
698 | tr->speed=0; |
704 | tr->speed = 0; |
699 | return; |
705 | return; |
700 | } |
706 | } |
701 | |
707 | |
702 | if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0) |
708 | if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0) |
703 | { |
709 | { |
704 | double espeed; |
710 | double espeed; |
705 | int edirection; |
711 | int edirection; |
706 | if (time - tr->time == 1) |
712 | if (time - tr->time == 1) |
707 | { |
713 | { |
708 | /* dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed); */ |
714 | /* dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed); */ |
709 | espeed=speed+(speed-tr->speed)*lag.u.num/10; |
715 | espeed = speed + (speed - tr->speed) * lag.u.num / 10; |
710 | /* dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360)); */ |
716 | /* dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360)); */ |
711 | edirection=direction+tracking_angle_diff(direction,tr->direction,360)*lag.u.num/10; |
717 | edirection = direction + tracking_angle_diff(direction, tr->direction, 360) * lag.u.num / 10; |
712 | } |
718 | } |
713 | else |
719 | else |
714 | { |
720 | { |
715 | //dbg(1,"no speed and direction extrapolation\n"); |
721 | //dbg(1,"no speed and direction extrapolation\n"); |
716 | espeed=speed; |
722 | espeed = speed; |
717 | edirection=direction; |
723 | edirection = direction; |
718 | } |
724 | } |
719 | //dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection); |
725 | //dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection); |
720 | //dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y); |
726 | //dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y); |
721 | transform_project(pro, &tr->curr_in, espeed*lag.u.num/36, edirection, &tr->curr_in); |
727 | transform_project(pro, &tr->curr_in, espeed * lag.u.num / 36, edirection, &tr->curr_in); |
722 | //dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y); |
728 | //dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y); |
723 | } |
729 | } |
724 | tr->time=time; |
730 | tr->time = time; |
725 | tr->pro=pro; |
731 | tr->pro = pro; |
726 | #if 0 |
732 | #if 0 |
727 | |
733 | |
728 | tracking_process_cdf(&tr->cdf, pc, &pcf, angle, &anglef, speed, fixtime); |
734 | tracking_process_cdf(&tr->cdf, pc, &pcf, angle, &anglef, speed, fixtime); |
729 | #endif |
735 | #endif |
730 | tr->curr_angle=tr->direction=direction; |
736 | tr->curr_angle = tr->direction = direction; |
731 | tr->speed=speed; |
737 | tr->speed = speed; |
732 | tr->last_in=tr->curr_in; |
738 | tr->last_in = tr->curr_in; |
733 | tr->last_out=tr->curr_out; |
739 | tr->last_out = tr->curr_out; |
734 | tr->last[0]=tr->curr[0]; |
740 | tr->last[0] = tr->curr[0]; |
735 | tr->last[1]=tr->curr[1]; |
741 | tr->last[1] = tr->curr[1]; |
736 | if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500) |
742 | if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500) |
737 | { |
743 | { |
738 | //dbg(1, "update\n"); |
744 | //dbg(1, "update\n"); |
739 | tracking_flush(tr); |
745 | tracking_flush(tr); |
740 | tracking_doupdate_lines(tr, &tr->curr_in, pro); |
746 | tracking_doupdate_lines(tr, &tr->curr_in, pro); |
741 | tr->last_updated=tr->curr_in; |
747 | tr->last_updated = tr->curr_in; |
742 | //dbg(1,"update end\n"); |
748 | //dbg(1,"update end\n"); |
743 | } |
749 | } |
744 | |
750 | |
745 | tr->street_direction=0; |
751 | tr->street_direction = 0; |
746 | t=tr->lines; |
752 | t = tr->lines; |
747 | tr->curr_line=NULL; |
753 | tr->curr_line = NULL; |
748 | min=INT_MAX/2; |
754 | min = INT_MAX / 2; |
749 | while (t) |
755 | while (t) |
750 | { |
756 | { |
751 | struct street_data *sd=t->street; |
757 | struct street_data *sd = t->street; |
752 | for (i = 0; i < sd->count-1 ; i++) |
758 | for (i = 0; i < sd->count - 1; i++) |
753 | { |
759 | { |
754 | value=tracking_value(tr,t,i,&lpnt,min,-1); |
760 | value = tracking_value(tr, t, i, &lpnt, min, -1); |
755 | if (value < min) |
761 | if (value < min) |
756 | { |
762 | { |
757 | struct coord lpnt_tmp; |
763 | struct coord lpnt_tmp; |
758 | int angle_delta=tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360); |
764 | int angle_delta = tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360); |
759 | tr->curr_line=t; |
765 | tr->curr_line = t; |
760 | tr->pos=i; |
766 | tr->pos = i; |
761 | tr->curr[0]=sd->c[i]; |
767 | tr->curr[0] = sd->c[i]; |
762 | tr->curr[1]=sd->c[i+1]; |
768 | tr->curr[1] = sd->c[i + 1]; |
763 | /* |
769 | /* |
764 | dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i, |
770 | dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i, |
765 | transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt_tmp), |
771 | transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt_tmp), |
766 | tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*tr->angle_pref, |
772 | tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*tr->angle_pref, |
767 | tracking_is_connected(tr, tr->last, &sd->c[i]) ? tr->connected_pref : 0, |
773 | tracking_is_connected(tr, tr->last, &sd->c[i]) ? tr->connected_pref : 0, |
768 | lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? tr->nostop_pref : 0, |
774 | lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? tr->nostop_pref : 0, |
769 | value |
775 | value |
770 | ); |
776 | ); |
771 | */ |
777 | */ |
772 | tr->curr_out.x=lpnt.x; |
778 | tr->curr_out.x = lpnt.x; |
773 | tr->curr_out.y=lpnt.y; |
779 | tr->curr_out.y = lpnt.y; |
774 | tr->coord_geo_valid=0; |
780 | tr->coord_geo_valid = 0; |
775 | if (angle_delta < 70) |
781 | if (angle_delta < 70) |
776 | tr->street_direction=1; |
782 | tr->street_direction = 1; |
777 | else if (angle_delta > 110) |
783 | else if (angle_delta > 110) |
778 | tr->street_direction=-1; |
784 | tr->street_direction = -1; |
779 | else |
785 | else |
780 | tr->street_direction=0; |
786 | tr->street_direction = 0; |
781 | min=value; |
787 | min = value; |
782 | } |
788 | } |
783 | } |
789 | } |
784 | t=t->next; |
790 | t = t->next; |
785 | } |
791 | } |
786 | |
792 | |
787 | //dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min); |
793 | //dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min); |
788 | |
794 | |
789 | if (!tr->curr_line || min > tr->offroad_limit_pref) |
795 | if (!tr->curr_line || min > tr->offroad_limit_pref) |
790 | { |
796 | { |
791 | tr->curr_out=tr->curr_in; |
797 | tr->curr_out = tr->curr_in; |
792 | tr->coord_geo_valid=0; |
798 | tr->coord_geo_valid = 0; |
793 | tr->street_direction=0; |
799 | tr->street_direction = 0; |
794 | } |
800 | } |
795 | |
801 | |
796 | if (tr->curr_line && (tr->curr_line->street->flags & AF_UNDERGROUND)) |
802 | if (tr->curr_line && (tr->curr_line->street->flags & AF_UNDERGROUND)) |
797 | { |
803 | { |
798 | //dbg(0,"AF_UNDERGROUND 1"); |
804 | //dbg(0,"AF_UNDERGROUND 1"); |
799 | if (tr->no_gps) |
805 | if (tr->no_gps) |
800 | { |
806 | { |
801 | tr->tunnel=1; |
807 | tr->tunnel = 1; |
802 | } |
808 | } |
803 | } |
809 | } |
804 | else if (tr->tunnel) |
810 | else if (tr->tunnel) |
805 | { |
811 | { |
806 | //dbg(0,"AF_UNDERGROUND 2"); |
812 | //dbg(0,"AF_UNDERGROUND 2"); |
807 | tr->speed=0; |
813 | tr->speed = 0; |
808 | } |
814 | } |
809 | //dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y); |
815 | //dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y); |
810 | } |
816 | } |
811 | |
817 | |
812 | static int |
|
|
813 | tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial) |
818 | static int tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial) |
814 | { |
819 | { |
815 | switch (attr->type) { |
820 | switch (attr->type) |
|
|
821 | { |
816 | case attr_angle_pref: |
822 | case attr_angle_pref: |
817 | tr->angle_pref=attr->u.num; |
823 | tr->angle_pref = attr->u.num; |
818 | return 1; |
824 | return 1; |
819 | case attr_connected_pref: |
825 | case attr_connected_pref: |
820 | tr->connected_pref=attr->u.num; |
826 | tr->connected_pref = attr->u.num; |
821 | return 1; |
827 | return 1; |
822 | case attr_nostop_pref: |
828 | case attr_nostop_pref: |
823 | tr->nostop_pref=attr->u.num; |
829 | tr->nostop_pref = attr->u.num; |
824 | return 1; |
830 | return 1; |
825 | case attr_offroad_limit_pref: |
831 | case attr_offroad_limit_pref: |
826 | tr->offroad_limit_pref=attr->u.num; |
832 | tr->offroad_limit_pref = attr->u.num; |
827 | return 1; |
833 | return 1; |
828 | case attr_route_pref: |
834 | case attr_route_pref: |
829 | tr->route_pref=attr->u.num; |
835 | tr->route_pref = attr->u.num; |
830 | return 1; |
836 | return 1; |
831 | case attr_overspeed_pref: |
837 | case attr_overspeed_pref: |
832 | tr->overspeed_pref=attr->u.num; |
838 | tr->overspeed_pref = attr->u.num; |
833 | return 1; |
839 | return 1; |
834 | case attr_overspeed_percent_pref: |
840 | case attr_overspeed_percent_pref: |
835 | tr->overspeed_percent_pref=attr->u.num; |
841 | tr->overspeed_percent_pref = attr->u.num; |
836 | return 1; |
842 | return 1; |
837 | case attr_tunnel_extrapolation: |
843 | case attr_tunnel_extrapolation: |
838 | tr->tunnel_extrapolation=attr->u.num; |
844 | tr->tunnel_extrapolation = attr->u.num; |
839 | return 1; |
845 | return 1; |
840 | default: |
846 | default: |
841 | return 0; |
847 | return 0; |
842 | } |
848 | } |
843 | } |
849 | } |
844 | |
850 | |
845 | int |
|
|
846 | tracking_set_attr(struct tracking *tr, struct attr *attr) |
851 | int tracking_set_attr(struct tracking *tr, struct attr *attr) |
847 | { |
852 | { |
848 | return tracking_set_attr_do(tr, attr, 0); |
853 | return tracking_set_attr_do(tr, attr, 0); |
849 | } |
854 | } |
850 | |
855 | |
851 | struct tracking * |
856 | struct tracking * |
852 | tracking_new(struct attr *parent, struct attr **attrs) |
857 | tracking_new(struct attr *parent, struct attr **attrs) |
853 | { |
858 | { |
854 | struct tracking *this=g_new0(struct tracking, 1); |
859 | struct tracking *this=g_new0(struct tracking, 1); |
855 | struct attr hist_size; |
860 | struct attr hist_size; |
856 | this->angle_pref=10; |
861 | this->angle_pref = 10; |
857 | this->connected_pref=10; |
862 | this->connected_pref = 10; |
858 | this->nostop_pref=10; |
863 | this->nostop_pref = 10; |
859 | this->offroad_limit_pref=5000; |
864 | this->offroad_limit_pref = 15000; // was 5000 originally! |
860 | this->route_pref=300; |
865 | this->route_pref = 300; |
861 | |
866 | |
862 | |
|
|
863 | if (! attr_generic_get_attr(attrs, NULL, attr_cdf_histsize, &hist_size, NULL)) { |
867 | if (!attr_generic_get_attr(attrs, NULL, attr_cdf_histsize, &hist_size, NULL)) |
|
|
868 | { |
864 | hist_size.u.num = 0; |
869 | hist_size.u.num = 0; |
865 | } |
870 | } |
866 | if (attrs) { |
871 | if (attrs) |
|
|
872 | { |
867 | for (;*attrs; attrs++) |
873 | for (; *attrs; attrs++) |
868 | tracking_set_attr_do(this, *attrs, 1); |
874 | tracking_set_attr_do(this, *attrs, 1); |
869 | } |
875 | } |
870 | |
876 | |
871 | tracking_init_cdf(&this->cdf, hist_size.u.num); |
877 | tracking_init_cdf(&this->cdf, hist_size.u.num); |
872 | |
878 | |
873 | return this; |
879 | return this; |
874 | } |
880 | } |
875 | |
881 | |
876 | void |
|
|
877 | tracking_set_mapset(struct tracking *this, struct mapset *ms) |
882 | void tracking_set_mapset(struct tracking *this, struct mapset *ms) |
878 | { |
883 | { |
879 | this->ms=ms; |
884 | this->ms = ms; |
880 | } |
885 | } |
881 | |
886 | |
882 | void |
|
|
883 | tracking_set_route(struct tracking *this, struct route *rt) |
887 | void tracking_set_route(struct tracking *this, struct route *rt) |
884 | { |
888 | { |
885 | this->rt=rt; |
889 | this->rt = rt; |
886 | } |
890 | } |
887 | |
891 | |
888 | void |
|
|
889 | tracking_destroy(struct tracking *tr) |
892 | void tracking_destroy(struct tracking *tr) |
890 | { |
893 | { |
891 | if (tr->attr) |
894 | if (tr->attr) |
892 | attr_free(tr->attr); |
895 | attr_free(tr->attr); |
893 | tracking_flush(tr); |
896 | tracking_flush(tr); |
894 | g_free(tr); |
897 | g_free(tr); |
895 | } |
898 | } |
896 | |
899 | |
897 | struct map * |
900 | struct map * |
898 | tracking_get_map(struct tracking *this_) |
901 | tracking_get_map(struct tracking *this_) |
899 | { |
902 | { |
900 | struct attr *attrs[5]; |
903 | struct attr *attrs[5]; |
901 | struct attr type,navigation,data,description; |
904 | struct attr type, navigation, data, description; |
902 | type.type=attr_type; |
905 | type.type = attr_type; |
903 | type.u.str="tracking"; |
906 | type.u.str = "tracking"; |
904 | navigation.type=attr_trackingo; |
907 | navigation.type = attr_trackingo; |
905 | navigation.u.tracking=this_; |
908 | navigation.u.tracking = this_; |
906 | data.type=attr_data; |
909 | data.type = attr_data; |
907 | data.u.str=""; |
910 | data.u.str = ""; |
908 | description.type=attr_description; |
911 | description.type = attr_description; |
909 | description.u.str="Tracking"; |
912 | description.u.str = "Tracking"; |
910 | |
913 | |
911 | attrs[0]=&type; |
914 | attrs[0] = &type; |
912 | attrs[1]=&navigation; |
915 | attrs[1] = &navigation; |
913 | attrs[2]=&data; |
916 | attrs[2] = &data; |
914 | attrs[3]=&description; |
917 | attrs[3] = &description; |
915 | attrs[4]=NULL; |
918 | attrs[4] = NULL; |
916 | if (! this_->map) |
919 | if (!this_->map) |
917 | this_->map=map_new(NULL, attrs); |
920 | this_->map = map_new(NULL, attrs); |
918 | return this_->map; |
921 | return this_->map; |
919 | } |
922 | } |
920 | |
923 | |
921 | |
|
|
922 | struct map_priv { |
924 | struct map_priv |
|
|
925 | { |
923 | struct tracking *tracking; |
926 | struct tracking *tracking; |
924 | }; |
927 | }; |
925 | |
928 | |
926 | struct map_rect_priv { |
929 | struct map_rect_priv |
|
|
930 | { |
927 | struct tracking *tracking; |
931 | struct tracking *tracking; |
928 | struct item item; |
932 | struct item item; |
929 | struct tracking_line *curr,*next; |
933 | struct tracking_line *curr, *next; |
930 | int coord; |
934 | int coord; |
931 | enum attr_type attr_next; |
935 | enum attr_type attr_next; |
932 | int ccount; |
936 | int ccount; |
933 | int debug_idx; |
937 | int debug_idx; |
934 | char *str; |
938 | char *str; |
935 | }; |
939 | }; |
936 | |
940 | |
937 | static int |
|
|
938 | tracking_map_item_coord_get(void *priv_data, struct coord *c, int count) |
941 | static int tracking_map_item_coord_get(void *priv_data, struct coord *c, int count) |
939 | { |
942 | { |
940 | struct map_rect_priv *this=priv_data; |
943 | struct map_rect_priv *this = priv_data; |
941 | enum projection pro; |
944 | enum projection pro; |
942 | int ret=0; |
945 | int ret = 0; |
943 | dbg(1,"enter\n"); |
946 | dbg(1, "enter\n"); |
944 | while (this->ccount < 2 && count > 0) { |
947 | while (this->ccount < 2 && count > 0) |
|
|
948 | { |
945 | pro = map_projection(this->curr->street->item.map); |
949 | pro = map_projection(this->curr->street->item.map); |
946 | if (projection_mg != pro) { |
950 | if (projection_mg != pro) |
|
|
951 | { |
947 | transform_from_to(&this->curr->street->c[this->ccount+this->coord], |
952 | transform_from_to(&this->curr->street->c[this->ccount + this->coord], pro, c, projection_mg); |
948 | pro, |
953 | } |
949 | c ,projection_mg); |
|
|
950 | } else |
954 | else |
951 | *c=this->curr->street->c[this->ccount+this->coord]; |
955 | *c = this->curr->street->c[this->ccount + this->coord]; |
952 | dbg(1,"coord %d 0x%x,0x%x\n",this->ccount,c->x,c->y); |
956 | dbg(1, "coord %d 0x%x,0x%x\n", this->ccount, c->x, c->y); |
953 | this->ccount++; |
957 | this->ccount++; |
954 | ret++; |
958 | ret++; |
955 | c++; |
959 | c++; |
956 | count--; |
960 | count--; |
957 | } |
961 | } |
958 | return ret; |
962 | return ret; |
959 | } |
963 | } |
960 | |
964 | |
961 | static int |
|
|
962 | tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr) |
965 | static int tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr) |
963 | { |
966 | { |
964 | struct map_rect_priv *this_=priv_data; |
967 | struct map_rect_priv *this_ = priv_data; |
965 | struct coord lpnt,*c; |
968 | struct coord lpnt, *c; |
966 | struct tracking *tr=this_->tracking; |
969 | struct tracking *tr = this_->tracking; |
967 | int value; |
970 | int value; |
968 | attr->type=attr_type; |
971 | attr->type = attr_type; |
969 | |
972 | |
970 | if (this_->str) { |
973 | if (this_->str) |
|
|
974 | { |
971 | g_free(this_->str); |
975 | g_free(this_->str); |
972 | this_->str=NULL; |
976 | this_->str = NULL; |
973 | } |
977 | } |
974 | |
978 | |
975 | switch(attr_type) { |
979 | switch (attr_type) |
|
|
980 | { |
976 | case attr_debug: |
981 | case attr_debug: |
977 | switch(this_->debug_idx) { |
982 | switch (this_->debug_idx) |
|
|
983 | { |
978 | case 0: |
984 | case 0: |
979 | this_->debug_idx++; |
985 | this_->debug_idx++; |
980 | this_->str=attr->u.str=g_strdup_printf("overall: %d (limit %d)",tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, -1), tr->offroad_limit_pref); |
986 | this_->str = attr->u.str = g_strdup_printf("overall: %d (limit %d)", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, -1), tr->offroad_limit_pref); |
981 | return 1; |
987 | return 1; |
982 | case 1: |
988 | case 1: |
983 | this_->debug_idx++; |
989 | this_->debug_idx++; |
984 | c=&this_->curr->street->c[this_->coord]; |
990 | c = &this_->curr->street->c[this_->coord]; |
985 | value=tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 1); |
991 | value = tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 1); |
986 | this_->str=attr->u.str=g_strdup_printf("distance: (0x%x,0x%x) from (0x%x,0x%x)-(0x%x,0x%x) at (0x%x,0x%x) %d", |
992 | this_->str = attr->u.str = g_strdup_printf("distance: (0x%x,0x%x) from (0x%x,0x%x)-(0x%x,0x%x) at (0x%x,0x%x) %d", tr->curr_in.x, tr->curr_in.y, c[0].x, c[0].y, c[1].x, c[1].y, lpnt.x, lpnt.y, value); |
987 | tr->curr_in.x, tr->curr_in.y, |
|
|
988 | c[0].x, c[0].y, c[1].x, c[1].y, |
|
|
989 | lpnt.x, lpnt.y, value); |
|
|
990 | return 1; |
993 | return 1; |
991 | case 2: |
994 | case 2: |
992 | this_->debug_idx++; |
995 | this_->debug_idx++; |
993 | this_->str=attr->u.str=g_strdup_printf("angle: %d to %d (flags %d) %d", |
996 | this_->str = attr->u.str = g_strdup_printf("angle: %d to %d (flags %d) %d", tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3, tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 2)); |
994 | tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3, |
|
|
995 | tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 2)); |
|
|
996 | return 1; |
997 | return 1; |
997 | case 3: |
998 | case 3: |
998 | this_->debug_idx++; |
999 | this_->debug_idx++; |
999 | this_->str=attr->u.str=g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 4)); |
1000 | this_->str = attr->u.str = g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 4)); |
1000 | return 1; |
1001 | return 1; |
1001 | case 4: |
1002 | case 4: |
1002 | this_->debug_idx++; |
1003 | this_->debug_idx++; |
1003 | this_->str=attr->u.str=g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 8)); |
1004 | this_->str = attr->u.str = g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 8)); |
1004 | return 1; |
1005 | return 1; |
1005 | case 5: |
1006 | case 5: |
1006 | this_->debug_idx++; |
1007 | this_->debug_idx++; |
1007 | this_->str=attr->u.str=g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 16)); |
1008 | this_->str = attr->u.str = g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 16)); |
1008 | return 1; |
1009 | return 1; |
1009 | case 6: |
1010 | case 6: |
1010 | this_->debug_idx++; |
1011 | this_->debug_idx++; |
1011 | this_->str=attr->u.str=g_strdup_printf("overspeed: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 32)); |
1012 | this_->str = attr->u.str = g_strdup_printf("overspeed: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 32)); |
1012 | return 1; |
1013 | return 1; |
1013 | case 7: |
1014 | case 7: |
1014 | this_->debug_idx++; |
1015 | this_->debug_idx++; |
1015 | this_->str=attr->u.str=g_strdup_printf("line %p", this_->curr); |
1016 | this_->str = attr->u.str = g_strdup_printf("line %p", this_->curr); |
1016 | return 1; |
1017 | return 1; |
|
|
1018 | default: |
|
|
1019 | this_->attr_next = attr_none; |
|
|
1020 | return 0; |
|
|
1021 | } |
|
|
1022 | case attr_any: |
|
|
1023 | while (this_->attr_next != attr_none) |
|
|
1024 | { |
|
|
1025 | if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr)) |
|
|
1026 | return 1; |
|
|
1027 | } |
|
|
1028 | return 0; |
1017 | default: |
1029 | default: |
1018 | this_->attr_next=attr_none; |
1030 | attr->type = attr_none; |
1019 | return 0; |
1031 | return 0; |
1020 | } |
1032 | } |
1021 | case attr_any: |
|
|
1022 | while (this_->attr_next != attr_none) { |
|
|
1023 | if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr)) |
|
|
1024 | return 1; |
|
|
1025 | } |
|
|
1026 | return 0; |
|
|
1027 | default: |
|
|
1028 | attr->type=attr_none; |
|
|
1029 | return 0; |
|
|
1030 | } |
|
|
1031 | } |
1033 | } |
1032 | |
1034 | |
1033 | static struct item_methods tracking_map_item_methods = { |
1035 | static struct item_methods tracking_map_item_methods = { NULL, tracking_map_item_coord_get, NULL, tracking_map_item_attr_get, }; |
1034 | NULL, |
|
|
1035 | tracking_map_item_coord_get, |
|
|
1036 | NULL, |
|
|
1037 | tracking_map_item_attr_get, |
|
|
1038 | }; |
|
|
1039 | |
1036 | |
1040 | |
|
|
1041 | static void |
|
|
1042 | tracking_map_destroy(struct map_priv *priv) |
1037 | static void tracking_map_destroy(struct map_priv *priv) |
1043 | { |
1038 | { |
1044 | g_free(priv); |
1039 | g_free(priv); |
1045 | } |
1040 | } |
1046 | |
1041 | |
1047 | static void |
|
|
1048 | tracking_map_rect_init(struct map_rect_priv *priv) |
1042 | static void tracking_map_rect_init(struct map_rect_priv *priv) |
1049 | { |
1043 | { |
1050 | priv->next=priv->tracking->lines; |
1044 | priv->next = priv->tracking->lines; |
1051 | priv->curr=NULL; |
1045 | priv->curr = NULL; |
1052 | priv->coord=0; |
1046 | priv->coord = 0; |
1053 | priv->item.id_lo=0; |
1047 | priv->item.id_lo = 0; |
1054 | priv->item.id_hi=0; |
1048 | priv->item.id_hi = 0; |
1055 | } |
1049 | } |
1056 | |
1050 | |
1057 | static struct map_rect_priv * |
1051 | static struct map_rect_priv * |
1058 | tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel) |
1052 | tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel) |
1059 | { |
1053 | { |
1060 | struct tracking *tracking=priv->tracking; |
1054 | struct tracking *tracking = priv->tracking; |
1061 | struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1); |
1055 | struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1); |
1062 | ret->tracking=tracking; |
1056 | ret->tracking = tracking; |
1063 | tracking_map_rect_init(ret); |
1057 | tracking_map_rect_init(ret); |
1064 | ret->item.meth=&tracking_map_item_methods; |
1058 | ret->item.meth = &tracking_map_item_methods; |
1065 | ret->item.priv_data=ret; |
1059 | ret->item.priv_data = ret; |
1066 | ret->item.type=type_tracking_100; |
1060 | ret->item.type = type_tracking_100; |
1067 | return ret; |
1061 | return ret; |
1068 | } |
1062 | } |
1069 | |
1063 | |
1070 | static void |
|
|
1071 | tracking_map_rect_destroy(struct map_rect_priv *priv) |
1064 | static void tracking_map_rect_destroy(struct map_rect_priv *priv) |
1072 | { |
1065 | { |
1073 | g_free(priv); |
1066 | g_free(priv); |
1074 | } |
1067 | } |
1075 | |
1068 | |
1076 | static struct item * |
1069 | static struct item * |
1077 | tracking_map_get_item(struct map_rect_priv *priv) |
1070 | tracking_map_get_item(struct map_rect_priv *priv) |
1078 | { |
1071 | { |
1079 | struct item *ret=&priv->item; |
1072 | struct item *ret = &priv->item; |
1080 | int value; |
1073 | int value; |
1081 | struct coord lpnt; |
1074 | struct coord lpnt; |
1082 | |
1075 | |
1083 | if (!priv->next) |
1076 | if (!priv->next) |
1084 | return NULL; |
1077 | return NULL; |
1085 | if (! priv->curr || priv->coord + 2 >= priv->curr->street->count) { |
1078 | if (!priv->curr || priv->coord + 2 >= priv->curr->street->count) |
|
|
1079 | { |
1086 | priv->curr=priv->next; |
1080 | priv->curr = priv->next; |
1087 | priv->next=priv->curr->next; |
1081 | priv->next = priv->curr->next; |
1088 | priv->coord=0; |
1082 | priv->coord = 0; |
1089 | priv->item.id_lo=0; |
1083 | priv->item.id_lo = 0; |
1090 | priv->item.id_hi++; |
1084 | priv->item.id_hi++; |
1091 | } else { |
1085 | } |
|
|
1086 | else |
|
|
1087 | { |
1092 | priv->coord++; |
1088 | priv->coord++; |
1093 | priv->item.id_lo++; |
1089 | priv->item.id_lo++; |
1094 | } |
1090 | } |
1095 | value=tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX/2, -1); |
1091 | value = tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX / 2, -1); |
1096 | if (value < 64) |
1092 | if (value < 64) |
1097 | priv->item.type=type_tracking_100; |
1093 | priv->item.type = type_tracking_100; |
1098 | else if (value < 128) |
1094 | else if (value < 128) |
1099 | priv->item.type=type_tracking_90; |
1095 | priv->item.type = type_tracking_90; |
1100 | else if (value < 256) |
1096 | else if (value < 256) |
1101 | priv->item.type=type_tracking_80; |
1097 | priv->item.type = type_tracking_80; |
1102 | else if (value < 512) |
1098 | else if (value < 512) |
1103 | priv->item.type=type_tracking_70; |
1099 | priv->item.type = type_tracking_70; |
1104 | else if (value < 1024) |
1100 | else if (value < 1024) |
1105 | priv->item.type=type_tracking_60; |
1101 | priv->item.type = type_tracking_60; |
1106 | else if (value < 2048) |
1102 | else if (value < 2048) |
1107 | priv->item.type=type_tracking_50; |
1103 | priv->item.type = type_tracking_50; |
1108 | else if (value < 4096) |
1104 | else if (value < 4096) |
1109 | priv->item.type=type_tracking_40; |
1105 | priv->item.type = type_tracking_40; |
1110 | else if (value < 8192) |
1106 | else if (value < 8192) |
1111 | priv->item.type=type_tracking_30; |
1107 | priv->item.type = type_tracking_30; |
1112 | else if (value < 16384) |
1108 | else if (value < 16384) |
1113 | priv->item.type=type_tracking_20; |
1109 | priv->item.type = type_tracking_20; |
1114 | else if (value < 32768) |
1110 | else if (value < 32768) |
1115 | priv->item.type=type_tracking_10; |
1111 | priv->item.type = type_tracking_10; |
1116 | else |
1112 | else |
1117 | priv->item.type=type_tracking_0; |
1113 | priv->item.type = type_tracking_0; |
1118 | dbg(1,"item %d %d points\n", priv->coord, priv->curr->street->count); |
1114 | dbg(1, "item %d %d points\n", priv->coord, priv->curr->street->count); |
1119 | priv->ccount=0; |
1115 | priv->ccount = 0; |
1120 | priv->attr_next=attr_debug; |
1116 | priv->attr_next = attr_debug; |
1121 | priv->debug_idx=0; |
1117 | priv->debug_idx = 0; |
1122 | return ret; |
1118 | return ret; |
1123 | } |
1119 | } |
1124 | |
1120 | |
1125 | static struct item * |
1121 | static struct item * |
1126 | tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo) |
1122 | tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo) |
1127 | { |
1123 | { |
1128 | struct item *ret; |
1124 | struct item *ret; |
1129 | tracking_map_rect_init(priv); |
1125 | tracking_map_rect_init(priv); |
1130 | while ((ret=tracking_map_get_item(priv))) { |
1126 | while ((ret = tracking_map_get_item(priv))) |
|
|
1127 | { |
1131 | if (ret->id_hi == id_hi && ret->id_lo == id_lo) |
1128 | if (ret->id_hi == id_hi && ret->id_lo == id_lo) |
1132 | return ret; |
1129 | return ret; |
1133 | } |
1130 | } |
1134 | return NULL; |
1131 | return NULL; |
1135 | } |
1132 | } |
1136 | |
1133 | |
1137 | static struct map_methods tracking_map_meth = { |
1134 | static struct map_methods tracking_map_meth = { projection_mg, "utf-8", tracking_map_destroy, tracking_map_rect_new, tracking_map_rect_destroy, tracking_map_get_item, tracking_map_get_item_byid, NULL, NULL, NULL, }; |
1138 | projection_mg, |
|
|
1139 | "utf-8", |
|
|
1140 | tracking_map_destroy, |
|
|
1141 | tracking_map_rect_new, |
|
|
1142 | tracking_map_rect_destroy, |
|
|
1143 | tracking_map_get_item, |
|
|
1144 | tracking_map_get_item_byid, |
|
|
1145 | NULL, |
|
|
1146 | NULL, |
|
|
1147 | NULL, |
|
|
1148 | }; |
|
|
1149 | |
1135 | |
1150 | static struct map_priv * |
1136 | static struct map_priv * |
1151 | tracking_map_new(struct map_methods *meth, struct attr **attrs, struct callback_list *cbl) |
1137 | tracking_map_new(struct map_methods *meth, struct attr **attrs, struct callback_list *cbl) |
1152 | { |
1138 | { |
1153 | struct map_priv *ret; |
1139 | struct map_priv *ret; |
1154 | struct attr *tracking_attr; |
1140 | struct attr *tracking_attr; |
1155 | |
1141 | |
1156 | tracking_attr=attr_search(attrs, NULL, attr_trackingo); |
1142 | tracking_attr = attr_search(attrs, NULL, attr_trackingo); |
1157 | if (! tracking_attr) |
1143 | if (!tracking_attr) |
1158 | return NULL; |
|
|
1159 | ret=g_new0(struct map_priv, 1); |
1144 | return NULL; ret=g_new0(struct map_priv, 1); |
1160 | *meth=tracking_map_meth; |
1145 | *meth = tracking_map_meth; |
1161 | ret->tracking=tracking_attr->u.tracking; |
1146 | ret->tracking = tracking_attr->u.tracking; |
1162 | |
1147 | |
1163 | return ret; |
1148 | return ret; |
1164 | } |
1149 | } |
1165 | |
1150 | |
1166 | |
|
|
1167 | void |
|
|
1168 | tracking_init(void) |
1151 | void tracking_init(void) |
1169 | { |
1152 | { |
1170 | plugin_register_map_type("tracking", tracking_map_new); |
1153 | plugin_register_map_type("tracking", tracking_map_new); |
1171 | } |
1154 | } |