]> git.tdb.fi Git - r2c2.git/blob - source/libmarklin/route.cpp
Fix pathfinder to not block itself in some situations
[r2c2.git] / source / libmarklin / route.cpp
1 /* $Id$
2
3 This file is part of the MSP Märklin suite
4 Copyright © 2007-2010  Mikkosoft Productions, Mikko Rasa
5 Distributed under the GPL
6 */
7
8 #include <queue>
9 #include <msp/strings/formatter.h>
10 #include "layout.h"
11 #include "route.h"
12 #include "track.h"
13 #include "tracktype.h"
14
15 using namespace std;
16 using namespace Msp;
17
18 namespace {
19
20 using namespace Marklin;
21
22 typedef std::pair<const Track *, unsigned> Key;
23
24 struct Node
25 {
26         const Track *track;
27         unsigned ep;
28         Node *prev;
29         float dist;
30
31         Node():
32                 track(0), ep(0), prev(0), dist(0)
33         { }
34
35         Node(const Track &t, unsigned e):
36                 track(&t), ep(e), prev(0), dist(0)
37         { }
38
39         Node(const Track &t, unsigned e, Node &r, float d):
40                 track(&t), ep(e), prev(&r), dist(prev->dist+d)
41         { }
42
43         bool operator<(const Node &other) const
44         { return dist>other.dist; }
45 };
46
47 struct TrackMatch
48 {
49         const Track &track;
50
51         TrackMatch(const Track &t): track(t) { }
52
53         bool operator()(const Track &t) const { return &t==&track; }
54 };
55
56 struct TrackInRoute
57 {
58         const Route &route;
59
60         TrackInRoute(const Route &r): route(r) { }
61
62         bool operator()(const Track &t) const { return route.get_tracks().count(&t); }
63 };
64
65 template<typename Pred>
66 list<const Track *> dijkstra(const Track &from, unsigned ep, const Pred &goal)
67 {
68         map<Key, Node> track_nodes;
69         priority_queue<Node> nodes;
70         Node *final = 0;
71
72         nodes.push(Node(from, ep));
73
74         while(!nodes.empty())
75         {
76                 Node lowest = nodes.top();
77                 nodes.pop();
78
79                 Key key(lowest.track, lowest.ep);
80                 if(track_nodes.count(key))
81                         continue;
82
83                 Node &ref = track_nodes[key] = lowest;
84                 if(goal(*lowest.track))
85                 {
86                         final = &ref;
87                         break;
88                 }
89
90                 const TrackType &type = lowest.track->get_type();
91                 const vector<Endpoint> &eps = type.get_endpoints();
92                 const vector<Track *> &links = lowest.track->get_links();
93                 for(unsigned i=0; i<eps.size(); ++i)
94                 {
95                         if(i==lowest.ep || !links[i])
96                                 continue;
97
98                         unsigned link_ep = links[i]->get_endpoint_by_link(*lowest.track);
99                         if(track_nodes.count(Key(links[i], link_ep)))
100                                 continue;
101
102                         unsigned mask = eps[i].paths&eps[lowest.ep].paths;
103                         if(!mask)
104                                 continue;
105
106                         unsigned path=0;
107                         for(; !(mask&1); ++path, mask>>=1) ;
108                         nodes.push(Node(*links[i], links[i]->get_endpoint_by_link(*lowest.track), ref, type.get_path_length(path)));
109                 }
110         }
111
112         if(!final)
113                 throw InvalidParameterValue("Could not find a route");
114
115         list<const Track *> result;
116         for(Node *node=final; node; node=node->prev)
117                 result.push_front(node->track);
118
119         return result;
120 }
121
122 unsigned count = 0;
123
124 template<typename Pred>
125 Route *create_route(const Track &from, unsigned ep, const Pred &goal)
126 {
127         list<const Track *> tracks = dijkstra(from, ep, goal);
128
129         Route *route = new Route(from.get_layout(), format("-%d-", ++count));
130         for(list<const Track *>::iterator i=tracks.begin(); i!=tracks.end(); ++i)
131                 route->add_track(**i);
132
133         return route;
134 }
135
136 }
137
138
139 namespace Marklin {
140
141 Route::Route(Layout &l, const string &n):
142         layout(l),
143         name(n)
144 {
145         layout.add_route(*this);
146         layout.signal_track_removed.connect(sigc::mem_fun(this, &Route::track_removed));
147 }
148
149 Route::~Route()
150 {
151         layout.remove_route(*this);
152 }
153
154 int Route::get_turnout(unsigned id) const
155 {
156         map<unsigned, int>::const_iterator i = turnouts.find(id);
157         if(i!=turnouts.end())
158                 return i->second;
159         return -1;
160 }
161
162 void Route::add_track(const Track &trk)
163 {
164         if(tracks.count(&trk))
165                 return;
166
167         if(!tracks.empty())
168         {
169                 unsigned valid = check_validity(trk);
170                 if(!(valid&1))
171                         throw Exception("Not linked to existing tracks");
172                 else if(!(valid&2))
173                         throw Exception("Branching routes not allowed");
174                 else if(!(valid&4))
175                         throw Exception("Route must be smooth");
176         }
177
178         tracks.insert(&trk);
179         update_turnouts();
180 }
181
182 void Route::add_tracks(const set<const Track *> &trks)
183 {
184         set<const Track *> pending;
185         for(set<const Track *>::const_iterator i=trks.begin(); i!=trks.end(); ++i)
186                 if(!tracks.count(*i))
187                         pending.insert(*i);
188
189         while(!pending.empty())
190         {
191                 bool found = false;
192                 for(set<const Track *>::const_iterator i=pending.begin(); i!=pending.end(); ++i)
193                         if(tracks.empty() || check_validity(**i)==7)
194                         {
195                                 tracks.insert(*i);
196                                 pending.erase(*i);
197                                 found = true;
198                                 break;
199                         }
200
201                 if(!found)
202                         throw Exception("Could not add all tracks to route");
203         }
204
205         update_turnouts();
206 }
207
208 void Route::save(list<DataFile::Statement> &st) const
209 {
210         for(map<unsigned, int>::const_iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
211                 st.push_back((DataFile::Statement("turnout"), i->first, i->second));
212 }
213
214 void Route::update_turnouts()
215 {
216         set<unsigned> found;
217         for(set<const Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
218                 if(unsigned tid=(*i)->get_turnout_id())
219                 {
220                         found.insert(tid);
221
222                         const vector<Endpoint> &endpoints = (*i)->get_type().get_endpoints();
223                         const vector<Track *> &links = (*i)->get_links();
224
225                         // Build a combined path mask from linked endpoints
226                         unsigned mask = 15;
227                         for(unsigned j=0; j<endpoints.size(); ++j)
228                         {
229                                 if(!tracks.count(links[j]))
230                                         continue;
231
232                                 if(unsigned tid2=links[j]->get_turnout_id())
233                                 {
234                                         const Endpoint &ep = links[j]->get_type().get_endpoints()[links[j]->get_endpoint_by_link(**i)];
235                                         int p = get_turnout(tid2);
236                                         if(p>=0 && !(ep.paths&(1<<p)))
237                                         {
238                                                 // The linked track is a turnout and has a path which is incompatible with this endpoint
239                                                 mask &= ~endpoints[j].paths;
240                                                 continue;
241                                         }
242                                 }
243                                 mask &= endpoints[j].paths;
244                         }
245
246                         if(mask && !(mask&(mask-1)))
247                         {
248                                 // Exactly one possible choice, set the path accordingly
249                                 unsigned path = 0;
250                                 for(; (mask && !(mask&1)); mask>>=1, ++path) ;
251                                 turnouts[tid] = path;
252                         }
253                         else if(!turnouts.count(tid))
254                                 // More than one possible choice, and no existing entry - set as undecided
255                                 turnouts[tid] = -1;
256                 }
257
258         // Remove any turnouts that do not exist in the route
259         for(map<unsigned, int>::iterator i=turnouts.begin(); i!=turnouts.end();)
260         {
261                 if(!found.count(i->first))
262                         turnouts.erase(i++);
263                 else
264                         ++i;
265         }
266 }
267
268 unsigned Route::check_validity(const Track &trk) const
269 {
270         unsigned result = 4;
271         for(set<const Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
272         {
273                 int epi=(*i)->get_endpoint_by_link(trk);
274                 if(epi>=0)
275                 {
276                         // Linked to an existing track - good
277                         result |= 1;
278                         const vector<Endpoint> &endpoints = (*i)->get_type().get_endpoints();
279                         if(unsigned tid=(*i)->get_turnout_id())
280                         {
281                                 int r = get_turnout(tid);
282                                 if(r>=0)
283                                 {
284                                         // Linking to a turnout with path set is only good if we're continuing that path
285                                         if(endpoints[epi].paths&(1<<r))
286                                                 result |= 2;
287                                 }
288                                 else
289                                 {
290                                         // Linked to a turnout with no path set - find out other linked tracks 
291                                         unsigned count = 0;
292                                         const vector<Track *> &links = (*i)->get_links();
293                                         int epj = -1;
294                                         for(unsigned k=0; k<endpoints.size(); ++k)
295                                                 if(tracks.count(links[k]))
296                                                 {
297                                                         ++count;
298                                                         epj = k;
299                                                 }
300
301                                         // Only good if at most one other track is linked to the turnout
302                                         if(count<=1)
303                                         {
304                                                 result |= 2;
305                                                 if(epj>=0 && !(endpoints[epi].paths&endpoints[epj].paths))
306                                                         // Impossible path through the turnout - not good
307                                                         result &= 3;
308                                         }
309                                 }
310                         }
311                         else
312                                 // Linked to something linear - good
313                                 result |= 2;
314                 }
315         }
316
317         return result;
318 }
319
320 void Route::track_removed(Track &t)
321 {
322         tracks.erase(&t);
323 }
324
325 Route *Route::find(const Track &from, unsigned ep, const Track &to)
326 {
327         return create_route(from, ep, TrackMatch(to));
328 }
329
330 Route *Route::find(const Track &from, unsigned ep, const Route &to)
331 {
332         return create_route(from, ep, TrackInRoute(to));
333 }
334
335
336 Route::Loader::Loader(Route &r):
337         DataFile::BasicLoader<Route>(r)
338 {
339         add("turnout", &Loader::turnout);
340 }
341
342 void Route::Loader::turnout(unsigned id, unsigned path)
343 {
344         obj.turnouts[id] = path;
345 }
346
347 } // namespace Marklin