Rewrite goal initialization for TrainRouteMetric

[r2c2.git] / source / libr2c2 / trainroutemetric.cpp

diff --git a/source/libr2c2/trainroutemetric.cpp b/source/libr2c2/trainroutemetric.cpp
index a2cffbc896504d7df252905a449c63fbbe11c91b..d3878d4aaec5d3b6dc875ee7b0453e0da82376f4 100644 (file)
--- a/source/libr2c2/trainroutemetric.cpp
+++ b/source/libr2c2/trainroutemetric.cpp
@@ -7,16 +7,30 @@ using namespace std;
 
 namespace R2C2 {
 
-TrainRouteMetric::TrainRouteMetric(const TrackChain &tc)
+TrainRouteMetric::TrainRouteMetric(const TrackChain &tc, TrackChain::Direction dir)
 {
-       const TrackChain::TrackSet &ctracks = tc.get_tracks();
-       for(TrackChain::TrackSet::const_iterator i=ctracks.begin(); i!=ctracks.end(); ++i)
+       vector<TrackIter> ends;
+       if(dir==TrackChain::UNSPECIFIED)
        {
+               for(unsigned i=0; i<2; ++i)
+                       if(TrackIter end = tc.get_end(i))
+                               ends.push_back(end);
+       }
+       else if(TrackIter end = tc.get_end(dir))
+               ends.push_back(end);
+
+       /* Initialize goals for the ends of the target chain.  We travel away from
+       the goals in the search phase so the iters appear to point the wrong way. */
+       for(vector<TrackIter>::const_iterator i=ends.begin(); i!=ends.end(); ++i)
+       {
+               const TrackType::Endpoint &ep = i->endpoint();
                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));
+               {
+                       TrackIter iter(i->track(), j);
+                       if(ep.has_common_paths(iter.endpoint()))
+                               goals.push_back(iter);
+               }
        }
 
        list<TrackIter> queue;
@@ -26,6 +40,9 @@ TrainRouteMetric::TrainRouteMetric(const TrackChain &tc)
                queue.push_back(i->track);
        }
 
+       /* Use Dijkstra's algorithm to find the shortest distance from the goal to
+       every reachable track in the layout.  Entry points here become exit points
+       when looking up distances to the goal. */
        while(!queue.empty())
        {
                TrackIter track = queue.front();
@@ -57,6 +74,22 @@ void TrainRouteMetric::chain_to(const TrainRouteMetric &metric)
                i->base_distance = metric.get_distance_from(*i->track.track(), i->track.entry());
 }
 
+float TrainRouteMetric::get_distance_from(const Track &track) const
+{
+       map<Key, Data>::const_iterator i = tracks.lower_bound(Key(&track, 0));
+       map<Key, Data>::const_iterator j = tracks.upper_bound(Key(&track, 255));
+
+       float result = -1;
+       for(; i!=j; ++i)
+       {
+               float d = i->second.distance+i->second.goal->base_distance;
+               if(result<0 || d<result)
+                       result = d;
+       }
+
+       return result;
+}
+
 float TrainRouteMetric::get_distance_from(const Track &track, unsigned exit) const
 {
        map<Key, Data>::const_iterator i = tracks.find(Key(&track, exit));