]> git.tdb.fi Git - r2c2.git/blob - source/libr2c2/track.cpp
Allow zones with no qualifier or no number
[r2c2.git] / source / libr2c2 / track.cpp
1 #include <cmath>
2 #include <msp/core/maputils.h>
3 #include <msp/strings/format.h>
4 #include "block.h"
5 #include "catalogue.h"
6 #include "driver.h"
7 #include "layout.h"
8 #include "track.h"
9 #include "trackattachment.h"
10 #include "tracktype.h"
11
12 using namespace std;
13 using namespace Msp;
14
15 namespace {
16
17 struct AttachmentCompare
18 {
19         unsigned entry;
20
21         AttachmentCompare(unsigned e): entry(e) { }
22
23         bool operator()(const R2C2::TrackAttachment *a1, const R2C2::TrackAttachment *a2) const
24         { return a1->get_offset_from_endpoint(entry)<a2->get_offset_from_endpoint(entry); }
25 };
26
27 }
28
29 namespace R2C2 {
30
31 Track::Track(Layout &l, const TrackType &t):
32         Object(l),
33         type(t),
34         block(0),
35         slope(0),
36         flex(false),
37         turnout_addr(0),
38         sensor_addr(0),
39         links(type.get_endpoints().size()),
40         active_path(0),
41         path_changing(false),
42         preferred_exit(-1)
43 {
44         if(type.is_turnout())
45         {
46                 turnout_addr = layout.allocate_turnout_address();
47
48                 if(layout.has_driver())
49                 {
50                         Driver &driver = layout.get_driver();
51                         turnout_id = driver.add_turnout(turnout_addr, type);
52                         driver.signal_turnout.connect(sigc::mem_fun(this, &Track::turnout_event));
53                         driver.signal_turnout_failed.connect(sigc::mem_fun(this, &Track::turnout_failed));
54                 }
55         }
56
57         layout.add(*this);
58
59         for(unsigned paths = type.get_paths(); !(paths&1); ++active_path, paths>>=1) ;
60 }
61
62 Track::~Track()
63 {
64         break_links();
65         if(layout.has_driver() && turnout_id)
66                 layout.get_driver().remove_turnout(turnout_id);
67         layout.remove(*this);
68 }
69
70 Track *Track::clone(Layout *to_layout) const
71 {
72         Track *track = new Track((to_layout ? *to_layout : layout), type);
73         track->set_position(position);
74         track->set_rotation(rotation);
75         return track;
76 }
77
78 void Track::set_block(Block *b)
79 {
80         if(b && !b->has_track(*this))
81                 throw logic_error("track not in block");
82         if(!b && block && block->has_track(*this))
83                 throw logic_error("track still in block");
84
85         block = b;
86 }
87
88 Block &Track::get_block() const
89 {
90         if(!block)
91                 throw logic_error("!block");
92
93         return *block;
94 }
95
96 void Track::set_position(const Vector &p)
97 {
98         position = p;
99         signal_moved.emit();
100         propagate_slope();
101 }
102
103 void Track::set_rotation(const Angle &r)
104 {
105         rotation = wrap_positive(r);
106         signal_moved.emit();
107 }
108
109 void Track::set_tilt(const Angle &t)
110 {
111         if(links.size()!=2)
112                 return;
113
114         tilt = t;
115         slope = tan(tilt)*type.get_path_length(0);
116         signal_moved.emit();
117         propagate_slope();
118 }
119
120 void Track::set_flex(bool f)
121 {
122         flex = f;
123 }
124
125 void Track::propagate_slope()
126 {
127         for(vector<Track *>::const_iterator i=links.begin(); i!=links.end(); ++i)
128                 if(*i)
129                         (*i)->check_slope();
130 }
131
132 void Track::check_slope()
133 {
134         if(links.size()!=2)
135                 return;
136
137         if(links[0] && links[1])
138         {
139                 Vector epp0 = links[0]->get_snap_node(links[0]->get_link_slot(*this)).position;
140                 Vector epp1 = links[1]->get_snap_node(links[1]->get_link_slot(*this)).position;
141                 position.z = epp0.z;
142                 slope = epp1.z-position.z;
143                 tilt = Geometry::atan(slope/type.get_path_length(0));
144         }
145         else
146         {
147                 if(links[0])
148                 {
149                         Vector epp = links[0]->get_snap_node(links[0]->get_link_slot(*this)).position;
150                         position.z = epp.z;
151                 }
152                 else if(links[1])
153                 {
154                         Vector epp = links[1]->get_snap_node(links[1]->get_link_slot(*this)).position;
155                         position.z = epp.z-slope;
156                 }
157         }
158
159         signal_moved.emit();
160 }
161
162 void Track::set_turnout_address(unsigned a)
163 {
164         if(!type.is_turnout())
165                 throw logic_error("not a turnout");
166         if(!a)
167                 throw invalid_argument("Track::set_turnout_id");
168
169         Driver *driver = (layout.has_driver() ? &layout.get_driver() : 0);
170
171         if(driver && turnout_id)
172                 driver->remove_turnout(turnout_id);
173         turnout_addr = a;
174         layout.create_blocks(*this);
175         layout.update_routes();
176         if(driver && turnout_addr)
177                 turnout_id = driver->add_turnout(turnout_addr, type);
178         else
179                 turnout_id = 0;
180 }
181
182 void Track::set_sensor_address(unsigned a)
183 {
184         if(type.is_turnout())
185                 throw logic_error("is a turnout");
186
187         sensor_addr = a;
188         layout.create_blocks(*this);
189 }
190
191 void Track::set_preferred_exit(int e)
192 {
193         preferred_exit = e;
194 }
195
196 void Track::set_active_path(unsigned p)
197 {
198         if(!type.is_turnout())
199                 throw logic_error("not a turnout");
200         if(!(type.get_paths()&(1<<p)))
201                 throw invalid_argument("Track::set_active_path");
202
203         if(active_path==p)
204                 return;
205
206         signal_path_changing(p);
207         path_changing = true;
208         layout.get_driver().set_turnout(turnout_id, p);
209 }
210
211 float Track::get_path_length(int p) const
212 {
213         if(p<0)
214                 p = active_path;
215         return type.get_path_length(p);
216 }
217
218 OrientedPoint Track::get_point(unsigned epi, unsigned path, float d) const
219 {
220         OrientedPoint p = type.get_point(epi, path, d);
221
222         p.position = position+rotated_vector(p.position, rotation);
223         p.rotation += rotation;
224         if(type.get_endpoints().size()==2)
225         {
226                 float dz = tan(tilt)*d;
227                 if(epi==0)
228                 {
229                         p.position.z += dz;
230                         p.tilt = tilt;
231                 }
232                 else
233                 {
234                         p.position.z += slope-dz;
235                         p.tilt = -tilt;
236                 }
237         }
238
239         return p;
240 }
241
242 OrientedPoint Track::get_point(unsigned epi, float d) const
243 {
244         return get_point(epi, active_path, d);
245 }
246
247 unsigned Track::get_n_snap_nodes() const
248 {
249         return type.get_endpoints().size();
250 }
251
252 Snap Track::get_snap_node(unsigned i) const
253 {
254         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
255         if(i>=eps.size())
256                 throw out_of_range("Track::get_snap_node");
257
258         Snap result;
259         const TrackType::Endpoint &ep = eps[i];
260
261         result.position = position+rotated_vector(ep.pos, rotation);
262         if(eps.size()==2 && i==1)
263                 result.position.z += slope;
264
265         result.rotation = rotation+ep.dir;
266
267         return result;
268 }
269
270 bool Track::snap(Snap &sn, float limit, SnapType what) const
271 {
272         if(Object::snap(sn, limit, what))
273                 return true;
274
275         if(what&SNAP_SEGMENT)
276         {
277                 Vector local = rotated_vector(sn.position-position, -rotation);
278
279                 OrientedPoint np = type.get_nearest_point(local);
280                 Vector span = local-np.position;
281                 if(dot(span, span)<=limit*limit)
282                 {
283                         sn.position = position+rotated_vector(np.position, rotation);
284                         sn.rotation = np.rotation+rotation;
285                         return true;
286                 }
287         }
288
289         return false;
290 }
291
292 SnapType Track::get_default_snap_type_to(const Object &other) const
293 {
294         if(dynamic_cast<const Track *>(&other))
295                 return SNAP_NODE;
296
297         return NO_SNAP;
298 }
299
300 unsigned Track::get_n_link_slots() const
301 {
302         return links.size();
303 }
304
305 Track *Track::get_link(unsigned i) const
306 {
307         if(i>=links.size())
308                 throw out_of_range("Track::get_link");
309
310         return links[i];
311 }
312
313 int Track::get_link_slot(const Object &other) const
314 {
315         for(unsigned i=0; i<links.size(); ++i)
316                 if(links[i]==&other)
317                         return i;
318
319         return -1;
320 }
321
322 bool Track::link_to(Object &other)
323 {
324         Track *otrack = dynamic_cast<Track *>(&other);
325         if(!otrack)
326                 return false;
327
328         float gauge_ratio = otrack->get_type().get_gauge()/type.get_gauge();
329         if(gauge_ratio<0.99 || gauge_ratio>1.01)
330                 return false;
331
332         float limit = type.get_gauge();
333         if(!flex && !otrack->get_flex())
334                 limit /= 10;
335         limit *= limit;
336
337         unsigned nsn = get_n_snap_nodes();
338         unsigned other_nsn = other.get_n_snap_nodes();
339         for(unsigned i=0; i<nsn; ++i)
340         {
341                 Snap sn = get_snap_node(i);
342                 for(unsigned j=0; j<other_nsn; ++j)
343                 {
344                         Snap osn = other.get_snap_node(j);
345                         Vector span = osn.position-sn.position;
346                         Angle da = wrap_balanced(osn.rotation-sn.rotation-Angle::half_turn());
347
348                         if(dot(span, span)<limit && abs(da).radians()<0.01)
349                         {
350                                 break_link(i);
351                                 otrack->break_link(j);
352                                 links[i] = otrack;
353                                 otrack->links[j] = this;
354                                 check_slope();
355                                 layout.create_blocks(*this);
356
357                                 signal_link_changed.emit(i, otrack);
358                                 otrack->signal_link_changed.emit(j, this);
359                                 return true;
360                         }
361                 }
362         }
363
364         return false;
365 }
366
367 bool Track::break_link(unsigned i)
368 {
369         if(i>=links.size())
370                 throw out_of_range("Track::break_link");
371
372         Track *other = links[i];
373         if(!other)
374                 return false;
375
376         links[i] = 0;
377         if(!other->break_link(*this))
378         {
379                 /* If the call doesn't succeed, it means that the other track already
380                 broke the link and is calling us right now.  Recreate blocks in the inner
381                 call so it occurs before any signals are emitted. */
382                 layout.create_blocks(*this);
383         }
384
385         signal_link_changed.emit(i, 0);
386
387         return true;
388 }
389
390 void Track::add_attachment(TrackAttachment &a)
391 {
392         if(find(attachments.begin(), attachments.end(), &a)!=attachments.end())
393                 throw key_error(&a);
394         attachments.push_back(&a);
395 }
396
397 void Track::remove_attachment(TrackAttachment &a)
398 {
399         AttachmentList::iterator i = find(attachments.begin(), attachments.end(), &a);
400         if(i==attachments.end())
401                 throw key_error(&a);
402         attachments.erase(i);
403 }
404
405 Track::AttachmentList Track::get_attachments_ordered(unsigned epi) const
406 {
407         AttachmentList result = attachments;
408         result.sort(AttachmentCompare(epi));
409         return result;
410 }
411
412 void Track::save(list<DataFile::Statement> &st) const
413 {
414         st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
415         st.push_back((DataFile::Statement("rotation"), rotation.radians()));
416         st.push_back((DataFile::Statement("tilt"), tilt.radians()));
417         if(turnout_addr)
418                 st.push_back((DataFile::Statement("turnout_address"), turnout_addr));
419         if(sensor_addr)
420                 st.push_back((DataFile::Statement("sensor_address"), sensor_addr));
421         if(flex)
422                 st.push_back((DataFile::Statement("flex"), true));
423 }
424
425 void Track::save_dynamic(list<DataFile::Statement> &st) const
426 {
427         if(turnout_addr)
428                 st.push_back((DataFile::Statement("path"), active_path));
429 }
430
431 void Track::turnout_event(unsigned id, unsigned state)
432 {
433         if(id==turnout_id)
434         {
435                 active_path = state;
436                 path_changing = false;
437                 signal_path_changed.emit(active_path);
438         }
439 }
440
441 void Track::turnout_failed(unsigned id)
442 {
443         if(id==turnout_id)
444         {
445                 path_changing = false;
446                 layout.emergency(block, "Turnout failed");
447         }
448 }
449
450
451 Track::Loader::Loader(Track &t):
452         DataFile::ObjectLoader<Track>(t)
453 {
454         add("path",       &Loader::path);
455         add("position",   &Loader::position);
456         add("rotation",   &Loader::rotation);
457         add("tilt",       &Loader::tilt);
458         add("turnout_id", &Loader::turnout_address);
459         add("turnout_address", &Loader::turnout_address);
460         add("sensor_id",  &Loader::sensor_address);
461         add("sensor_address",  &Loader::sensor_address);
462         add("flex",       &Track::flex);
463
464         // deprecated
465         add("slope",      &Loader::slope);
466 }
467
468 void Track::Loader::path(unsigned p)
469 {
470         obj.set_active_path(p);
471         if(obj.path_changing)
472         {
473                 obj.active_path = p;
474                 obj.signal_path_changed.emit(p);
475         }
476 }
477
478 void Track::Loader::position(float x, float y, float z)
479 {
480         obj.set_position(Vector(x, y, z));
481 }
482
483 void Track::Loader::rotation(float r)
484 {
485         obj.set_rotation(Angle::from_radians(r));
486 }
487
488 void Track::Loader::sensor_address(unsigned addr)
489 {
490         obj.set_sensor_address(addr);
491 }
492
493 void Track::Loader::slope(float s)
494 {
495         obj.set_tilt(Geometry::atan(s/obj.type.get_path_length(0)));
496 }
497
498 void Track::Loader::tilt(float t)
499 {
500         obj.set_tilt(Angle::from_radians(t));
501 }
502
503 void Track::Loader::turnout_address(unsigned addr)
504 {
505         obj.set_turnout_address(addr);
506 }
507
508 } // namespace R2C2