source/libr2c2/trainrouteplanner.cpp

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