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                 if(destination)
 217                 {
 218                         waypoints.resize(router->get_n_waypoints());
 219                         metrics.resize(waypoints.size()+1);
 220                         metrics[0] = &router->get_metric(-1);
 221                         for(unsigned i=0; i<waypoints.size(); ++i)
 222                         {
 223                                 waypoints[i] = &router->get_waypoint(i);
 224                                 metrics[i+1] = &router->get_metric(i);
 225                         }
 226                         has_duration = router->get_trip_duration();
 227                 }
 228         }
 229
 230         // If no maximum speed is specified, use a sensible default
 231         if(!speed)
 232                 speed = 20*train->get_layout().get_catalogue().get_scale();
 233 }
 234
 235
 236 TrainRoutePlanner::OccupiedTrack::OccupiedTrack(Track &t, unsigned p, OccupiedTrack *n):
 237         track(&t),
 238         path_length(track->get_type().get_path_length(p)),
 239         next(n),
 240         n_tracks(next ? next->n_tracks+1 : 1),
 241         refcount(1)
 242 {
 243         if(next)
 244                 ++next->refcount;
 245 }
 246
 247 TrainRoutePlanner::OccupiedTrack::OccupiedTrack(const OccupiedTrack &other):
 248         track(other.track),
 249         path_length(other.path_length),
 250         next(other.next),
 251         n_tracks(other.n_tracks),
 252         refcount(1)
 253 {
 254         if(next)
 255                 ++next->refcount;
 256 }
 257
 258 TrainRoutePlanner::OccupiedTrack::~OccupiedTrack()
 259 {
 260         if(next && !--next->refcount)
 261                 delete next;
 262 }
 263
 264
 265 TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf):
 266         info(&inf),
 267         occupied_tracks(0),
 268         state(MOVING),
 269         delay(info->router->get_departure_delay()),
 270         duration(info->router->get_trip_duration()),
 271         waypoint(info->waypoints.empty() ? -1 : 0),
 272         blocked_by(-1)
 273 {
 274         const Vehicle *veh = &info->train->get_vehicle(0);
 275         // TODO margins
 276         TrackOffsetIter track_and_offs = veh->get_placement().get_position(VehiclePlacement::FRONT_BUFFER);
 277         track = track_and_offs.track_iter();
 278         offset = track_and_offs.offset();
 279         path = track->get_active_path();
 280
 281         while(Vehicle *next = veh->get_link(1))
 282                 veh = next;
 283         track_and_offs = veh->get_placement().get_position(VehiclePlacement::BACK_BUFFER);
 284         back_offset = track_and_offs.offset();
 285
 286         TrackIter iter = track_and_offs.track_iter();
 287         while(1)
 288         {
 289                 occupied_tracks = new OccupiedTrack(*iter, iter->get_active_path(), occupied_tracks);
 290                 if(iter.track()==track.track())
 291                         break;
 292                 iter = iter.next();
 293         }
 294
 295         update_estimate();
 296 }
 297
 298 TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState &other):
 299         info(other.info),
 300         track(other.track),
 301         path(other.path),
 302         occupied_tracks(other.occupied_tracks),
 303         offset(other.offset),
 304         back_offset(other.back_offset),
 305         state(other.state),
 306         delay(other.delay),
 307         duration(other.duration),
 308         waypoint(other.waypoint),
 309         distance_traveled(other.distance_traveled),
 310         remaining_estimate(other.remaining_estimate),
 311         wait_time(other.wait_time),
 312         blocked_by(other.blocked_by)
 313 {
 314         ++occupied_tracks->refcount;
 315 }
 316
 317 TrainRoutePlanner::TrainRoutingState::~TrainRoutingState()
 318 {
 319         if(occupied_tracks && !--occupied_tracks->refcount)
 320                 delete occupied_tracks;
 321 }
 322
 323 Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() const
 324 {
 325         return ((track->get_type().get_path_length(path)-offset)/info->speed)*Time::sec+delay;
 326 }
 327
 328 bool TrainRoutePlanner::TrainRoutingState::is_occupying(Track &trk) const
 329 {
 330         if(state==ARRIVED && !duration && info->has_duration)
 331                 return false;
 332
 333         OccupiedTrack *occ = occupied_tracks;
 334         for(unsigned n=occ->n_tracks; n>0; --n, occ=occ->next)
 335                 if(occ->track==&trk)
 336                         return true;
 337         return false;
 338 }
 339
 340 bool TrainRoutePlanner::TrainRoutingState::check_arrival()
 341 {
 342         TrackIter next_track = track.next(path);
 343
 344         if(waypoint<0 && info->destination->has_track(*track) && !info->destination->has_track(*next_track))
 345         {
 346                 state = ARRIVED;
 347                 return true;
 348         }
 349         else if(waypoint>=0 && info->waypoints[waypoint]->has_track(*track) && !info->waypoints[waypoint]->has_track(*next_track))
 350         {
 351                 ++waypoint;
 352                 if(waypoint>=static_cast<int>(info->waypoints.size()))
 353                         waypoint = -1;
 354         }
 355
 356         return false;
 357 }
 358
 359 void TrainRoutePlanner::TrainRoutingState::advance(float distance)
 360 {
 361         offset += distance;
 362         back_offset += distance;
 363
 364         OccupiedTrack *last_occ = occupied_tracks;
 365         for(unsigned n=occupied_tracks->n_tracks; n>1; --n)
 366                 last_occ = last_occ->next;
 367
 368         // XXX What if there's multiple tracks to remove?
 369         if(back_offset>last_occ->path_length)
 370         {
 371                 back_offset -= last_occ->path_length;
 372                 if(occupied_tracks->refcount>1)
 373                 {
 374                         --occupied_tracks->refcount;
 375                         occupied_tracks = new OccupiedTrack(*occupied_tracks);
 376                 }
 377                 --occupied_tracks->n_tracks;
 378         }
 379
 380         distance_traveled += distance;
 381         remaining_estimate -= distance;
 382 }
 383
 384 void TrainRoutePlanner::TrainRoutingState::advance(const Time::TimeDelta &dt)
 385 {
 386         if(delay>=dt)
 387         {
 388                 delay -= dt;
 389                 return;
 390         }
 391
 392         float secs = dt/Time::sec;
 393         if(delay)
 394         {
 395                 secs -= delay/Time::sec;
 396                 delay = Time::zero;
 397         }
 398
 399         if(duration)
 400                 duration = max(duration-secs*Time::sec, Time::zero);
 401
 402         if(state==MOVING)
 403                 advance(info->speed*secs);
 404         else if(state!=ARRIVED)
 405                 wait_time += secs*Time::sec;
 406 }
 407
 408 void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path)
 409 {
 410         float distance = occupied_tracks->path_length-offset;
 411         track = track.next(path);
 412         path = next_path;
 413         occupied_tracks = new OccupiedTrack(*track, path, occupied_tracks);
 414         advance(distance);
 415         offset = 0;
 416 }
 417
 418 void TrainRoutePlanner::TrainRoutingState::update_estimate()
 419 {
 420         TrackIter iter = track.reverse(path);
 421         float distance = info->metrics[waypoint+1]->get_distance_from(*iter.track(), iter.entry());
 422         distance += track->get_type().get_path_length(path)-offset;
 423         remaining_estimate = distance;
 424 }
 425
 426
 427 TrainRoutePlanner::RoutingStep::RoutingStep():
 428         prev(0)
 429 { }
 430
 431 TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p):
 432         time(p->time),
 433         penalty(p->penalty),
 434         cost_estimate(p->cost_estimate),
 435         trains(p->trains),
 436         prev(p)
 437 { }
 438
 439 void TrainRoutePlanner::RoutingStep::create_successors(list<RoutingStep> &new_steps) const
 440 {
 441         RoutingStep next(this);
 442         if(next.update_states())
 443         {
 444                 if(next.check_deadlocks())
 445                         return;
 446
 447                 new_steps.push_back(next);
 448                 return;
 449         }
 450
 451         int train_index = find_next_train();
 452         if(train_index<0)
 453                 return;
 454
 455         TrainRoutingState &train = next.trains[train_index];
 456
 457         Time::TimeDelta dt = train.get_time_to_next_track();
 458         next.advance(dt);
 459
 460         if(train.check_arrival())
 461         {
 462                 new_steps.push_back(next);
 463                 return;
 464         }
 465
 466         TrackIter next_track = train.track.next(train.path);
 467         train.advance_track(0);
 468
 469         const TrackType::Endpoint &next_entry_ep = next_track.endpoint();
 470         for(unsigned i=0; next_entry_ep.paths>>i; ++i)
 471                 if(next_entry_ep.has_path(i))
 472                 {
 473                         train.path = i;
 474                         train.update_estimate();
 475                         next.update_estimate();
 476                         if(next.is_viable())
 477                                 new_steps.push_back(next);
 478                 }
 479
 480         new_steps.sort();
 481         for(list<RoutingStep>::iterator i=new_steps.begin(); ++i!=new_steps.end(); )
 482         {
 483                 i->penalty += 5*Time::sec;
 484                 i->update_estimate();
 485         }
 486
 487         if(next_entry_ep.paths!=next_track->get_type().get_paths())
 488         {
 489                 RoutingStep wait(this);
 490                 wait.advance(dt);
 491                 wait.trains[train_index].state = WAITING;
 492                 wait.penalty += 15*Time::sec;
 493                 wait.update_estimate();
 494                 if(wait.is_viable())
 495                         new_steps.push_back(wait);
 496         }
 497 }
 498
 499 bool TrainRoutePlanner::RoutingStep::update_states()
 500 {
 501         bool changes = false;
 502         for(vector<TrainRoutingState>::iterator i=trains.begin(); i!=trains.end(); ++i)
 503         {
 504                 if(i->state==ARRIVED)
 505                         continue;
 506
 507                 TrainState old_state = i->state;
 508
 509                 TrackIter next_track = i->track.next(i->path);
 510                 if(next_track)
 511                 {
 512                         i->blocked_by = get_occupant(*next_track);
 513                         if(i->blocked_by>=0)
 514                                 i->state = BLOCKED;
 515                         else if(i->state==BLOCKED)
 516                                 i->state = MOVING;
 517                 }
 518                 else
 519                         i->state = BLOCKED;
 520
 521                 if(i->state!=old_state)
 522                         changes = true;
 523         }
 524
 525         return changes;
 526 }
 527
 528 bool TrainRoutePlanner::RoutingStep::check_deadlocks() const
 529 {
 530         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 531         {
 532                 if(i->state!=BLOCKED)
 533                         continue;
 534
 535                 if(i->blocked_by<0)
 536                         return true;
 537
 538                 int slow = i->blocked_by;
 539                 int fast = trains[slow].blocked_by;
 540                 while(fast>=0 && trains[fast].blocked_by>=0)
 541                 {
 542                         if(fast==slow)
 543                                 return true;
 544
 545                         slow = trains[slow].blocked_by;
 546                         fast = trains[trains[fast].blocked_by].blocked_by;
 547                 }
 548         }
 549
 550         return false;
 551 }
 552
 553 int TrainRoutePlanner::RoutingStep::get_occupant(Track &track) const
 554 {
 555         for(unsigned i=0; i<trains.size(); ++i)
 556                 if(trains[i].is_occupying(track))
 557                         return i;
 558
 559         return -1;
 560 }
 561
 562 int TrainRoutePlanner::RoutingStep::find_next_train() const
 563 {
 564         Time::TimeDelta min_dt;
 565         int next_train = -1;
 566         for(unsigned i=0; i<trains.size(); ++i)
 567                 if(trains[i].state==MOVING)
 568                 {
 569                         Time::TimeDelta dt = trains[i].get_time_to_next_track();
 570                         if(dt<min_dt || next_train<0)
 571                         {
 572                                 min_dt = dt;
 573                                 next_train = i;
 574                         }
 575                 }
 576
 577         return next_train;
 578 }
 579
 580 void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt)
 581 {
 582         time += dt;
 583         for(vector<TrainRoutingState>::iterator i=trains.begin(); i!=trains.end(); ++i)
 584                 i->advance(dt);
 585 }
 586
 587 void TrainRoutePlanner::RoutingStep::update_estimate()
 588 {
 589         cost_estimate = penalty;
 590         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 591                 if(i->remaining_estimate>=0)
 592                         cost_estimate += i->wait_time+((i->distance_traveled+i->remaining_estimate)/i->info->speed)*Time::sec;
 593 }
 594
 595 bool TrainRoutePlanner::RoutingStep::is_viable() const
 596 {
 597         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 598                 if(i->remaining_estimate<0)
 599                         return false;
 600
 601         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 602                 if(i->state==MOVING)
 603                         return true;
 604
 605         return false;
 606 }
 607
 608 bool TrainRoutePlanner::RoutingStep::is_goal() const
 609 {
 610         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 611                 if(i->state!=ARRIVED)
 612                         return false;
 613         return true;
 614 }
 615
 616 bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const
 617 {
 618         return cost_estimate<other.cost_estimate;
 619 }
 620
 621
 622 TrainRoutePlanner::PlanningThread::PlanningThread(TrainRoutePlanner &p):
 623         planner(p)
 624 {
 625         launch();
 626 }
 627
 628 void TrainRoutePlanner::PlanningThread::main()
 629 {
 630         planner.create_plan();
 631 }
 632
 633 } // namespace R2C2