X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Ftrainrouteplanner.cpp;h=b6d2f07cb66203ded55f3471110f94a0270287c0;hb=528c4f8760ac54fb2d1f3002248ecf6774956030;hp=b4f01ab6a860084901add49d3ab21de3405d49d0;hpb=bf321305d7bf65aa5033a835f61370cd48f54619;p=r2c2.git diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp index b4f01ab..b6d2f07 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,7 @@ namespace R2C2 { TrainRoutePlanner::TrainRoutePlanner(Layout &layout): goal(0), + timeout(10*Time::sec), result(PENDING), thread(0) { @@ -22,14 +24,23 @@ TrainRoutePlanner::TrainRoutePlanner(Layout &layout): for(map::const_iterator i=trains.begin(); i!=trains.end(); ++i) { TrainRoutingInfo info(*i->second); - if(info.router && info.router->get_destination()) + if(!info.waypoints.empty()) routed_trains.push_back(info); } } TrainRoutePlanner::~TrainRoutePlanner() { - delete thread; + if(thread) + { + thread->join(); + delete thread; + } +} + +void TrainRoutePlanner::set_timeout(const Time::TimeDelta &t) +{ + timeout = t; } TrainRoutePlanner::Result TrainRoutePlanner::plan() @@ -55,9 +66,13 @@ TrainRoutePlanner::Result TrainRoutePlanner::check() { if(result==PENDING && goal) { + if(thread) + { + thread->join(); + delete thread; + thread = 0; + } finalize_plan(); - delete thread; - thread = 0; } return result; @@ -104,6 +119,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(); @@ -114,6 +131,13 @@ void TrainRoutePlanner::create_plan() } add_steps(step); + + if(++count>=1000) + { + if(Time::now()>timeout_stamp) + break; + count = 0; + } } result = FAILED; @@ -133,7 +157,7 @@ void TrainRoutePlanner::finalize_plan() { i->routes.clear(); i->sequence.clear(); - for(unsigned j=0; j<3; ++j) + for(unsigned j=0; j<2; ++j) i->track_history[j] = 0; } @@ -168,16 +192,13 @@ void TrainRoutePlanner::finalize_plan() route = new Route(j->info->train->get_layout()); route->set_name("Router"); route->set_temporary(true); - for(unsigned k=2; k>0; --k) - if(history[k]) - route->add_track(*history[k]); + for(unsigned k=0; (k<2 && history[k]); ++k) + route->add_track(*history[k]); j->info->routes.push_front(route); } - if(history[0]) - route->add_track(*history[0]); - for(unsigned k=2; k>0; --k) - history[k] = history[k-1]; + 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(); @@ -209,8 +230,22 @@ void TrainRoutePlanner::finalize_plan() TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t): train(&t), speed(train->get_maximum_speed()), - router(train->get_ai_of_type()) + first_noncritical(train->get_last_critical_block().next().block()), + router(train->get_ai_of_type()), + 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(); + } + // If no maximum speed is specified, use a sensible default if(!speed) speed = 20*train->get_layout().get_catalogue().get_scale(); @@ -248,10 +283,12 @@ TrainRoutePlanner::OccupiedTrack::~OccupiedTrack() TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf): info(&inf), + critical(true), occupied_tracks(0), state(MOVING), delay(info->router->get_departure_delay()), - waypoint(info->router->get_n_waypoints() ? 0 : -1), + duration(info->router->get_trip_duration()), + waypoint(0), blocked_by(-1) { const Vehicle *veh = &info->train->get_vehicle(0); @@ -282,11 +319,13 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState 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), delay(other.delay), + duration(other.duration), waypoint(other.waypoint), distance_traveled(other.distance_traveled), remaining_estimate(other.remaining_estimate), @@ -309,6 +348,9 @@ Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() c 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) @@ -318,20 +360,23 @@ bool TrainRoutePlanner::TrainRoutingState::is_occupying(Track &trk) const 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; - } + 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(info->first_noncritical->has_track(*track)) + critical = false; return false; } @@ -341,20 +386,37 @@ 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; + 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; + } + + 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; @@ -376,6 +438,9 @@ void TrainRoutePlanner::TrainRoutingState::advance(const Time::TimeDelta &dt) delay = Time::zero; } + if(duration) + duration = max(duration-secs*Time::sec, Time::zero); + if(state==MOVING) advance(info->speed*secs); else if(state!=ARRIVED) @@ -395,9 +460,18 @@ void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path) 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; + 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; +} + +bool TrainRoutePlanner::TrainRoutingState::is_viable() const +{ + if(remaining_estimate<0) + return false; + if(critical && state==BLOCKED) + return false; + return true; } @@ -407,6 +481,7 @@ TrainRoutePlanner::RoutingStep::RoutingStep(): TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p): time(p->time), + penalty(p->penalty), cost_estimate(p->cost_estimate), trains(p->trains), prev(p) @@ -439,30 +514,51 @@ void TrainRoutePlanner::RoutingStep::create_successors(list &new_st 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); - } + const TrackType::Endpoint &entry_ep = train.track.endpoint(); + if(train.critical) + { + 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 + { + for(unsigned i=0; entry_ep.paths>>i; ++i) + if(entry_ep.has_path(i)) + create_successor(next, train_index, i, new_steps); + } + + new_steps.sort(); + for(list::iterator i=new_steps.begin(); ++i!=new_steps.end(); ) + { + i->penalty += 5*Time::sec; + i->update_estimate(); + } - if(next_entry_ep.paths!=next_track->get_type().get_paths()) + if(entry_ep.paths!=train.track->get_type().get_paths() && !train.critical) { RoutingStep wait(this); wait.advance(dt); wait.trains[train_index].state = WAITING; + wait.penalty += 15*Time::sec; + 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.path = path; + train.update_estimate(); + next.update_estimate(); + if(next.is_viable()) + new_steps.push_back(next); +} + bool TrainRoutePlanner::RoutingStep::update_states() { bool changes = false; @@ -478,7 +574,11 @@ bool TrainRoutePlanner::RoutingStep::update_states() { i->blocked_by = get_occupant(*next_track); if(i->blocked_by>=0) + { + if(i->info->first_noncritical->has_track(*next_track)) + i->critical = false; i->state = BLOCKED; + } else if(i->state==BLOCKED) i->state = MOVING; } @@ -553,7 +653,7 @@ void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt) void TrainRoutePlanner::RoutingStep::update_estimate() { - cost_estimate = Time::zero; + cost_estimate = penalty; 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; @@ -562,7 +662,7 @@ void TrainRoutePlanner::RoutingStep::update_estimate() bool TrainRoutePlanner::RoutingStep::is_viable() const { for(vector::const_iterator i=trains.begin(); i!=trains.end(); ++i) - if(i->remaining_estimate<0) + if(!i->is_viable()) return false; for(vector::const_iterator i=trains.begin(); i!=trains.end(); ++i)