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.

diff --git a/source/libr2c2/trainroutemetric.cpp b/source/libr2c2/trainroutemetric.cpp
new file mode 100644 (file)
index 0000000..a2cffbc
--- /dev/null
+++ b/source/libr2c2/trainroutemetric.cpp
@@ -0,0 +1,90 @@
+#include <list>
+#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; j<nls; ++j)
+                       if(Track *link = (*i)->get_link(j))
+                               if(!ctracks.count(link))
+                                       goals.push_back(TrackIter(*i, j));
+       }
+
+       list<TrackIter> queue;
+       for(vector<Goal>::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<Goal>::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<Key, Data>::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 (file)
index 0000000..c63f25e
--- /dev/null
+++ b/source/libr2c2/trainroutemetric.h
@@ -0,0 +1,50 @@
+#ifndef LIBR2C2_TRAINROUTEMETRIC_H_
+#define LIBR2C2_TRAINROUTEMETRIC_H_
+
+#include <map>
+#include <vector>
+
+namespace R2C2 {
+
+class Layout;
+class Track;
+class TrackChain;
+
+// Metrics store iterators facing away from the goal
+class TrainRouteMetric
+{
+private:
+       typedef std::pair<const Track *, unsigned> 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<Goal> goals;
+       std::map<Key, Data> 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 22ac5400ea4fb050437ccdff14de77dbafc483e9..9ba860be29cd765a1ade273779a040e2ccbdcfff 100644 (file)
--- 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_estimate<Time::zero)
+               remaining_estimate = Time::zero;
 }
 
 void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path)
@@ -253,6 +261,14 @@ void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path)
        offset = 0;
 }
 
+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/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<RoutingStep> &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<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
+       {
+               if(i->remaining_estimate<Time::zero)
+               {
+                       total_estimate = i->remaining_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<Time::zero)
+               return false;
+
        for(vector<TrainRoutingState>::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<other.time;
+       return total_estimate<other.total_estimate;
 }
 
 } // namespace R2C2

diff --git a/source/libr2c2/trainrouteplanner.h b/source/libr2c2/trainrouteplanner.h
index 7d1ff9a0709a61f4fdfe413687cf990790760caf..215cbfd6c9b1660356c375d1d1bb6aa471f8b4d3 100644 (file)
--- a/source/libr2c2/trainrouteplanner.h
+++ b/source/libr2c2/trainrouteplanner.h
@@ -62,6 +62,7 @@ private:
                TrainState state;
                Msp::Time::TimeDelta delay;
                int waypoint;
+               Msp::Time::TimeDelta remaining_estimate;
                int blocked_by;
 
                TrainRoutingState(TrainRoutingInfo &);
@@ -73,11 +74,13 @@ private:
                bool check_arrival();
                void advance(float);
                void advance_track(unsigned);
+               void update_estimate();
        };
 
        struct RoutingStep
        {
                Msp::Time::TimeDelta time;
+               Msp::Time::TimeDelta total_estimate;
                std::vector<TrainRoutingState> 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 b388bb2be9bc1a1062f49ce6d41f7b3e8ee86382..93dbedf544c27b029a56dc352f1f9e6e502be4ad 100644 (file)
--- 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<TrainRouteMetric *>::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<unsigned>(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<TrainRouteMetric *>::iterator i=metrics.begin(); i!=metrics.end(); ++i)
+               delete *i;
+       metrics.clear();
+
+       if(!destination)
+               return;
+
+       metrics.push_back(new TrainRouteMetric(*destination));
+       for(vector<const TrackChain *>::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<unsigned, Train *> &trains = layout.get_trains();
        for(map<unsigned, Train *>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
                if(TrainRouter *router = i->second->get_ai_of_type<TrainRouter>())
+               {
+                       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 4f620764fd8c1a679205f59bb49662ad0fc0d20a..c69d28c9389057c3ea37cb14fd5ed3ed5ad0a52a 100644 (file)
--- 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<const TrackChain *> waypoints;
+       std::vector<TrainRouteMetric *> metrics;
        std::list<Wait> 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 &);