X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Ftrainrouteplanner.cpp;h=dbdf293c89b03293761b7273adfd13886c218b58;hb=54ab7c88976c860ea729e30a175fbc181f0d68cd;hp=ce38c4e194e193de84a53595621a493dbef2c89b;hpb=1c072afdb1866ba397ee8e6155f5f68c6c7ab4da;p=r2c2.git diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp index ce38c4e..dbdf293 100644 --- a/source/libr2c2/trainrouteplanner.cpp +++ b/source/libr2c2/trainrouteplanner.cpp @@ -1,6 +1,8 @@ +#include "catalogue.h" #include "layout.h" #include "route.h" #include "train.h" +#include "trainroutemetric.h" #include "trainrouteplanner.h" #include "trainrouter.h" #include "vehicle.h" @@ -16,179 +18,160 @@ TrainRoutePlanner::TrainRoutePlanner(Layout &layout) for(map::const_iterator i=trains.begin(); i!=trains.end(); ++i) { TrainRoutingInfo info(*i->second); - if(info.router && info.router->has_destination()) + if(info.router && info.router->get_destination()) routed_trains.push_back(info); } - steps.push_back(RoutingStep()); - RoutingStep &start = steps.back(); + queue.push_back(RoutingStep()); + RoutingStep &start = queue.back(); for(vector::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) start.trains.push_back(TrainRoutingState(*i)); + start.update_estimate(); } void TrainRoutePlanner::plan() { - RoutingStep *goal = 0; - for(list::iterator i=steps.begin(); i!=steps.end(); ++i) + const RoutingStep *goal = 0; + while(!queue.empty()) { - if(i->is_goal()) + const RoutingStep &step = get_step(); + if(step.is_goal()) { - goal = &*i; + goal = &step; break; } - if(update_states(*i)) - { - int next_train = find_next_train(*i); - if(next_train>=0) - add_steps(*i, next_train); - } + add_steps(step); } if(goal) create_routes(*goal); } -bool TrainRoutePlanner::update_states(RoutingStep &step) +const TrainRoutePlanner::RoutingStep &TrainRoutePlanner::get_step() { - RoutingStep next(&step); - bool changes = false; - for(vector::iterator i=next.trains.begin(); i!=next.trains.end(); ++i) + steps.splice(steps.end(), queue, queue.begin()); + return steps.back(); +} + +void TrainRoutePlanner::add_steps(const RoutingStep &step) +{ + list new_steps; + step.create_successors(new_steps); + new_steps.sort(); + queue.merge(new_steps); +} + +void TrainRoutePlanner::create_routes(const RoutingStep &goal) +{ + for(vector::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) { - TrainState old_state = i->state; - if(i->state==BLOCKED) - i->state = MOVING; + Route *route = new Route(i->train->get_layout()); + route->set_name("Router"); + route->set_temporary(true); + i->routes.push_front(route); - TrackIter next_track = i->track.next(i->path); - if(!next_track) - return false; + for(unsigned j=0; j<3; ++j) + i->track_history[j] = 0; + } - for(vector::iterator j=next.trains.begin(); j!=next.trains.end(); ++j) - if(j!=i) + map sequenced_tracks; + unsigned sequence = steps.size(); + for(const RoutingStep *i=&goal; i; i=i->prev) + for(vector::const_iterator j=i->trains.begin(); j!=i->trains.end(); ++j) + { + Track **history = j->info->track_history; + if(j->track.track()==history[0]) + continue; + + Route *route = j->info->routes.front(); + bool start_new_route = route->has_track(*j->track); + if(!start_new_route) { - if(j->track.track()==next_track.track()) + unsigned nls = j->track->get_n_link_slots(); + for(unsigned k=0; (!start_new_route && ktrack.reverse(j->path).entry(); - if(next_track.entry()==other_exit) - return false; + Track *link = j->track->get_link(k); + start_new_route = (link && link!=history[0] && route->has_track(*link)); } - else if(!j->is_occupied(*next_track)) - continue; - - i->state = BLOCKED; } - if(i->state!=old_state) - changes = true; - } - - if(changes) - { - list::iterator i; - for(i=steps.begin(); (i!=steps.end() && !(next<*i)); ++i) ; - steps.insert(i, next); - } - - return !changes; -} - -int TrainRoutePlanner::find_next_train(RoutingStep &step) -{ - Time::TimeDelta min_dt; - int next_train = -1; - for(unsigned i=0; iinfo->train->get_layout()); + route->set_name("Router"); + route->set_temporary(true); + for(unsigned k=2; k>0; --k) + route->add_track(*history[k]); + j->info->routes.push_front(route); } - } - return next_train; -} - -void TrainRoutePlanner::add_steps(RoutingStep &step, unsigned train_index) -{ - TrainRoutingState &train = step.trains[train_index]; - Time::TimeDelta dt = train.get_time_to_next_track(); - TrackIter next_track = train.track.next(train.path); - - list new_steps; - - RoutingStep next(&step); - next.advance(dt); - TrainRouter &router = *train.info->router; - if(router.is_destination(*train.track) && !router.is_destination(*next_track)) - { - next.trains[train_index].state = ARRIVED; - new_steps.push_back(next); - } - else - { - next.trains[train_index].advance_track(0); + if(history[0]) + route->add_track(*history[0]); + for(unsigned k=2; k>0; --k) + history[k] = history[k-1]; + history[0] = j->track.track(); - const TrackType::Endpoint &next_entry_ep = next_track.endpoint(); - for(unsigned i=0; next_entry_ep.paths>>i; ++i) - if(next_entry_ep.has_path(i)) + bool waitable = j->track.endpoint().paths!=j->track->get_type().get_paths(); + map::iterator k = sequenced_tracks.find(j->track.track()); + if(k!=sequenced_tracks.end()) { - next.trains[train_index].path = i; - new_steps.push_back(next); + if(!k->second->preceding) + { + k->second->preceding = j->info; + k->second->sequence_in = sequence; + } + j->info->sequence.push_front(SequencingInfo(j->track.track(), sequence)); + if(waitable) + k->second = &j->info->sequence.front(); + --sequence; + } + else if(waitable) + { + j->info->sequence.push_front(SequencingInfo(j->track.track(), sequence)); + sequenced_tracks[j->track.track()] = &j->info->sequence.front(); + --sequence; } - - if(next_entry_ep.paths!=next_track->get_type().get_paths()) - { - RoutingStep wait(&step); - wait.advance(dt); - wait.trains[train_index].state = WAITING; - new_steps.push_back(wait); } - } - - new_steps.sort(); - steps.merge(new_steps); -} -void TrainRoutePlanner::create_routes(RoutingStep &goal) -{ for(vector::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) { - i->route = new Route(i->train->get_layout()); - i->route->set_name("Router"); - } - - for(RoutingStep *i=&goal; i; i=i->prev) - { - for(vector::iterator j=i->trains.begin(); j!=i->trains.end(); ++j) + for(list::iterator j=i->routes.begin(); j!=i->routes.end(); ++j) { - if(j->state==WAITING || j->state==BLOCKED) - j->info->waits.push_front(&*j); - j->info->route->add_track(*j->track); + if(j==i->routes.begin()) + i->router->set_route(*j); + else + i->router->add_route(**j); } - } - for(vector::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) - { - i->router->set_route(i->route); - TrainRoutingState *current_wait = 0; - for(list::iterator j=i->waits.begin(); j!=i->waits.end(); ++j) - if(!current_wait || (*j)->track.track()!=current_wait->track.track()) - { - Block &block = (*j)->track.next()->get_block(); - i->router->add_wait(block, 0); - current_wait = *j; - } + for(list::iterator j=i->sequence.begin(); j!=i->sequence.end(); ++j) + { + if(j->preceding && j->preceding!=&*i) + i->router->add_sequence_point(j->track->get_block(), *j->preceding->train, j->sequence_in, j->sequence_out); + else + i->router->add_sequence_point(j->track->get_block(), j->sequence_out); + } } } +TrainRoutePlanner::SequencingInfo::SequencingInfo(Track *t, unsigned o): + track(t), + preceding(0), + sequence_in(0), + sequence_out(o) +{ } + + TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t): train(&t), - router(train->get_ai_of_type()), - route(0) -{ } + speed(train->get_maximum_speed()), + router(train->get_ai_of_type()) +{ + // If no maximum speed is specified, use a sensible default + if(!speed) + speed = 20*train->get_layout().get_catalogue().get_scale(); +} TrainRoutePlanner::OccupiedTrack::OccupiedTrack(Track &t, unsigned p, OccupiedTrack *n): @@ -223,37 +206,24 @@ TrainRoutePlanner::OccupiedTrack::~OccupiedTrack() TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf): info(&inf), occupied_tracks(0), - state(MOVING) + state(MOVING), + delay(info->router->get_departure_delay()), + waypoint(info->router->get_n_waypoints() ? 0 : -1), + blocked_by(-1) { const Vehicle *veh = &info->train->get_vehicle(0); - track = TrackIter(veh->get_track(), veh->get_entry()); // TODO margins - offset = veh->get_offset()+veh->get_type().get_length()/2; + TrackOffsetIter track_and_offs = veh->get_placement().get_position(VehiclePlacement::FRONT_BUFFER); + track = track_and_offs.track_iter(); + offset = track_and_offs.offset(); path = track->get_active_path(); - float path_length = track->get_type().get_path_length(path); - while(offset>path_length) - { - offset -= path_length; - track = track.next(); - path = track->get_active_path(); - path_length = track->get_type().get_path_length(path); - } - while(Vehicle *next = veh->get_link(1)) veh = next; - back_offset = veh->get_offset()-veh->get_type().get_length()/2; - - TrackIter iter(veh->get_track(), veh->get_entry()); - while(back_offset<0) - { - TrackIter prev = iter.flip().reverse(); - if(!prev) - break; - iter = prev; - back_offset += iter->get_type().get_path_length(iter->get_active_path()); - } + track_and_offs = veh->get_placement().get_position(VehiclePlacement::BACK_BUFFER); + back_offset = track_and_offs.offset(); + TrackIter iter = track_and_offs.track_iter(); while(1) { occupied_tracks = new OccupiedTrack(*iter, iter->get_active_path(), occupied_tracks); @@ -261,6 +231,8 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf): break; iter = iter.next(); } + + update_estimate(); } TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState &other): @@ -270,24 +242,29 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState occupied_tracks(other.occupied_tracks), offset(other.offset), back_offset(other.back_offset), - state(other.state) + state(other.state), + delay(other.delay), + waypoint(other.waypoint), + distance_traveled(other.distance_traveled), + remaining_estimate(other.remaining_estimate), + wait_time(other.wait_time), + blocked_by(other.blocked_by) { ++occupied_tracks->refcount; } TrainRoutePlanner::TrainRoutingState::~TrainRoutingState() { - if(!--occupied_tracks->refcount) + if(occupied_tracks && !--occupied_tracks->refcount) delete occupied_tracks; } Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() const { - // TODO Consider the speed of the train - return (track->get_type().get_path_length(path)-offset)*Time::sec; + return ((track->get_type().get_path_length(path)-offset)/info->speed)*Time::sec+delay; } -bool TrainRoutePlanner::TrainRoutingState::is_occupied(Track &trk) const +bool TrainRoutePlanner::TrainRoutingState::is_occupying(Track &trk) const { OccupiedTrack *occ = occupied_tracks; for(unsigned n=occ->n_tracks; n>0; --n, occ=occ->next) @@ -296,6 +273,26 @@ bool TrainRoutePlanner::TrainRoutingState::is_occupied(Track &trk) const return false; } +bool TrainRoutePlanner::TrainRoutingState::check_arrival() +{ + TrainRouter &router = *info->router; + TrackIter next_track = track.next(path); + + if(waypoint<0 && router.is_destination(*track) && !router.is_destination(*next_track)) + { + state = ARRIVED; + return true; + } + else if(waypoint>=0 && router.is_waypoint(waypoint, *track) && !router.is_waypoint(waypoint, *next_track)) + { + ++waypoint; + if(waypoint>=static_cast(router.get_n_waypoints())) + waypoint = -1; + } + + return false; +} + void TrainRoutePlanner::TrainRoutingState::advance(float distance) { offset += distance; @@ -316,6 +313,30 @@ void TrainRoutePlanner::TrainRoutingState::advance(float distance) } --occupied_tracks->n_tracks; } + + distance_traveled += distance; + remaining_estimate -= distance; +} + +void TrainRoutePlanner::TrainRoutingState::advance(const Time::TimeDelta &dt) +{ + if(delay>=dt) + { + delay -= dt; + return; + } + + float secs = dt/Time::sec; + if(delay) + { + secs -= delay/Time::sec; + delay = Time::zero; + } + + if(state==MOVING) + advance(info->speed*secs); + else if(state!=ARRIVED) + wait_time += secs*Time::sec; } void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path) @@ -328,26 +349,184 @@ void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path) offset = 0; } +void TrainRoutePlanner::TrainRoutingState::update_estimate() +{ + TrackIter iter = track.reverse(path); + float distance = info->router->get_metric(waypoint).get_distance_from(*iter.track(), iter.entry()); + distance += track->get_type().get_path_length(path)-offset; + remaining_estimate = distance; +} + TrainRoutePlanner::RoutingStep::RoutingStep(): prev(0) { } -TrainRoutePlanner::RoutingStep::RoutingStep(RoutingStep *p): +TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p): time(p->time), + cost_estimate(p->cost_estimate), trains(p->trains), prev(p) { } +void TrainRoutePlanner::RoutingStep::create_successors(list &new_steps) const +{ + RoutingStep next(this); + if(next.update_states()) + { + if(next.check_deadlocks()) + return; + + new_steps.push_back(next); + return; + } + + int train_index = find_next_train(); + if(train_index<0) + return; + + TrainRoutingState &train = next.trains[train_index]; + + Time::TimeDelta dt = train.get_time_to_next_track(); + next.advance(dt); + + if(train.check_arrival()) + { + new_steps.push_back(next); + return; + } + + TrackIter next_track = train.track.next(train.path); + train.advance_track(0); + + const TrackType::Endpoint &next_entry_ep = next_track.endpoint(); + for(unsigned i=0; next_entry_ep.paths>>i; ++i) + if(next_entry_ep.has_path(i)) + { + train.path = i; + train.update_estimate(); + next.update_estimate(); + if(next.is_viable()) + new_steps.push_back(next); + } + + if(next_entry_ep.paths!=next_track->get_type().get_paths()) + { + RoutingStep wait(this); + wait.advance(dt); + wait.trains[train_index].state = WAITING; + if(wait.is_viable()) + new_steps.push_back(wait); + } +} + +bool TrainRoutePlanner::RoutingStep::update_states() +{ + bool changes = false; + for(vector::iterator i=trains.begin(); i!=trains.end(); ++i) + { + if(i->state==ARRIVED) + continue; + + TrainState old_state = i->state; + + TrackIter next_track = i->track.next(i->path); + if(next_track) + { + i->blocked_by = get_occupant(*next_track); + if(i->blocked_by>=0) + i->state = BLOCKED; + else if(i->state==BLOCKED) + i->state = MOVING; + } + else + i->state = BLOCKED; + + if(i->state!=old_state) + changes = true; + } + + return changes; +} + +bool TrainRoutePlanner::RoutingStep::check_deadlocks() const +{ + for(vector::const_iterator i=trains.begin(); i!=trains.end(); ++i) + { + if(i->state!=BLOCKED) + continue; + + if(i->blocked_by<0) + return true; + + int slow = i->blocked_by; + int fast = trains[slow].blocked_by; + while(fast>=0 && trains[fast].blocked_by>=0) + { + if(fast==slow) + return true; + + slow = trains[slow].blocked_by; + fast = trains[trains[fast].blocked_by].blocked_by; + } + } + + return false; +} + +int TrainRoutePlanner::RoutingStep::get_occupant(Track &track) const +{ + for(unsigned i=0; i::iterator i=trains.begin(); i!=trains.end(); ++i) + i->advance(dt); +} + +void TrainRoutePlanner::RoutingStep::update_estimate() +{ + cost_estimate = Time::zero; + for(vector::const_iterator i=trains.begin(); i!=trains.end(); ++i) + if(i->remaining_estimate>=0) + cost_estimate += i->wait_time+((i->distance_traveled+i->remaining_estimate)/i->info->speed)*Time::sec; +} + +bool TrainRoutePlanner::RoutingStep::is_viable() const +{ + for(vector::const_iterator i=trains.begin(); i!=trains.end(); ++i) + if(i->remaining_estimate<0) + return false; + + for(vector::const_iterator i=trains.begin(); i!=trains.end(); ++i) if(i->state==MOVING) - { - float distance = dt/Time::sec; - i->advance(distance); - } + return true; + + return false; } bool TrainRoutePlanner::RoutingStep::is_goal() const @@ -360,7 +539,7 @@ bool TrainRoutePlanner::RoutingStep::is_goal() const bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const { - return time