X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Ftrainrouteplanner.cpp;h=ad97cf79f77c0255f23b9c1be7ed5cf0cfe58e69;hb=5b71cb905051d654c150cb0e098cade0cb502105;hp=ca85ac74084dace04e65d24d48cec278b9c67350;hpb=862d8bea6d31e6fe9a60747e0b9559f77e72bf44;p=r2c2.git diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp index ca85ac7..ad97cf7 100644 --- a/source/libr2c2/trainrouteplanner.cpp +++ b/source/libr2c2/trainrouteplanner.cpp @@ -1,4 +1,5 @@ #include +#include #include "catalogue.h" #include "layout.h" #include "route.h" @@ -15,6 +16,8 @@ namespace R2C2 { TrainRoutePlanner::TrainRoutePlanner(Layout &layout): goal(0), + path_switch_bias(15*Time::sec), + timeout(10*Time::sec), result(PENDING), thread(0) { @@ -36,6 +39,11 @@ TrainRoutePlanner::~TrainRoutePlanner() } } +void TrainRoutePlanner::set_timeout(const Time::TimeDelta &t) +{ + timeout = t; +} + TrainRoutePlanner::Result TrainRoutePlanner::plan() { prepare_plan(); @@ -112,6 +120,8 @@ void TrainRoutePlanner::prepare_plan() void TrainRoutePlanner::create_plan() { + Time::TimeStamp timeout_stamp = Time::now()+timeout; + unsigned count = 0; while(!queue.empty()) { const RoutingStep &step = get_step(); @@ -122,6 +132,13 @@ void TrainRoutePlanner::create_plan() } add_steps(step); + + if(++count>=1000) + { + if(Time::now()>timeout_stamp) + break; + count = 0; + } } result = FAILED; @@ -131,7 +148,12 @@ void TrainRoutePlanner::add_steps(const RoutingStep &step) { list new_steps; step.create_successors(new_steps); + if(new_steps.empty()) + return; + new_steps.sort(); + if(!queue.empty() && new_steps.front().cost_estimate::const_iterator j=i->trains.begin(); j!=i->trains.end(); ++j) { Track **history = j->info->track_history; + // Don't process the same track again. if(j->track.track()==history[0]) continue; @@ -158,6 +181,8 @@ void TrainRoutePlanner::finalize_plan() bool start_new_route = true; if(!j->info->routes.empty()) { + /* If we already have a route and this track or any linked track is + in it, start a new one to avoid loops. */ route = j->info->routes.front(); start_new_route = route->has_track(*j->track); if(!start_new_route) @@ -176,6 +201,8 @@ void TrainRoutePlanner::finalize_plan() route = new Route(j->info->train->get_layout()); route->set_name("Router"); route->set_temporary(true); + /* Have the routes overlap by two tracks to ensure that turnout + paths can be deduced. */ for(unsigned k=0; (k<2 && history[k]); ++k) route->add_track(*history[k]); j->info->routes.push_front(route); @@ -189,6 +216,7 @@ void TrainRoutePlanner::finalize_plan() map::iterator k = sequenced_tracks.find(j->track.track()); if(k!=sequenced_tracks.end()) { + // Add a sequence point if another train uses this track afterwards. if(!k->second->preceding_train) { k->second->preceding_train = j->info->train; @@ -201,6 +229,8 @@ void TrainRoutePlanner::finalize_plan() } else if(waitable) { + /* Create a sequence point if it's possible to wait and let another + train past. */ j->info->sequence.push_front(TrainRouter::SequencePoint(j->track->get_block(), sequence)); sequenced_tracks[j->track.track()] = &j->info->sequence.front(); --sequence; @@ -213,23 +243,28 @@ void TrainRoutePlanner::finalize_plan() TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t): train(&t), + length(0), speed(train->get_maximum_speed()), first_noncritical(train->get_last_critical_block().next().block()), router(train->get_ai_of_type()), - waypoints(router ? router->get_n_waypoints() : 0), has_duration(false) { - if(!waypoints.empty()) + if(unsigned n_wps = router->get_n_waypoints()) { - metrics.resize(waypoints.size()); - for(unsigned i=0; iget_waypoint(i); - metrics[i] = &router->get_metric(i); + waypoints.push_back(router->get_waypoint(i)); + metrics.push_back(&router->get_metric(i)); } has_duration = router->get_trip_duration(); } + unsigned n_vehs = train->get_n_vehicles(); + for(unsigned i=0; iget_vehicle(i).get_type().get_length(); + // If no maximum speed is specified, use a sensible default if(!speed) speed = 20*train->get_layout().get_catalogue().get_scale(); @@ -314,6 +349,7 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState distance_traveled(other.distance_traveled), remaining_estimate(other.remaining_estimate), wait_time(other.wait_time), + estimated_wait(other.estimated_wait), blocked_by(other.blocked_by) { ++occupied_tracks->refcount; @@ -327,7 +363,27 @@ TrainRoutePlanner::TrainRoutingState::~TrainRoutingState() Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() const { - return ((track->get_type().get_path_length(path)-offset)/info->speed)*Time::sec+delay; + return ((occupied_tracks->path_length-offset)/info->speed)*Time::sec+delay; +} + +Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_pass(Track &trk) const +{ + if(is_occupying(trk)) + { + float passed_length = 0; + for(const OccupiedTrack *occ=occupied_tracks; (occ && occ->track!=&trk); occ=occ->next) + passed_length += occ->path_length; + return (max(info->length-passed_length, 0.0f)/info->speed)*Time::sec+delay; + } + + for(unsigned wp=waypoint; wpwaypoints.size(); ++wp) + { + float distance = info->metrics[wp]->get_distance_from(trk); + if(distance>=0 && distancelength)/info->speed)*Time::sec+delay; + } + + return Time::day; } bool TrainRoutePlanner::TrainRoutingState::is_occupying(Track &trk) const @@ -346,19 +402,22 @@ bool TrainRoutePlanner::TrainRoutingState::check_arrival() { TrackIter next_track = track.next(path); - const TrackChain *wp_chain = info->waypoints[waypoint]; - if(wp_chain->has_track(*track) && !wp_chain->has_track(*next_track)) - { - if(waypoint+1waypoints.size()) - ++waypoint; - else + // Check if we're about the exit the current waypoint's tracks. + const TrainRouter::Waypoint &wp = info->waypoints[waypoint]; + if(wp.chain->has_track(*track) && !wp.chain->has_track(*next_track)) + if(wp.direction==TrackChain::UNSPECIFIED || track==wp.chain->iter_for(*track, wp.direction)) { - state = ARRIVED; - return true; + if(waypoint+1waypoints.size()) + ++waypoint; + else + { + state = ARRIVED; + return true; + } } - } - if(info->first_noncritical->has_track(*track)) + // If we're entering the first non-critical block, clear the critical flag. + if(info->first_noncritical->has_track(*next_track)) critical = false; return false; @@ -369,6 +428,7 @@ void TrainRoutePlanner::TrainRoutingState::advance(float distance) offset += distance; back_offset += distance; + // See if the tail end of the train has passed any sensors. unsigned count_to_free = 0; unsigned last_sensor_addr = 0; float distance_after_sensor = 0; @@ -391,6 +451,7 @@ void TrainRoutePlanner::TrainRoutingState::advance(float distance) occ = occ->next; } + // Free the last passed sensor and any tracks behind it. if(count_to_free && back_offset>distance_after_sensor) { back_offset -= distance_after_sensor; @@ -415,6 +476,7 @@ void TrainRoutePlanner::TrainRoutingState::advance(const Time::TimeDelta &dt) } float secs = dt/Time::sec; + // There may be negative delay remaining after previous step. if(delay) { secs -= delay/Time::sec; @@ -424,6 +486,9 @@ void TrainRoutePlanner::TrainRoutingState::advance(const Time::TimeDelta &dt) if(duration) duration = max(duration-secs*Time::sec, Time::zero); + if(estimated_wait) + estimated_wait = max(estimated_wait-secs*Time::sec, Time::zero); + if(state==MOVING) advance(info->speed*secs); else if(state!=ARRIVED) @@ -440,12 +505,22 @@ void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path) offset = 0; } +void TrainRoutePlanner::TrainRoutingState::set_path(unsigned p) +{ + path = p; + OccupiedTrack *next_occ = occupied_tracks->next; + if(!--occupied_tracks->refcount) + delete occupied_tracks; + occupied_tracks = new OccupiedTrack(*track, path, next_occ); + update_estimate(); +} + void TrainRoutePlanner::TrainRoutingState::update_estimate() { TrackIter iter = track.reverse(path); remaining_estimate = info->metrics[waypoint]->get_distance_from(*iter.track(), iter.entry()); if(remaining_estimate>=0) - remaining_estimate += track->get_type().get_path_length(path)-offset; + remaining_estimate += occupied_tracks->path_length-offset; } bool TrainRoutePlanner::TrainRoutingState::is_viable() const @@ -459,13 +534,14 @@ bool TrainRoutePlanner::TrainRoutingState::is_viable() const TrainRoutePlanner::RoutingStep::RoutingStep(): + preferred(false), prev(0) { } TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p): time(p->time), - penalty(p->penalty), cost_estimate(p->cost_estimate), + preferred(false), trains(p->trains), prev(p) { } @@ -473,14 +549,8 @@ TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *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); + if(next.update_states() && next.check_deadlocks()) return; - } int train_index = find_next_train(); if(train_index<0) @@ -491,6 +561,8 @@ void TrainRoutePlanner::RoutingStep::create_successors(list &new_st Time::TimeDelta dt = train.get_time_to_next_track(); next.advance(dt); + /* Check arrival after the train has advanced to the end of its current track + so travel time and occupied tracks will be correct. */ if(train.check_arrival()) { new_steps.push_back(next); @@ -502,44 +574,52 @@ void TrainRoutePlanner::RoutingStep::create_successors(list &new_st const TrackType::Endpoint &entry_ep = train.track.endpoint(); if(train.critical) { - train.path = train.track->get_type().coerce_path(train.track.entry(), train.track->get_active_path()); - train.update_estimate(); - next.update_estimate(); - if(next.is_viable()) - new_steps.push_back(next); + /* Only create a successor step matching the currently set path for a + critical track. */ + unsigned critical_path = train.track->get_type().coerce_path(train.track.entry(), train.track->get_active_path()); + create_successor(next, train_index, critical_path, new_steps); } else { + // Create successor steps for all possible paths through the new track. for(unsigned i=0; entry_ep.paths>>i; ++i) if(entry_ep.has_path(i)) - { - train.path = i; - train.update_estimate(); - next.update_estimate(); - if(next.is_viable()) - new_steps.push_back(next); - } - } - - new_steps.sort(); - for(list::iterator i=new_steps.begin(); ++i!=new_steps.end(); ) - { - i->penalty += 5*Time::sec; - i->update_estimate(); + create_successor(next, train_index, i, new_steps); } if(entry_ep.paths!=train.track->get_type().get_paths() && !train.critical) { + /* Create a waiting state before the track if there's at least one path + that doesn't pass through the entry endpoint. */ RoutingStep wait(this); wait.advance(dt); wait.trains[train_index].state = WAITING; - wait.penalty += 15*Time::sec; + + Time::TimeDelta estimated_wait = Time::day; + for(unsigned i=0; i(train_index) && wait.trains[i].state!=ARRIVED) + { + Time::TimeDelta ttp = wait.trains[i].get_time_to_pass(*train.track); + estimated_wait = min(estimated_wait, ttp); + } + wait.trains[train_index].estimated_wait = estimated_wait; + wait.update_estimate(); if(wait.is_viable()) new_steps.push_back(wait); } } +void TrainRoutePlanner::RoutingStep::create_successor(RoutingStep &next, unsigned train_index, unsigned path, list &new_steps) +{ + TrainRoutingState &train = next.trains[train_index]; + + train.set_path(path); + next.update_estimate(); + if(next.is_viable()) + new_steps.push_back(next); +} + bool TrainRoutePlanner::RoutingStep::update_states() { bool changes = false; @@ -556,12 +636,23 @@ bool TrainRoutePlanner::RoutingStep::update_states() i->blocked_by = get_occupant(*next_track); if(i->blocked_by>=0) { + /* If the train is still traversing its last critical track, the + flag needs to be cleared here to pass viability test. */ if(i->info->first_noncritical->has_track(*next_track)) i->critical = false; + + if(i->state!=BLOCKED) + i->estimated_wait = trains[i->blocked_by].get_time_to_pass(*next_track); + + /* Trains in the WAITING state will also transition to BLOCKED and + then to MOVING when the other train has passed. */ i->state = BLOCKED; } else if(i->state==BLOCKED) + { + i->estimated_wait = Time::zero; i->state = MOVING; + } } else i->state = BLOCKED; @@ -580,9 +671,12 @@ bool TrainRoutePlanner::RoutingStep::check_deadlocks() const if(i->state!=BLOCKED) continue; + // A train blocked by end of track is always considered a deadlock. if(i->blocked_by<0) return true; + /* Use the tortoise and hare algorithm to check if trains are blocked + cyclically (A blocks B, which blocks ..., which blocks A). */ int slow = i->blocked_by; int fast = trains[slow].blocked_by; while(fast>=0 && trains[fast].blocked_by>=0) @@ -634,10 +728,10 @@ void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt) void TrainRoutePlanner::RoutingStep::update_estimate() { - cost_estimate = penalty; + 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; + cost_estimate += i->wait_time+i->estimated_wait+((i->distance_traveled+i->remaining_estimate)/i->info->speed)*Time::sec; } bool TrainRoutePlanner::RoutingStep::is_viable() const @@ -663,6 +757,8 @@ bool TrainRoutePlanner::RoutingStep::is_goal() const bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const { + if(preferred!=other.preferred) + return preferred>other.preferred; return cost_estimate