From d578d036656c0e89fe9dca5aefd1f81d2777a69e Mon Sep 17 00:00:00 2001 From: Mikko Rasa Date: Sun, 30 Mar 2014 20:22:31 +0300 Subject: [PATCH] Add a distance metric to turn the routing into an A* search The original straightforward implementation exploded in complexity much sooner than I expected. The reasons are not entirely clear, but the inability to converge states and drop those where the same position was already reached through a faster path may have something to do with it. --- source/libr2c2/trainroutemetric.cpp | 90 ++++++++++++++++++++++++++++ source/libr2c2/trainroutemetric.h | 50 ++++++++++++++++ source/libr2c2/trainrouteplanner.cpp | 40 ++++++++++++- source/libr2c2/trainrouteplanner.h | 4 ++ source/libr2c2/trainrouter.cpp | 49 ++++++++++++++- source/libr2c2/trainrouter.h | 5 ++ 6 files changed, 234 insertions(+), 4 deletions(-) create mode 100644 source/libr2c2/trainroutemetric.cpp create mode 100644 source/libr2c2/trainroutemetric.h diff --git a/source/libr2c2/trainroutemetric.cpp b/source/libr2c2/trainroutemetric.cpp new file mode 100644 index 0000000..a2cffbc --- /dev/null +++ b/source/libr2c2/trainroutemetric.cpp @@ -0,0 +1,90 @@ +#include +#include "track.h" +#include "trackchain.h" +#include "trainroutemetric.h" + +using namespace std; + +namespace R2C2 { + +TrainRouteMetric::TrainRouteMetric(const TrackChain &tc) +{ + const TrackChain::TrackSet &ctracks = tc.get_tracks(); + for(TrackChain::TrackSet::const_iterator i=ctracks.begin(); i!=ctracks.end(); ++i) + { + unsigned nls = (*i)->get_n_link_slots(); + for(unsigned j=0; jget_link(j)) + if(!ctracks.count(link)) + goals.push_back(TrackIter(*i, j)); + } + + list queue; + for(vector::iterator i=goals.begin(); i!=goals.end(); ++i) + { + tracks[Key(i->track.track(), i->track.entry())] = Data(0, &*i); + queue.push_back(i->track); + } + + while(!queue.empty()) + { + TrackIter track = queue.front(); + queue.pop_front(); + const Data &data = tracks[Key(track.track(), track.entry())]; + + const TrackType::Endpoint &ep = track.endpoint(); + for(unsigned i=0; ep.paths>>i; ++i) + if(ep.has_path(i)) + { + TrackIter next = track.next(i); + if(!next) + continue; + + Data &target = tracks[Key(next.track(), next.entry())]; + float dist = data.distance+track->get_type().get_path_length(i); + if(target.distance<0 || target.distance>dist) + { + target = Data(dist, data.goal); + queue.push_back(next); + } + } + } +} + +void TrainRouteMetric::chain_to(const TrainRouteMetric &metric) +{ + for(vector::iterator i=goals.begin(); i!=goals.end(); ++i) + i->base_distance = metric.get_distance_from(*i->track.track(), i->track.entry()); +} + +float TrainRouteMetric::get_distance_from(const Track &track, unsigned exit) const +{ + map::const_iterator i = tracks.find(Key(&track, exit)); + if(i==tracks.end()) + return -1; + + return i->second.distance+i->second.goal->base_distance; +} + + +TrainRouteMetric::Goal::Goal(): + base_distance(0) +{ } + +TrainRouteMetric::Goal::Goal(const TrackIter &t): + track(t), + base_distance(0) +{ } + + +TrainRouteMetric::Data::Data(): + distance(-1), + goal(0) +{ } + +TrainRouteMetric::Data::Data(float d, const Goal *g): + distance(d), + goal(g) +{ } + +} // namespace R2C2 diff --git a/source/libr2c2/trainroutemetric.h b/source/libr2c2/trainroutemetric.h new file mode 100644 index 0000000..c63f25e --- /dev/null +++ b/source/libr2c2/trainroutemetric.h @@ -0,0 +1,50 @@ +#ifndef LIBR2C2_TRAINROUTEMETRIC_H_ +#define LIBR2C2_TRAINROUTEMETRIC_H_ + +#include +#include + +namespace R2C2 { + +class Layout; +class Track; +class TrackChain; + +// Metrics store iterators facing away from the goal +class TrainRouteMetric +{ +private: + typedef std::pair Key; + + struct Goal + { + TrackIter track; + float base_distance; + + Goal(); + Goal(const TrackIter &); + }; + + struct Data + { + float distance; + const Goal *goal; + + Data(); + Data(float, const Goal *); + }; + + std::vector goals; + std::map tracks; + +public: + TrainRouteMetric() { } + TrainRouteMetric(const TrackChain &); + + void chain_to(const TrainRouteMetric &); + float get_distance_from(const Track &, unsigned) const; +}; + +} // namespace R2C2 + +#endif diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp index 22ac540..9ba860b 100644 --- a/source/libr2c2/trainrouteplanner.cpp +++ b/source/libr2c2/trainrouteplanner.cpp @@ -2,6 +2,7 @@ #include "layout.h" #include "route.h" #include "train.h" +#include "trainroutemetric.h" #include "trainrouteplanner.h" #include "trainrouter.h" #include "vehicle.h" @@ -164,6 +165,8 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf): break; iter = iter.next(); } + + update_estimate(); } TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState &other): @@ -176,6 +179,7 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState state(other.state), delay(other.delay), waypoint(other.waypoint), + remaining_estimate(other.remaining_estimate), blocked_by(other.blocked_by) { ++occupied_tracks->refcount; @@ -241,6 +245,10 @@ void TrainRoutePlanner::TrainRoutingState::advance(float distance) } --occupied_tracks->n_tracks; } + + remaining_estimate -= (distance/info->speed)*Time::sec; + if(remaining_estimaterouter->get_metric(waypoint).get_distance_from(*iter.track(), iter.entry()); + distance += track->get_type().get_path_length(path)-offset; + remaining_estimate = (distance/info->speed)*Time::sec+delay; +} + TrainRoutePlanner::RoutingStep::RoutingStep(): prev(0) @@ -260,6 +276,7 @@ TrainRoutePlanner::RoutingStep::RoutingStep(): TrainRoutePlanner::RoutingStep::RoutingStep(const RoutingStep *p): time(p->time), + total_estimate(p->total_estimate), trains(p->trains), prev(p) { } @@ -299,6 +316,8 @@ 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); } @@ -411,8 +430,27 @@ void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt) } } +void TrainRoutePlanner::RoutingStep::update_estimate() +{ + for(vector::const_iterator i=trains.begin(); i!=trains.end(); ++i) + { + if(i->remaining_estimateremaining_estimate; + return; + } + + Time::TimeDelta t = time+i->remaining_estimate; + if(i==trains.begin() || t>total_estimate) + total_estimate = t; + } +} + bool TrainRoutePlanner::RoutingStep::is_viable() const { + if(total_estimate::const_iterator i=trains.begin(); i!=trains.end(); ++i) if(i->state==MOVING) return true; @@ -430,7 +468,7 @@ bool TrainRoutePlanner::RoutingStep::is_goal() const bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const { - return time trains; const RoutingStep *prev; @@ -90,6 +93,7 @@ private: int get_occupant(Track &) const; int find_next_train() const; void advance(const Msp::Time::TimeDelta &); + void update_estimate(); bool is_viable() const; bool is_goal() const; diff --git a/source/libr2c2/trainrouter.cpp b/source/libr2c2/trainrouter.cpp index b388bb2..93dbedf 100644 --- a/source/libr2c2/trainrouter.cpp +++ b/source/libr2c2/trainrouter.cpp @@ -3,6 +3,7 @@ #include "trackiter.h" #include "train.h" #include "trackchain.h" +#include "trainroutemetric.h" #include "trainrouteplanner.h" #include "trainrouter.h" @@ -22,6 +23,12 @@ TrainRouter::TrainRouter(Train &t): train.signal_advanced.connect(sigc::mem_fun(this, &TrainRouter::train_advanced)); } +TrainRouter::~TrainRouter() +{ + for(vector::iterator i=metrics.begin(); i!=metrics.end(); ++i) + delete *i; +} + void TrainRouter::set_priority(int p) { priority = p; @@ -116,6 +123,21 @@ bool TrainRouter::is_waypoint(unsigned index, Track &track) const return waypoints[index]->has_track(track); } +const TrainRouteMetric &TrainRouter::get_metric(int index) const +{ + if(!destination) + throw logic_error("no metrics"); + else if(update_pending) + throw logic_error("metrics are stale"); + + if(index<0) + return *metrics.front(); + else if(static_cast(index)>=waypoints.size()) + throw out_of_range("TrainRouter::get_metric"); + else + return *metrics[index+1]; +} + void TrainRouter::set_departure_delay(const Time::TimeDelta &d) { delay = d; @@ -284,6 +306,23 @@ const Route *TrainRouter::get_route_for_block(const Block &block) const return 0; } +void TrainRouter::create_metrics() +{ + for(vector::iterator i=metrics.begin(); i!=metrics.end(); ++i) + delete *i; + metrics.clear(); + + if(!destination) + return; + + metrics.push_back(new TrainRouteMetric(*destination)); + for(vector::const_iterator i=waypoints.begin(); i!=waypoints.end(); ++i) + metrics.push_back(new TrainRouteMetric(**i)); + + for(unsigned i=metrics.size(); --i>0; ) + metrics[i]->chain_to(*metrics[(i+1)%metrics.size()]); +} + Route *TrainRouter::create_lead_route(Route *lead, const Route *target) { if(!lead) @@ -326,13 +365,17 @@ bool TrainRouter::is_on_route(const Block &block) void TrainRouter::create_plans(Layout &layout) { - TrainRoutePlanner planner(layout); - planner.plan(); - const map &trains = layout.get_trains(); for(map::const_iterator i=trains.begin(); i!=trains.end(); ++i) if(TrainRouter *router = i->second->get_ai_of_type()) + { + if(router->update_pending) + router->create_metrics(); router->update_pending = false; + } + + TrainRoutePlanner planner(layout); + planner.plan(); } diff --git a/source/libr2c2/trainrouter.h b/source/libr2c2/trainrouter.h index 4f62076..c69d28c 100644 --- a/source/libr2c2/trainrouter.h +++ b/source/libr2c2/trainrouter.h @@ -11,6 +11,7 @@ class Block; class Layout; class Track; class TrackChain; +class TrainRouteMetric; class TrainRouter: public TrainAI { @@ -43,6 +44,7 @@ private: unsigned arriving; const TrackChain *destination; std::vector waypoints; + std::vector metrics; std::list waits; Msp::Time::TimeDelta delay; @@ -50,6 +52,7 @@ private: public: TrainRouter(Train &); + ~TrainRouter(); void set_priority(int); int get_priority() const { return priority; } @@ -64,6 +67,7 @@ public: void add_waypoint(const TrackChain &); unsigned get_n_waypoints() const { return waypoints.size(); } bool is_waypoint(unsigned, Track &) const; + const TrainRouteMetric &get_metric(int = -1) const; void set_departure_delay(const Msp::Time::TimeDelta &); const Msp::Time::TimeDelta &get_departure_delay() const { return delay; } @@ -77,6 +81,7 @@ private: void train_advanced(Block &); const Route *get_route_for_block(const Block &) const; + void create_metrics(); Route *create_lead_route(Route *, const Route *); bool advance_route(RouteList::iterator &, const Block &); bool is_on_route(const Block &); -- 2.45.2