source/libr2c2/trainrouteplanner.cpp

   1 #include "catalogue.h"
   2 #include "layout.h"
   3 #include "route.h"
   4 #include "train.h"
   5 #include "trainrouteplanner.h"
   6 #include "trainrouter.h"
   7 #include "vehicle.h"
   8
   9 using namespace std;
  10 using namespace Msp;
  11
  12 namespace R2C2 {
  13
  14 TrainRoutePlanner::TrainRoutePlanner(Layout &layout)
  15 {
  16         const map<unsigned, Train *> &trains = layout.get_trains();
  17         for(map<unsigned, Train *>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
  18         {
  19                 TrainRoutingInfo info(*i->second);
  20                 if(info.router && info.router->get_destination())
  21                         routed_trains.push_back(info);
  22         }
  23
  24         queue.push_back(RoutingStep());
  25         RoutingStep &start = queue.back();
  26         for(vector<TrainRoutingInfo>::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i)
  27                 start.trains.push_back(TrainRoutingState(*i));
  28         start.update_estimate();
  29 }
  30
  31 void TrainRoutePlanner::plan()
  32 {
  33         const RoutingStep *goal = 0;
  34         while(!queue.empty())
  35         {
  36                 const RoutingStep &step = get_step();
  37                 if(step.is_goal())
  38                 {
  39                         goal = &step;
  40                         break;
  41                 }
  42
  43                 add_steps(step);
  44         }
  45
  46         if(goal)
  47                 create_routes(*goal);
  48 }
  49
  50 const TrainRoutePlanner::RoutingStep &TrainRoutePlanner::get_step()
  51 {
  52         steps.splice(steps.end(), queue, queue.begin());
  53         return steps.back();
  54 }
  55
  56 void TrainRoutePlanner::add_steps(const RoutingStep &step)
  57 {
  58         list<RoutingStep> new_steps;
  59         step.create_successors(new_steps);
  60         new_steps.sort();
  61         queue.merge(new_steps);
  62 }
  63
  64 void TrainRoutePlanner::create_routes(const RoutingStep &goal)
  65 {
  66         for(vector<TrainRoutingInfo>::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i)
  67         {
  68                 i->route = new Route(i->train->get_layout());
  69                 i->route->set_name("Router");
  70                 i->route->set_temporary(true);
  71         }
  72
  73         for(const RoutingStep *i=&goal; i; i=i->prev)
  74         {
  75                 for(vector<TrainRoutingState>::const_iterator j=i->trains.begin(); j!=i->trains.end(); ++j)
  76                 {
  77                         if(j->state==WAITING || j->state==BLOCKED)
  78                                 j->info->waits.push_front(&*j);
  79                         j->info->route->add_track(*j->track);
  80                 }
  81         }
  82
  83         for(vector<TrainRoutingInfo>::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i)
  84         {
  85                 i->router->set_route(i->route);
  86                 const TrainRoutingState *current_wait = 0;
  87                 for(list<const TrainRoutingState *>::const_iterator j=i->waits.begin(); j!=i->waits.end(); ++j)
  88                         if(!current_wait || (*j)->track.track()!=current_wait->track.track())
  89                         {
  90                                 Block &block = (*j)->track.next()->get_block();
  91                                 i->router->add_wait(block, 0);
  92                                 current_wait = *j;
  93                         }
  94         }
  95 }
  96
  97
  98 TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t):
  99         train(&t),
 100         speed(train->get_maximum_speed()),
 101         router(train->get_ai_of_type<TrainRouter>()),
 102         route(0)
 103 {
 104         // If no maximum speed is specified, use a sensible default
 105         if(!speed)
 106                 speed = 20*train->get_layout().get_catalogue().get_scale();
 107 }
 108
 109
 110 TrainRoutePlanner::OccupiedTrack::OccupiedTrack(Track &t, unsigned p, OccupiedTrack *n):
 111         track(&t),
 112         path_length(track->get_type().get_path_length(p)),
 113         next(n),
 114         n_tracks(next ? next->n_tracks+1 : 1),
 115         refcount(1)
 116 {
 117         if(next)
 118                 ++next->refcount;
 119 }
 120
 121 TrainRoutePlanner::OccupiedTrack::OccupiedTrack(const OccupiedTrack &other):
 122         track(other.track),
 123         path_length(other.path_length),
 124         next(other.next),
 125         n_tracks(other.n_tracks),
 126         refcount(1)
 127 {
 128         if(next)
 129                 ++next->refcount;
 130 }
 131
 132 TrainRoutePlanner::OccupiedTrack::~OccupiedTrack()
 133 {
 134         if(next && !--next->refcount)
 135                 delete next;
 136 }
 137
 138
 139 TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf):
 140         info(&inf),
 141         occupied_tracks(0),
 142         state(MOVING),
 143         delay(info->router->get_departure_delay()),
 144         waypoint(info->router->get_n_waypoints() ? 0 : -1),
 145         blocked_by(-1)
 146 {
 147         const Vehicle *veh = &info->train->get_vehicle(0);
 148         // TODO margins
 149         TrackOffsetIter track_and_offs = veh->get_placement().get_position(VehiclePlacement::FRONT_BUFFER);
 150         track = track_and_offs.track_iter();
 151         offset = track_and_offs.offset();
 152         path = track->get_active_path();
 153
 154         while(Vehicle *next = veh->get_link(1))
 155                 veh = next;
 156         track_and_offs = veh->get_placement().get_position(VehiclePlacement::BACK_BUFFER);
 157         back_offset = track_and_offs.offset();
 158
 159         TrackIter iter = track_and_offs.track_iter();
 160         while(1)
 161         {
 162                 occupied_tracks = new OccupiedTrack(*iter, iter->get_active_path(), occupied_tracks);
 163                 if(iter.track()==track.track())
 164                         break;
 165                 iter = iter.next();
 166         }
 167 }
 168
 169 TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState &other):
 170         info(other.info),
 171         track(other.track),
 172         path(other.path),
 173         occupied_tracks(other.occupied_tracks),
 174         offset(other.offset),
 175         back_offset(other.back_offset),
 176         state(other.state),
 177         delay(other.delay),
 178         waypoint(other.waypoint),
 179         blocked_by(other.blocked_by)
 180 {
 181         ++occupied_tracks->refcount;
 182 }
 183
 184 TrainRoutePlanner::TrainRoutingState::~TrainRoutingState()
 185 {
 186         if(occupied_tracks && !--occupied_tracks->refcount)
 187                 delete occupied_tracks;
 188 }
 189
 190 Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() const
 191 {
 192         return ((track->get_type().get_path_length(path)-offset)/info->speed)*Time::sec+delay;
 193 }
 194
 195 bool TrainRoutePlanner::TrainRoutingState::is_occupying(Track &trk) const
 196 {
 197         OccupiedTrack *occ = occupied_tracks;
 198         for(unsigned n=occ->n_tracks; n>0; --n, occ=occ->next)
 199                 if(occ->track==&trk)
 200                         return true;
 201         return false;
 202 }
 203
 204 bool TrainRoutePlanner::TrainRoutingState::check_arrival()
 205 {
 206         TrainRouter &router = *info->router;
 207         TrackIter next_track = track.next(path);
 208
 209         if(waypoint<0 && router.is_destination(*track) && !router.is_destination(*next_track))
 210         {
 211                 state = ARRIVED;
 212                 return true;
 213         }
 214         else if(waypoint>=0 && router.is_waypoint(waypoint, *track) && !router.is_waypoint(waypoint, *next_track))
 215         {
 216                 ++waypoint;
 217                 if(waypoint>=static_cast<int>(router.get_n_waypoints()))
 218                         waypoint = -1;
 219         }
 220
 221         return false;
 222 }
 223
 224 void TrainRoutePlanner::TrainRoutingState::advance(float distance)
 225 {
 226         offset += distance;
 227         back_offset += distance;
 228
 229         OccupiedTrack *last_occ = occupied_tracks;
 230         for(unsigned n=occupied_tracks->n_tracks; n>1; --n)
 231                 last_occ = last_occ->next;
 232
 233         // XXX What if there's multiple tracks to remove?
 234         if(back_offset>last_occ->path_length)
 235         {
 236                 back_offset -= last_occ->path_length;
 237                 if(occupied_tracks->refcount>1)
 238                 {
 239                         --occupied_tracks->refcount;
 240                         occupied_tracks = new OccupiedTrack(*occupied_tracks);
 241                 }
 242                 --occupied_tracks->n_tracks;
 243         }
 244 }
 245
 246 void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path)
 247 {
 248         float distance = occupied_tracks->path_length-offset;
 249         track = track.next(path);
 250         path = next_path;
 251         occupied_tracks = new OccupiedTrack(*track, path, occupied_tracks);
 252         advance(distance);
 253         offset = 0;
 254 }
 255
 256
 257 TrainRoutePlanner::RoutingStep::RoutingStep():
 258         prev(0)
 259 { }
 260
 261 TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p):
 262         time(p->time),
 263         trains(p->trains),
 264         prev(p)
 265 { }
 266
 267 void TrainRoutePlanner::RoutingStep::create_successors(list<RoutingStep> &new_steps) const
 268 {
 269         RoutingStep next(this);
 270         if(next.update_states())
 271         {
 272                 if(next.check_deadlocks())
 273                         return;
 274
 275                 new_steps.push_back(next);
 276                 return;
 277         }
 278
 279         int train_index = find_next_train();
 280         if(train_index<0)
 281                 return;
 282
 283         TrainRoutingState &train = next.trains[train_index];
 284
 285         Time::TimeDelta dt = train.get_time_to_next_track();
 286         next.advance(dt);
 287
 288         if(train.check_arrival())
 289         {
 290                 new_steps.push_back(next);
 291                 return;
 292         }
 293
 294         TrackIter next_track = train.track.next(train.path);
 295         train.advance_track(0);
 296
 297         const TrackType::Endpoint &next_entry_ep = next_track.endpoint();
 298         for(unsigned i=0; next_entry_ep.paths>>i; ++i)
 299                 if(next_entry_ep.has_path(i))
 300                 {
 301                         train.path = i;
 302                         if(next.is_viable())
 303                                 new_steps.push_back(next);
 304                 }
 305
 306         if(next_entry_ep.paths!=next_track->get_type().get_paths())
 307         {
 308                 RoutingStep wait(this);
 309                 wait.advance(dt);
 310                 wait.trains[train_index].state = WAITING;
 311                 if(wait.is_viable())
 312                         new_steps.push_back(wait);
 313         }
 314 }
 315
 316 bool TrainRoutePlanner::RoutingStep::update_states()
 317 {
 318         bool changes = false;
 319         for(vector<TrainRoutingState>::iterator i=trains.begin(); i!=trains.end(); ++i)
 320         {
 321                 if(i->state==ARRIVED)
 322                         continue;
 323
 324                 TrainState old_state = i->state;
 325
 326                 TrackIter next_track = i->track.next(i->path);
 327                 if(next_track)
 328                 {
 329                         i->blocked_by = get_occupant(*next_track);
 330                         if(i->blocked_by>=0)
 331                                 i->state = BLOCKED;
 332                         else if(i->state==BLOCKED)
 333                                 i->state = MOVING;
 334                 }
 335                 else
 336                         i->state = BLOCKED;
 337
 338                 if(i->state!=old_state)
 339                         changes = true;
 340         }
 341
 342         return changes;
 343 }
 344
 345 bool TrainRoutePlanner::RoutingStep::check_deadlocks() const
 346 {
 347         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 348         {
 349                 if(i->state!=BLOCKED)
 350                         continue;
 351
 352                 if(i->blocked_by<0)
 353                         return true;
 354
 355                 int slow = i->blocked_by;
 356                 int fast = trains[slow].blocked_by;
 357                 while(fast>=0 && trains[fast].blocked_by>=0)
 358                 {
 359                         if(fast==slow)
 360                                 return true;
 361
 362                         slow = trains[slow].blocked_by;
 363                         fast = trains[trains[fast].blocked_by].blocked_by;
 364                 }
 365         }
 366
 367         return false;
 368 }
 369
 370 int TrainRoutePlanner::RoutingStep::get_occupant(Track &track) const
 371 {
 372         for(unsigned i=0; i<trains.size(); ++i)
 373                 if(trains[i].is_occupying(track))
 374                         return i;
 375
 376         return -1;
 377 }
 378
 379 int TrainRoutePlanner::RoutingStep::find_next_train() const
 380 {
 381         Time::TimeDelta min_dt;
 382         int next_train = -1;
 383         for(unsigned i=0; i<trains.size(); ++i)
 384                 if(trains[i].state==MOVING)
 385                 {
 386                         Time::TimeDelta dt = trains[i].get_time_to_next_track();
 387                         if(dt<min_dt || next_train<0)
 388                         {
 389                                 min_dt = dt;
 390                                 next_train = i;
 391                         }
 392                 }
 393
 394         return next_train;
 395 }
 396
 397 void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt)
 398 {
 399         time += dt;
 400         for(vector<TrainRoutingState>::iterator i=trains.begin(); i!=trains.end(); ++i)
 401         {
 402                 if(i->delay)
 403                 {
 404                         i->delay -= dt;
 405                         if(i->delay>Time::zero)
 406                                 continue;
 407                         i->delay = Time::zero;
 408                 }
 409                 else if(i->state==MOVING)
 410                         i->advance(i->info->speed*(dt/Time::sec));
 411         }
 412 }
 413
 414 bool TrainRoutePlanner::RoutingStep::is_viable() const
 415 {
 416         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 417                 if(i->state==MOVING)
 418                         return true;
 419
 420         return false;
 421 }
 422
 423 bool TrainRoutePlanner::RoutingStep::is_goal() const
 424 {
 425         for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
 426                 if(i->state!=ARRIVED)
 427                         return false;
 428         return true;
 429 }
 430
 431 bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const
 432 {
 433         return time<other.time;
 434 }
 435
 436 } // namespace R2C2