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