]> git.tdb.fi Git - r2c2.git/blob - source/libr2c2/trainrouteplanner.cpp
Store routing information in TrainRoutePlanner to avoid threading problems
[r2c2.git] / source / libr2c2 / trainrouteplanner.cpp
1 #include <msp/core/maputils.h>
2 #include "catalogue.h"
3 #include "layout.h"
4 #include "route.h"
5 #include "train.h"
6 #include "trainroutemetric.h"
7 #include "trainrouteplanner.h"
8 #include "trainrouter.h"
9 #include "vehicle.h"
10
11 using namespace std;
12 using namespace Msp;
13
14 namespace R2C2 {
15
16 TrainRoutePlanner::TrainRoutePlanner(Layout &layout):
17         goal(0),
18         result(PENDING),
19         thread(0)
20 {
21         const map<unsigned, Train *> &trains = layout.get_trains();
22         for(map<unsigned, Train *>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
23         {
24                 TrainRoutingInfo info(*i->second);
25                 if(info.destination)
26                         routed_trains.push_back(info);
27         }
28 }
29
30 TrainRoutePlanner::~TrainRoutePlanner()
31 {
32         delete thread;
33 }
34
35 TrainRoutePlanner::Result TrainRoutePlanner::plan()
36 {
37         prepare_plan();
38         create_plan();
39         if(result==PENDING)
40                 finalize_plan();
41
42         return result;
43 }
44
45 void TrainRoutePlanner::plan_async()
46 {
47         if(thread)
48                 throw logic_error("already planning");
49
50         prepare_plan();
51         thread = new PlanningThread(*this);
52 }
53
54 TrainRoutePlanner::Result TrainRoutePlanner::check()
55 {
56         if(result==PENDING && goal)
57         {
58                 finalize_plan();
59                 delete thread;
60                 thread = 0;
61         }
62
63         return result;
64 }
65
66 const list<Route *> &TrainRoutePlanner::get_routes_for(const Train &train) const
67 {
68         return get_train_info(train).routes;
69 }
70
71 const list<TrainRouter::SequencePoint> &TrainRoutePlanner::get_sequence_for(const Train &train) const
72 {
73         return get_train_info(train).sequence;
74 }
75
76 const TrainRoutePlanner::TrainRoutingInfo &TrainRoutePlanner::get_train_info(const Train &train) const
77 {
78         for(vector<TrainRoutingInfo>::const_iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i)
79                 if(i->train==&train)
80                         return *i;
81
82         throw key_error(train.get_name());
83 }
84
85 const TrainRoutePlanner::RoutingStep &TrainRoutePlanner::get_step()
86 {
87         steps.splice(steps.end(), queue, queue.begin());
88         return steps.back();
89 }
90
91 void TrainRoutePlanner::prepare_plan()
92 {
93         steps.clear();
94         queue.clear();
95         goal = 0;
96         result = PENDING;
97
98         queue.push_back(RoutingStep());
99         RoutingStep &start = queue.back();
100         for(vector<TrainRoutingInfo>::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i)
101                 start.trains.push_back(TrainRoutingState(*i));
102         start.update_estimate();
103 }
104
105 void TrainRoutePlanner::create_plan()
106 {
107         while(!queue.empty())
108         {
109                 const RoutingStep &step = get_step();
110                 if(step.is_goal())
111                 {
112                         goal = &step;
113                         return;
114                 }
115
116                 add_steps(step);
117         }
118
119         result = FAILED;
120 }
121
122 void TrainRoutePlanner::add_steps(const RoutingStep &step)
123 {
124         list<RoutingStep> new_steps;
125         step.create_successors(new_steps);
126         new_steps.sort();
127         queue.merge(new_steps);
128 }
129
130 void TrainRoutePlanner::finalize_plan()
131 {
132         for(vector<TrainRoutingInfo>::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i)
133         {
134                 i->routes.clear();
135                 i->sequence.clear();
136                 for(unsigned j=0; j<2; ++j)
137                         i->track_history[j] = 0;
138         }
139
140         map<Track *, TrainRouter::SequencePoint *> sequenced_tracks;
141         unsigned sequence = steps.size();
142         for(const RoutingStep *i=goal; i; i=i->prev)
143                 for(vector<TrainRoutingState>::const_iterator j=i->trains.begin(); j!=i->trains.end(); ++j)
144                 {
145                         Track **history = j->info->track_history;
146                         if(j->track.track()==history[0])
147                                 continue;
148
149                         Route *route = 0;
150                         bool start_new_route = true;
151                         if(!j->info->routes.empty())
152                         {
153                                 route = j->info->routes.front();
154                                 start_new_route = route->has_track(*j->track);
155                                 if(!start_new_route)
156                                 {
157                                         unsigned nls = j->track->get_n_link_slots();
158                                         for(unsigned k=0; (!start_new_route && k<nls); ++k)
159                                         {
160                                                 Track *link = j->track->get_link(k);
161                                                 start_new_route = (link && link!=history[0] && route->has_track(*link));
162                                         }
163                                 }
164                         }
165
166                         if(start_new_route)
167                         {
168                                 route = new Route(j->info->train->get_layout());
169                                 route->set_name("Router");
170                                 route->set_temporary(true);
171                                 for(unsigned k=0; (k<2 && history[k]); ++k)
172                                         route->add_track(*history[k]);
173                                 j->info->routes.push_front(route);
174                         }
175
176                         route->add_track(*j->track.track());
177                         history[1] = history[0];
178                         history[0] = j->track.track();
179
180                         bool waitable = j->track.endpoint().paths!=j->track->get_type().get_paths();
181                         map<Track *, TrainRouter::SequencePoint *>::iterator k = sequenced_tracks.find(j->track.track());
182                         if(k!=sequenced_tracks.end())
183                         {
184                                 if(!k->second->preceding_train)
185                                 {
186                                         k->second->preceding_train = j->info->train;
187                                         k->second->sequence_in = sequence;
188                                 }
189                                 j->info->sequence.push_front(TrainRouter::SequencePoint(j->track->get_block(), sequence));
190                                 if(waitable)
191                                         k->second = &j->info->sequence.front();
192                                 --sequence;
193                         }
194                         else if(waitable)
195                         {
196                                 j->info->sequence.push_front(TrainRouter::SequencePoint(j->track->get_block(), sequence));
197                                 sequenced_tracks[j->track.track()] = &j->info->sequence.front();
198                                 --sequence;
199                         }
200                 }
201
202         result = COMPLETE;
203 }
204
205
206 TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t):
207         train(&t),
208         speed(train->get_maximum_speed()),
209         router(train->get_ai_of_type<TrainRouter>()),
210         destination(0),
211         has_duration(false)
212 {
213         if(router)
214         {
215                 destination = router->get_destination();
216                 waypoints.resize(router->get_n_waypoints());
217                 metrics.resize(waypoints.size()+1);
218                 metrics[0] = &router->get_metric(-1);
219                 for(unsigned i=0; i<waypoints.size(); ++i)
220                 {
221                         waypoints[i] = &router->get_waypoint(i);
222                         metrics[i+1] = &router->get_metric(i);
223                 }
224                 has_duration = router->get_trip_duration();
225         }
226
227         // If no maximum speed is specified, use a sensible default
228         if(!speed)
229                 speed = 20*train->get_layout().get_catalogue().get_scale();
230 }
231
232
233 TrainRoutePlanner::OccupiedTrack::OccupiedTrack(Track &t, unsigned p, OccupiedTrack *n):
234         track(&t),
235         path_length(track->get_type().get_path_length(p)),
236         next(n),
237         n_tracks(next ? next->n_tracks+1 : 1),
238         refcount(1)
239 {
240         if(next)
241                 ++next->refcount;
242 }
243
244 TrainRoutePlanner::OccupiedTrack::OccupiedTrack(const OccupiedTrack &other):
245         track(other.track),
246         path_length(other.path_length),
247         next(other.next),
248         n_tracks(other.n_tracks),
249         refcount(1)
250 {
251         if(next)
252                 ++next->refcount;
253 }
254
255 TrainRoutePlanner::OccupiedTrack::~OccupiedTrack()
256 {
257         if(next && !--next->refcount)
258                 delete next;
259 }
260
261
262 TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf):
263         info(&inf),
264         occupied_tracks(0),
265         state(MOVING),
266         delay(info->router->get_departure_delay()),
267         duration(info->router->get_trip_duration()),
268         waypoint(info->waypoints.empty() ? -1 : 0),
269         blocked_by(-1)
270 {
271         const Vehicle *veh = &info->train->get_vehicle(0);
272         // TODO margins
273         TrackOffsetIter track_and_offs = veh->get_placement().get_position(VehiclePlacement::FRONT_BUFFER);
274         track = track_and_offs.track_iter();
275         offset = track_and_offs.offset();
276         path = track->get_active_path();
277
278         while(Vehicle *next = veh->get_link(1))
279                 veh = next;
280         track_and_offs = veh->get_placement().get_position(VehiclePlacement::BACK_BUFFER);
281         back_offset = track_and_offs.offset();
282
283         TrackIter iter = track_and_offs.track_iter();
284         while(1)
285         {
286                 occupied_tracks = new OccupiedTrack(*iter, iter->get_active_path(), occupied_tracks);
287                 if(iter.track()==track.track())
288                         break;
289                 iter = iter.next();
290         }
291
292         update_estimate();
293 }
294
295 TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState &other):
296         info(other.info),
297         track(other.track),
298         path(other.path),
299         occupied_tracks(other.occupied_tracks),
300         offset(other.offset),
301         back_offset(other.back_offset),
302         state(other.state),
303         delay(other.delay),
304         duration(other.duration),
305         waypoint(other.waypoint),
306         distance_traveled(other.distance_traveled),
307         remaining_estimate(other.remaining_estimate),
308         wait_time(other.wait_time),
309         blocked_by(other.blocked_by)
310 {
311         ++occupied_tracks->refcount;
312 }
313
314 TrainRoutePlanner::TrainRoutingState::~TrainRoutingState()
315 {
316         if(occupied_tracks && !--occupied_tracks->refcount)
317                 delete occupied_tracks;
318 }
319
320 Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() const
321 {
322         return ((track->get_type().get_path_length(path)-offset)/info->speed)*Time::sec+delay;
323 }
324
325 bool TrainRoutePlanner::TrainRoutingState::is_occupying(Track &trk) const
326 {
327         if(state==ARRIVED && !duration && info->has_duration)
328                 return false;
329
330         OccupiedTrack *occ = occupied_tracks;
331         for(unsigned n=occ->n_tracks; n>0; --n, occ=occ->next)
332                 if(occ->track==&trk)
333                         return true;
334         return false;
335 }
336
337 bool TrainRoutePlanner::TrainRoutingState::check_arrival()
338 {
339         TrackIter next_track = track.next(path);
340
341         if(waypoint<0 && info->destination->has_track(*track) && !info->destination->has_track(*next_track))
342         {
343                 state = ARRIVED;
344                 return true;
345         }
346         else if(waypoint>=0 && info->waypoints[waypoint]->has_track(*track) && !info->waypoints[waypoint]->has_track(*next_track))
347         {
348                 ++waypoint;
349                 if(waypoint>=static_cast<int>(info->waypoints.size()))
350                         waypoint = -1;
351         }
352
353         return false;
354 }
355
356 void TrainRoutePlanner::TrainRoutingState::advance(float distance)
357 {
358         offset += distance;
359         back_offset += distance;
360
361         OccupiedTrack *last_occ = occupied_tracks;
362         for(unsigned n=occupied_tracks->n_tracks; n>1; --n)
363                 last_occ = last_occ->next;
364
365         // XXX What if there's multiple tracks to remove?
366         if(back_offset>last_occ->path_length)
367         {
368                 back_offset -= last_occ->path_length;
369                 if(occupied_tracks->refcount>1)
370                 {
371                         --occupied_tracks->refcount;
372                         occupied_tracks = new OccupiedTrack(*occupied_tracks);
373                 }
374                 --occupied_tracks->n_tracks;
375         }
376
377         distance_traveled += distance;
378         remaining_estimate -= distance;
379 }
380
381 void TrainRoutePlanner::TrainRoutingState::advance(const Time::TimeDelta &dt)
382 {
383         if(delay>=dt)
384         {
385                 delay -= dt;
386                 return;
387         }
388
389         float secs = dt/Time::sec;
390         if(delay)
391         {
392                 secs -= delay/Time::sec;
393                 delay = Time::zero;
394         }
395
396         if(duration)
397                 duration = max(duration-secs*Time::sec, Time::zero);
398
399         if(state==MOVING)
400                 advance(info->speed*secs);
401         else if(state!=ARRIVED)
402                 wait_time += secs*Time::sec;
403 }
404
405 void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path)
406 {
407         float distance = occupied_tracks->path_length-offset;
408         track = track.next(path);
409         path = next_path;
410         occupied_tracks = new OccupiedTrack(*track, path, occupied_tracks);
411         advance(distance);
412         offset = 0;
413 }
414
415 void TrainRoutePlanner::TrainRoutingState::update_estimate()
416 {
417         TrackIter iter = track.reverse(path);
418         float distance = info->metrics[waypoint+1]->get_distance_from(*iter.track(), iter.entry());
419         distance += track->get_type().get_path_length(path)-offset;
420         remaining_estimate = distance;
421 }
422
423
424 TrainRoutePlanner::RoutingStep::RoutingStep():
425         prev(0)
426 { }
427
428 TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p):
429         time(p->time),
430         cost_estimate(p->cost_estimate),
431         trains(p->trains),
432         prev(p)
433 { }
434
435 void TrainRoutePlanner::RoutingStep::create_successors(list<RoutingStep> &new_steps) const
436 {
437         RoutingStep next(this);
438         if(next.update_states())
439         {
440                 if(next.check_deadlocks())
441                         return;
442
443                 new_steps.push_back(next);
444                 return;
445         }
446
447         int train_index = find_next_train();
448         if(train_index<0)
449                 return;
450
451         TrainRoutingState &train = next.trains[train_index];
452
453         Time::TimeDelta dt = train.get_time_to_next_track();
454         next.advance(dt);
455
456         if(train.check_arrival())
457         {
458                 new_steps.push_back(next);
459                 return;
460         }
461
462         TrackIter next_track = train.track.next(train.path);
463         train.advance_track(0);
464
465         const TrackType::Endpoint &next_entry_ep = next_track.endpoint();
466         for(unsigned i=0; next_entry_ep.paths>>i; ++i)
467                 if(next_entry_ep.has_path(i))
468                 {
469                         train.path = i;
470                         train.update_estimate();
471                         next.update_estimate();
472                         if(next.is_viable())
473                                 new_steps.push_back(next);
474                 }
475
476         if(next_entry_ep.paths!=next_track->get_type().get_paths())
477         {
478                 RoutingStep wait(this);
479                 wait.advance(dt);
480                 wait.trains[train_index].state = WAITING;
481                 if(wait.is_viable())
482                         new_steps.push_back(wait);
483         }
484 }
485
486 bool TrainRoutePlanner::RoutingStep::update_states()
487 {
488         bool changes = false;
489         for(vector<TrainRoutingState>::iterator i=trains.begin(); i!=trains.end(); ++i)
490         {
491                 if(i->state==ARRIVED)
492                         continue;
493
494                 TrainState old_state = i->state;
495
496                 TrackIter next_track = i->track.next(i->path);
497                 if(next_track)
498                 {
499                         i->blocked_by = get_occupant(*next_track);
500                         if(i->blocked_by>=0)
501                                 i->state = BLOCKED;
502                         else if(i->state==BLOCKED)
503                                 i->state = MOVING;
504                 }
505                 else
506                         i->state = BLOCKED;
507
508                 if(i->state!=old_state)
509                         changes = true;
510         }
511
512         return changes;
513 }
514
515 bool TrainRoutePlanner::RoutingStep::check_deadlocks() const
516 {
517         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
518         {
519                 if(i->state!=BLOCKED)
520                         continue;
521
522                 if(i->blocked_by<0)
523                         return true;
524
525                 int slow = i->blocked_by;
526                 int fast = trains[slow].blocked_by;
527                 while(fast>=0 && trains[fast].blocked_by>=0)
528                 {
529                         if(fast==slow)
530                                 return true;
531
532                         slow = trains[slow].blocked_by;
533                         fast = trains[trains[fast].blocked_by].blocked_by;
534                 }
535         }
536
537         return false;
538 }
539
540 int TrainRoutePlanner::RoutingStep::get_occupant(Track &track) const
541 {
542         for(unsigned i=0; i<trains.size(); ++i)
543                 if(trains[i].is_occupying(track))
544                         return i;
545
546         return -1;
547 }
548
549 int TrainRoutePlanner::RoutingStep::find_next_train() const
550 {
551         Time::TimeDelta min_dt;
552         int next_train = -1;
553         for(unsigned i=0; i<trains.size(); ++i)
554                 if(trains[i].state==MOVING)
555                 {
556                         Time::TimeDelta dt = trains[i].get_time_to_next_track();
557                         if(dt<min_dt || next_train<0)
558                         {
559                                 min_dt = dt;
560                                 next_train = i;
561                         }
562                 }
563
564         return next_train;
565 }
566
567 void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt)
568 {
569         time += dt;
570         for(vector<TrainRoutingState>::iterator i=trains.begin(); i!=trains.end(); ++i)
571                 i->advance(dt);
572 }
573
574 void TrainRoutePlanner::RoutingStep::update_estimate()
575 {
576         cost_estimate = Time::zero;
577         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
578                 if(i->remaining_estimate>=0)
579                         cost_estimate += i->wait_time+((i->distance_traveled+i->remaining_estimate)/i->info->speed)*Time::sec;
580 }
581
582 bool TrainRoutePlanner::RoutingStep::is_viable() const
583 {
584         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
585                 if(i->remaining_estimate<0)
586                         return false;
587
588         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
589                 if(i->state==MOVING)
590                         return true;
591
592         return false;
593 }
594
595 bool TrainRoutePlanner::RoutingStep::is_goal() const
596 {
597         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
598                 if(i->state!=ARRIVED)
599                         return false;
600         return true;
601 }
602
603 bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const
604 {
605         return cost_estimate<other.cost_estimate;
606 }
607
608
609 TrainRoutePlanner::PlanningThread::PlanningThread(TrainRoutePlanner &p):
610         planner(p)
611 {
612         launch();
613 }
614
615 void TrainRoutePlanner::PlanningThread::main()
616 {
617         planner.create_plan();
618 }
619
620 } // namespace R2C2