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