X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Ftrainrouteplanner.cpp;h=ad97cf79f77c0255f23b9c1be7ed5cf0cfe58e69;hb=5b71cb905051d654c150cb0e098cade0cb502105;hp=99731c6f45c2068684144dd1e8352f8be7f92b22;hpb=a7e320b46a7d4070613b3210c28e65f579750f37;p=r2c2.git diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp index 99731c6..ad97cf7 100644 --- a/source/libr2c2/trainrouteplanner.cpp +++ b/source/libr2c2/trainrouteplanner.cpp @@ -1,6 +1,10 @@ +#include +#include +#include "catalogue.h" #include "layout.h" #include "route.h" #include "train.h" +#include "trainroutemetric.h" #include "trainrouteplanner.h" #include "trainrouter.h" #include "vehicle.h" @@ -10,186 +14,261 @@ using namespace Msp; namespace R2C2 { -TrainRoutePlanner::TrainRoutePlanner(Layout &layout) +TrainRoutePlanner::TrainRoutePlanner(Layout &layout): + goal(0), + path_switch_bias(15*Time::sec), + timeout(10*Time::sec), + result(PENDING), + thread(0) { const map &trains = layout.get_trains(); for(map::const_iterator i=trains.begin(); i!=trains.end(); ++i) { TrainRoutingInfo info(*i->second); - if(info.router && info.router->has_destination()) + if(!info.waypoints.empty()) routed_trains.push_back(info); } - - steps.push_back(RoutingStep()); - RoutingStep &start = steps.back(); - for(vector::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) - start.trains.push_back(TrainRoutingState(*i)); } -void TrainRoutePlanner::plan() +TrainRoutePlanner::~TrainRoutePlanner() { - RoutingStep *goal = 0; - for(list::iterator i=steps.begin(); i!=steps.end(); ++i) + if(thread) { - if(i->is_goal()) - { - goal = &*i; - break; - } - - if(update_states(*i)) - { - int next_train = find_next_train(*i); - if(next_train>=0) - add_steps(*i, next_train); - } + thread->join(); + delete thread; } - - if(goal) - create_routes(*goal); } -bool TrainRoutePlanner::update_states(RoutingStep &step) +void TrainRoutePlanner::set_timeout(const Time::TimeDelta &t) { - RoutingStep next(&step); - bool changes = false; - for(vector::iterator i=next.trains.begin(); i!=next.trains.end(); ++i) - { - TrainState old_state = i->state; - if(i->state==BLOCKED) - i->state = MOVING; + timeout = t; +} - TrackIter next_track = i->track.next(i->path); - if(!next_track) - return false; +TrainRoutePlanner::Result TrainRoutePlanner::plan() +{ + prepare_plan(); + create_plan(); + if(result==PENDING) + finalize_plan(); - for(vector::iterator j=next.trains.begin(); j!=next.trains.end(); ++j) - if(j!=i) - { - if(j->track.track()==next_track.track()) - { - unsigned other_exit = j->track.reverse(j->path).entry(); - if(next_track.entry()==other_exit) - return false; - } - else if(!j->is_occupied(*next_track)) - continue; + return result; +} - i->state = BLOCKED; - } +void TrainRoutePlanner::plan_async() +{ + if(thread) + throw logic_error("already planning"); - if(i->state!=old_state) - changes = true; - } + prepare_plan(); + thread = new PlanningThread(*this); +} - if(changes) +TrainRoutePlanner::Result TrainRoutePlanner::check() +{ + if(result==PENDING && goal) { - list::iterator i; - for(i=steps.begin(); (i!=steps.end() && !(next<*i)); ++i) ; - steps.insert(i, next); + if(thread) + { + thread->join(); + delete thread; + thread = 0; + } + finalize_plan(); } - return !changes; + return result; } -int TrainRoutePlanner::find_next_train(RoutingStep &step) +const list &TrainRoutePlanner::get_routes_for(const Train &train) const { - Time::TimeDelta min_dt; - int next_train = -1; - for(unsigned i=0; i &TrainRoutePlanner::get_sequence_for(const Train &train) const +{ + return get_train_info(train).sequence; } -void TrainRoutePlanner::add_steps(RoutingStep &step, unsigned train_index) +const TrainRoutePlanner::TrainRoutingInfo &TrainRoutePlanner::get_train_info(const Train &train) const { - TrainRoutingState &train = step.trains[train_index]; - Time::TimeDelta dt = train.get_time_to_next_track(); - TrackIter next_track = train.track.next(train.path); + for(vector::const_iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) + if(i->train==&train) + return *i; - list new_steps; + throw key_error(train.get_name()); +} - 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 +const TrainRoutePlanner::RoutingStep &TrainRoutePlanner::get_step() +{ + steps.splice(steps.end(), queue, queue.begin()); + return steps.back(); +} + +void TrainRoutePlanner::prepare_plan() +{ + steps.clear(); + queue.clear(); + goal = 0; + result = PENDING; + + 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::create_plan() +{ + Time::TimeStamp timeout_stamp = Time::now()+timeout; + unsigned count = 0; + while(!queue.empty()) { - next.trains[train_index].advance_track(0); + const RoutingStep &step = get_step(); + if(step.is_goal()) + { + goal = &step; + return; + } - 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)) - { - next.trains[train_index].path = i; - new_steps.push_back(next); - } + add_steps(step); - if(next_entry_ep.paths!=next_track->get_type().get_paths()) + if(++count>=1000) { - RoutingStep wait(&step); - wait.advance(dt); - wait.trains[train_index].state = WAITING; - new_steps.push_back(wait); + if(Time::now()>timeout_stamp) + break; + count = 0; } } + result = FAILED; +} + +void TrainRoutePlanner::add_steps(const RoutingStep &step) +{ + list new_steps; + step.create_successors(new_steps); + if(new_steps.empty()) + return; + new_steps.sort(); - steps.merge(new_steps); + if(!queue.empty() && new_steps.front().cost_estimate::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) { - i->route = new Route(i->train->get_layout()); - i->route->set_name("Router"); - i->route->set_temporary(true); + i->routes.clear(); + i->sequence.clear(); + for(unsigned j=0; j<2; ++j) + i->track_history[j] = 0; } - for(RoutingStep *i=&goal; i; i=i->prev) - { - for(vector::iterator j=i->trains.begin(); j!=i->trains.end(); ++j) + 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) { - if(j->state==WAITING || j->state==BLOCKED) - j->info->waits.push_front(&*j); - j->info->route->add_track(*j->track); - } - } + Track **history = j->info->track_history; + // Don't process the same track again. + if(j->track.track()==history[0]) + continue; - 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()) + Route *route = 0; + bool start_new_route = true; + if(!j->info->routes.empty()) { - Block &block = (*j)->track.next()->get_block(); - i->router->add_wait(block, 0); - current_wait = *j; + /* 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) + { + unsigned nls = j->track->get_n_link_slots(); + for(unsigned k=0; (!start_new_route && ktrack->get_link(k); + start_new_route = (link && link!=history[0] && route->has_track(*link)); + } + } } - } + + if(start_new_route) + { + 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); + } + + route->add_track(*j->track.track()); + history[1] = history[0]; + history[0] = j->track.track(); + + 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()) + { + // Add a sequence point if another train uses this track afterwards. + if(!k->second->preceding_train) + { + k->second->preceding_train = j->info->train; + k->second->sequence_in = sequence; + } + j->info->sequence.push_front(TrainRouter::SequencePoint(j->track->get_block(), sequence)); + if(waitable) + k->second = &j->info->sequence.front(); + --sequence; + } + 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; + } + } + + result = COMPLETE; } 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()), - route(0) -{ } + has_duration(false) +{ + if(unsigned n_wps = router->get_n_waypoints()) + { + waypoints.reserve(n_wps), + metrics.reserve(n_wps); + for(unsigned i=0; iget_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(); +} TrainRoutePlanner::OccupiedTrack::OccupiedTrack(Track &t, unsigned p, OccupiedTrack *n): @@ -223,9 +302,13 @@ TrainRoutePlanner::OccupiedTrack::~OccupiedTrack() TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf): info(&inf), + critical(true), occupied_tracks(0), state(MOVING), - delay(info->router->get_departure_delay()) + delay(info->router->get_departure_delay()), + duration(info->router->get_trip_duration()), + waypoint(0), + blocked_by(-1) { const Vehicle *veh = &info->train->get_vehicle(0); // TODO margins @@ -247,34 +330,67 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf): break; iter = iter.next(); } + + update_estimate(); } TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState &other): info(other.info), track(other.track), path(other.path), + critical(other.critical), occupied_tracks(other.occupied_tracks), offset(other.offset), back_offset(other.back_offset), - state(other.state) + state(other.state), + delay(other.delay), + duration(other.duration), + waypoint(other.waypoint), + 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; } 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+delay; + return ((occupied_tracks->path_length-offset)/info->speed)*Time::sec+delay; } -bool TrainRoutePlanner::TrainRoutingState::is_occupied(Track &trk) const +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 +{ + if(state==ARRIVED && !duration && info->has_duration) + return false; + OccupiedTrack *occ = occupied_tracks; for(unsigned n=occ->n_tracks; n>0; --n, occ=occ->next) if(occ->track==&trk) @@ -282,26 +398,101 @@ bool TrainRoutePlanner::TrainRoutingState::is_occupied(Track &trk) const return false; } +bool TrainRoutePlanner::TrainRoutingState::check_arrival() +{ + TrackIter next_track = track.next(path); + + // 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)) + { + if(waypoint+1waypoints.size()) + ++waypoint; + else + { + state = ARRIVED; + return true; + } + } + + // 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; +} + void TrainRoutePlanner::TrainRoutingState::advance(float distance) { offset += distance; back_offset += distance; - OccupiedTrack *last_occ = occupied_tracks; - for(unsigned n=occupied_tracks->n_tracks; n>1; --n) - last_occ = last_occ->next; + // 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; + OccupiedTrack *occ = occupied_tracks; + for(unsigned n=occupied_tracks->n_tracks; n>0; --n) + { + if(unsigned saddr = occ->track->get_sensor_address()) + { + if(saddr!=last_sensor_addr) + { + count_to_free = 0; + distance_after_sensor = 0; + } + last_sensor_addr = saddr; + } - // XXX What if there's multiple tracks to remove? - if(back_offset>last_occ->path_length) + ++count_to_free; + distance_after_sensor += occ->path_length; + + occ = occ->next; + } + + // Free the last passed sensor and any tracks behind it. + if(count_to_free && back_offset>distance_after_sensor) { - back_offset -= last_occ->path_length; + back_offset -= distance_after_sensor; if(occupied_tracks->refcount>1) { --occupied_tracks->refcount; occupied_tracks = new OccupiedTrack(*occupied_tracks); } - --occupied_tracks->n_tracks; + occupied_tracks->n_tracks -= count_to_free; } + + 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; + // There may be negative delay remaining after previous step. + if(delay) + { + secs -= delay/Time::sec; + delay = Time::zero; + } + + 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) + wait_time += secs*Time::sec; } void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path) @@ -314,35 +505,246 @@ 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 += occupied_tracks->path_length-offset; +} + +bool TrainRoutePlanner::TrainRoutingState::is_viable() const +{ + if(remaining_estimate<0) + return false; + if(critical && state==BLOCKED) + return false; + return true; +} + TrainRoutePlanner::RoutingStep::RoutingStep(): + preferred(false), prev(0) { } -TrainRoutePlanner::RoutingStep::RoutingStep(RoutingStep *p): +TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p): time(p->time), + cost_estimate(p->cost_estimate), + preferred(false), trains(p->trains), prev(p) { } -void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt) +void TrainRoutePlanner::RoutingStep::create_successors(list &new_steps) const { - time += dt; + RoutingStep next(this); + if(next.update_states() && next.check_deadlocks()) + 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); + + /* 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); + return; + } + + train.advance_track(0); + + const TrackType::Endpoint &entry_ep = train.track.endpoint(); + if(train.critical) + { + /* 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)) + 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; + + 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; for(vector::iterator i=trains.begin(); i!=trains.end(); ++i) { - if(i->delay) + if(i->state==ARRIVED) + continue; + + TrainState old_state = i->state; + + TrackIter next_track = i->track.next(i->path); + if(next_track) { - i->delay -= dt; - if(i->delay>Time::zero) - continue; - i->delay = Time::zero; + 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 if(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; + + // 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) { - float distance = dt/Time::sec; - i->advance(distance); + 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->estimated_wait+((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->is_viable()) + return false; + + for(vector::const_iterator i=trains.begin(); i!=trains.end(); ++i) + if(i->state==MOVING) + return true; + + return false; } bool TrainRoutePlanner::RoutingStep::is_goal() const @@ -355,7 +757,21 @@ bool TrainRoutePlanner::RoutingStep::is_goal() const bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const { - return timeother.preferred; + return cost_estimate