X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Ftrainrouteplanner.cpp;h=9e74eca1c7b1b2c0c18846d586cf86a774c363a1;hb=ee92bee98fa084645b17f65cebe4eabf70e4dee9;hp=e813b366fe88a4829d4cc155f9d731d41108d461;hpb=4232c65cb556d278dee218866de77402ff888f1e;p=r2c2.git diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp index e813b36..9e74eca 100644 --- a/source/libr2c2/trainrouteplanner.cpp +++ b/source/libr2c2/trainrouteplanner.cpp @@ -1,7 +1,9 @@ +#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" @@ -11,38 +13,118 @@ using namespace Msp; namespace R2C2 { -TrainRoutePlanner::TrainRoutePlanner(Layout &layout) +TrainRoutePlanner::TrainRoutePlanner(Layout &layout): + goal(0), + 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->get_destination()) + if(info.destination) routed_trains.push_back(info); } +} + +TrainRoutePlanner::~TrainRoutePlanner() +{ + if(thread) + { + thread->join(); + delete thread; + } +} + +TrainRoutePlanner::Result TrainRoutePlanner::plan() +{ + prepare_plan(); + create_plan(); + if(result==PENDING) + finalize_plan(); + + return result; +} + +void TrainRoutePlanner::plan_async() +{ + if(thread) + throw logic_error("already planning"); + + prepare_plan(); + thread = new PlanningThread(*this); +} + +TrainRoutePlanner::Result TrainRoutePlanner::check() +{ + if(result==PENDING && goal) + { + if(thread) + { + thread->join(); + delete thread; + thread = 0; + } + finalize_plan(); + } + + return result; +} + +const list &TrainRoutePlanner::get_routes_for(const Train &train) const +{ + return get_train_info(train).routes; +} + +const list &TrainRoutePlanner::get_sequence_for(const Train &train) const +{ + return get_train_info(train).sequence; +} + +const TrainRoutePlanner::TrainRoutingInfo &TrainRoutePlanner::get_train_info(const Train &train) const +{ + for(vector::const_iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) + if(i->train==&train) + return *i; - steps.push_back(RoutingStep()); - RoutingStep &start = steps.back(); + throw key_error(train.get_name()); +} + +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::plan() +void TrainRoutePlanner::create_plan() { - const RoutingStep *goal = 0; - for(list::iterator i=steps.begin(); i!=steps.end(); ++i) + while(!queue.empty()) { - if(i->is_goal()) + const RoutingStep &step = get_step(); + if(step.is_goal()) { - goal = &*i; - break; + goal = &step; + return; } - add_steps(*i); + add_steps(step); } - if(goal) - create_routes(*goal); + result = FAILED; } void TrainRoutePlanner::add_steps(const RoutingStep &step) @@ -50,40 +132,82 @@ void TrainRoutePlanner::add_steps(const RoutingStep &step) list new_steps; step.create_successors(new_steps); new_steps.sort(); - steps.merge(new_steps); + queue.merge(new_steps); } -void TrainRoutePlanner::create_routes(const RoutingStep &goal) +void TrainRoutePlanner::finalize_plan() { 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"); - i->route->set_temporary(true); + i->routes.clear(); + i->sequence.clear(); + for(unsigned j=0; j<2; ++j) + i->track_history[j] = 0; } - for(const RoutingStep *i=&goal; i; i=i->prev) - { + 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; + 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); - const TrainRoutingState *current_wait = 0; - for(list::const_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; + 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); + 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()) + { + 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) + { + j->info->sequence.push_front(TrainRouter::SequencePoint(j->track->get_block(), sequence)); + sequenced_tracks[j->track.track()] = &j->info->sequence.front(); + --sequence; + } + } + + result = COMPLETE; } @@ -91,8 +215,26 @@ TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t): train(&t), speed(train->get_maximum_speed()), router(train->get_ai_of_type()), - route(0) + destination(0), + has_duration(false) { + if(router) + { + destination = router->get_destination(); + if(destination) + { + waypoints.resize(router->get_n_waypoints()); + metrics.resize(waypoints.size()+1); + metrics[0] = &router->get_metric(-1); + for(unsigned i=0; iget_waypoint(i); + metrics[i+1] = &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(); @@ -133,7 +275,8 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf): 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(info->waypoints.empty() ? -1 : 0), blocked_by(-1) { const Vehicle *veh = &info->train->get_vehicle(0); @@ -156,6 +299,8 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf): break; iter = iter.next(); } + + update_estimate(); } TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState &other): @@ -167,7 +312,11 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState 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), + wait_time(other.wait_time), blocked_by(other.blocked_by) { ++occupied_tracks->refcount; @@ -175,7 +324,7 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState TrainRoutePlanner::TrainRoutingState::~TrainRoutingState() { - if(!--occupied_tracks->refcount) + if(occupied_tracks && !--occupied_tracks->refcount) delete occupied_tracks; } @@ -186,6 +335,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) @@ -195,18 +347,17 @@ 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)) + if(waypoint<0 && info->destination->has_track(*track) && !info->destination->has_track(*next_track)) { state = ARRIVED; return true; } - else if(waypoint>=0 && router.is_waypoint(waypoint, *track) && !router.is_waypoint(waypoint, *next_track)) + else if(waypoint>=0 && info->waypoints[waypoint]->has_track(*track) && !info->waypoints[waypoint]->has_track(*next_track)) { ++waypoint; - if(waypoint>=static_cast(router.get_n_waypoints())) + if(waypoint>=static_cast(info->waypoints.size())) waypoint = -1; } @@ -233,6 +384,33 @@ 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(duration) + duration = max(duration-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) @@ -245,6 +423,14 @@ void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path) offset = 0; } +void TrainRoutePlanner::TrainRoutingState::update_estimate() +{ + TrackIter iter = track.reverse(path); + float distance = info->metrics[waypoint+1]->get_distance_from(*iter.track(), iter.entry()); + distance += track->get_type().get_path_length(path)-offset; + remaining_estimate = distance; +} + TrainRoutePlanner::RoutingStep::RoutingStep(): prev(0) @@ -252,6 +438,8 @@ 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) { } @@ -291,15 +479,26 @@ void TrainRoutePlanner::RoutingStep::create_successors(list &new_st 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); } + 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()) { 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); } @@ -390,21 +589,23 @@ void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt) { time += dt; for(vector::iterator i=trains.begin(); i!=trains.end(); ++i) - { - if(i->delay) - { - i->delay -= dt; - if(i->delay>Time::zero) - continue; - i->delay = Time::zero; - } - else if(i->state==MOVING) - i->advance(i->info->speed*(dt/Time::sec)); - } + i->advance(dt); +} + +void TrainRoutePlanner::RoutingStep::update_estimate() +{ + 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; } 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) return true; @@ -422,7 +623,19 @@ bool TrainRoutePlanner::RoutingStep::is_goal() const bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const { - return time