Fix pathfinder to not block itself in some situations

Better handling of routes at the beginning of Train::reserve_more
Delay clearing route until stopping

diff --git a/source/libmarklin/route.cpp b/source/libmarklin/route.cpp
index 2ff6962fe67551a17be8bde9052595b6933bf171..ace8d0b950812c56a22e23a0ba381f4d389b9f7b 100644 (file)
--- a/source/libmarklin/route.cpp
+++ b/source/libmarklin/route.cpp
@@ -19,6 +19,8 @@ namespace {
 
 using namespace Marklin;
 
+typedef std::pair<const Track *, unsigned> Key;
+
 struct Node
 {
        const Track *track;
@@ -63,8 +65,7 @@ struct TrackInRoute
 template<typename Pred>
 list<const Track *> dijkstra(const Track &from, unsigned ep, const Pred &goal)
 {
-       // XXX Fails to find some routes - should use track+ep as key
-       map<const Track *, Node> track_nodes;
+       map<Key, Node> track_nodes;
        priority_queue<Node> nodes;
        Node *final = 0;
 
@@ -75,10 +76,11 @@ list<const Track *> dijkstra(const Track &from, unsigned ep, const Pred &goal)
                Node lowest = nodes.top();
                nodes.pop();
 
-               if(track_nodes.count(lowest.track))
+               Key key(lowest.track, lowest.ep);
+               if(track_nodes.count(key))
                        continue;
 
-               Node &ref = track_nodes[lowest.track] = lowest;
+               Node &ref = track_nodes[key] = lowest;
                if(goal(*lowest.track))
                {
                        final = &ref;
@@ -90,7 +92,11 @@ list<const Track *> dijkstra(const Track &from, unsigned ep, const Pred &goal)
                const vector<Track *> &links = lowest.track->get_links();
                for(unsigned i=0; i<eps.size(); ++i)
                {
-                       if(i==lowest.ep || !links[i] || track_nodes.count(links[i]))
+                       if(i==lowest.ep || !links[i])
+                               continue;
+
+                       unsigned link_ep = links[i]->get_endpoint_by_link(*lowest.track);
+                       if(track_nodes.count(Key(links[i], link_ep)))
                                continue;
 
                        unsigned mask = eps[i].paths&eps[lowest.ep].paths;

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 8ddb66bee3c5047a7c287a6925220d3456a1af0b..d5b2b226721c961fb88b96753434d665f1f0180d 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -343,6 +343,8 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
                        vehicles[0]->advance(reverse ? -d : d);
                }
        }
+       else if(end_of_route)
+               set_route(0);
 }
 
 void Train::save(list<DataFile::Statement> &st) const
@@ -461,10 +463,7 @@ void Train::sensor_event(unsigned addr, bool state)
                        {
                                unsigned nsens = reserve_more();
                                if(!nsens && end_of_route)
-                               {
                                        signal_arrived.emit();
-                                       set_route(0);
-                               }
                        }
                }
        }
@@ -525,15 +524,20 @@ unsigned Train::reserve_more()
                if(i->block->get_sensor_id())
                        ++nsens;
        
+       if(end_of_route)
+               return nsens;
+
        const Route *cur_route = 0;
        if(route)
        {
-               unsigned exit = last->block->traverse(last->entry);
-               Track *track = last->block->get_endpoints()[exit].track;
-               if(route->get_tracks().count(track))
-                       cur_route = route;
-               else if(next_route && next_route->get_tracks().count(track))
-                       cur_route = next_route;
+               const set<Track *> &tracks = last->block->get_tracks();
+               for(set<Track *>::const_iterator i=tracks.begin(); (cur_route!=route && i!=tracks.end()); ++i)
+               {
+                       if(route->get_tracks().count(*i))
+                               cur_route = route;
+                       else if(next_route && next_route->get_tracks().count(*i))
+                               cur_route = next_route;
+               }
        }
 
        bool got_more = false;