]> git.tdb.fi Git - r2c2.git/blob - source/libmarklin/track.cpp
Add framework for generating simple meshes for vehicles
[r2c2.git] / source / libmarklin / track.cpp
1 /* $Id$
2
3 This file is part of the MSP Märklin suite
4 Copyright © 2006-2010  Mikkosoft Productions, Mikko Rasa
5 Distributed under the GPL
6 */
7
8 #include <cmath>
9 #include "block.h"
10 #include "catalogue.h"
11 #include "driver.h"
12 #include "layout.h"
13 #include "track.h"
14 #include "tracktype.h"
15
16 using namespace std;
17 using namespace Msp;
18
19 namespace Marklin {
20
21 Track::Track(Layout &l, const TrackType &t):
22         layout(l),
23         type(t),
24         block(0),
25         rot(0),
26         slope(0),
27         flex(false),
28         turnout_id(type.is_turnout() ? layout.allocate_turnout_id(type.is_double_address()) : 0),
29         sensor_id(0),
30         links(type.get_endpoints().size()),
31         active_path(0)
32 {
33         layout.add_track(*this);
34
35         if(layout.has_driver())
36                 layout.get_driver().signal_turnout.connect(sigc::mem_fun(this, &Track::turnout_event));
37
38         for(unsigned paths = type.get_paths(); !(paths&1); ++active_path, paths>>=1) ;
39 }
40
41 Track::~Track()
42 {
43         break_links();
44         layout.remove_track(*this);
45 }
46
47 void Track::set_block(Block *b)
48 {
49         if(b && !b->has_track(*this))
50                 throw InvalidParameterValue("Track is not in the Block");
51         if(!b && block && block->has_track(*this))
52                 throw InvalidState("Track is still in a Block");
53
54         block = b;
55 }
56
57 Block &Track::get_block() const
58 {
59         if(!block)
60                 throw InvalidState("No Block");
61
62         return *block;
63 }
64
65 void Track::set_position(const Point &p)
66 {
67         pos = p;
68 }
69
70 void Track::set_rotation(float r)
71 {
72         rot = r;
73         while(rot<0)
74                 rot += M_PI*2;
75         while(rot>M_PI*2)
76                 rot -= M_PI*2;
77 }
78
79 void Track::set_slope(float s)
80 {
81         if(links.size()!=2)
82                 return;
83
84         slope = s;
85 }
86
87 void Track::set_flex(bool f)
88 {
89         flex = f;
90 }
91
92 void Track::check_slope()
93 {
94         if(links.size()!=2)
95                 return;
96
97         if(links[0] && links[1])
98         {
99                 Point epp0 = links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
100                 Point epp1 = links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
101                 pos.z = epp0.z;
102                 slope = epp1.z-pos.z;
103         }
104         else
105         {
106                 slope = 0;
107                 if(links[0])
108                 {
109                         Point epp = links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
110                         pos.z = epp.z;
111                 }
112                 else if(links[1])
113                 {
114                         Point epp = links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
115                         pos.z = epp.z;
116                 }
117         }
118 }
119
120 void Track::set_turnout_id(unsigned i)
121 {
122         if(!type.is_turnout())
123                 throw InvalidState("Not a turnout");
124
125         turnout_id = i;
126         layout.create_blocks(*this);
127         layout.update_routes();
128         if(layout.has_driver() && turnout_id)
129         {
130                 layout.get_driver().add_turnout(turnout_id);
131                 if(type.is_double_address())
132                         layout.get_driver().add_turnout(turnout_id+1);
133         }
134 }
135
136 void Track::set_sensor_id(unsigned i)
137 {
138         if(type.is_turnout())
139                 throw InvalidState("Can't set sensor on a turnout");
140
141         sensor_id = i;
142         layout.create_blocks(*this);
143         if(layout.has_driver() && sensor_id)
144                 layout.get_driver().add_sensor(sensor_id);
145 }
146
147 void Track::set_active_path(unsigned p)
148 {
149         if(!turnout_id)
150                 throw InvalidState("Not a turnout");
151         if(!(type.get_paths()&(1<<p)))
152                 throw InvalidParameterValue("Invalid path");
153
154         layout.get_driver().set_turnout(turnout_id, p&1);
155         if(type.is_double_address())
156                 layout.get_driver().set_turnout(turnout_id+1, p&2);
157         else if(type.get_n_paths()>2)
158                 active_path = (active_path&1) | (p&2);
159 }
160
161 int Track::get_endpoint_by_link(Track &other) const
162 {
163         for(unsigned i=0; i<links.size(); ++i)
164                 if(links[i]==&other)
165                         return i;
166
167         return -1;
168 }
169
170 Point Track::get_endpoint_position(unsigned epi) const
171 {
172         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
173         if(epi>=eps.size())
174                 throw InvalidParameterValue("TrackType::Endpoint index out of range");
175
176         const TrackType::Endpoint &ep = eps[epi];
177
178         float c = cos(rot);
179         float s = sin(rot);
180
181         Point p(pos.x+c*ep.pos.x-s*ep.pos.y, pos.y+s*ep.pos.x+c*ep.pos.y, pos.z);
182         if(eps.size()==2 && epi==1)
183                 p.z += slope;
184         return p;
185 }
186
187 float Track::get_endpoint_direction(unsigned epi) const
188 {
189         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
190         if(epi>=eps.size())
191                 throw InvalidParameterValue("TrackType::Endpoint index out of range");
192
193         const TrackType::Endpoint &ep = eps[epi];
194
195         return rot+ep.dir;
196 }
197
198 bool Track::snap_to(Track &other, bool link, float limit)
199 {
200         if(!limit || link)
201         {
202                 limit = layout.get_catalogue().get_gauge();
203                 if(link && !flex && !other.get_flex())
204                         limit /= 10;
205         }
206         limit *= limit;
207
208         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
209         const vector<TrackType::Endpoint> &other_eps = other.get_type().get_endpoints();
210
211         for(unsigned i=0; i<eps.size(); ++i)
212         {
213                 Point epp = get_endpoint_position(i);
214
215                 for(unsigned j=0; j<other_eps.size(); ++j)
216                 {
217                         if(other.get_link(j))
218                                 continue;
219
220                         Point epp2 = other.get_endpoint_position(j);
221                         float dx = epp2.x-epp.x;
222                         float dy = epp2.y-epp.y;
223                         float dz = epp2.z-epp.z;
224                         if(dx*dx+dy*dy<limit && dz*dz<limit)
225                         {
226                                 if(!link || (!flex && !other.get_flex()))
227                                 {
228                                         set_rotation(other.rot+other_eps[j].dir-eps[i].dir+M_PI);
229                                         Point p(epp2.x-(eps[i].pos.x*cos(rot)-eps[i].pos.y*sin(rot)),
230                                                 epp2.y-(eps[i].pos.y*cos(rot)+eps[i].pos.x*sin(rot)),
231                                                 epp2.z);
232                                         if(eps.size()==2 && i==1)
233                                                 p.z -= slope;
234                                         set_position(p);
235                                 }
236
237                                 if(link)
238                                 {
239                                         if(links[i])
240                                                 break_link(*links[i]);
241                                         links[i] = &other;
242                                         other.links[j] = this;
243                                         layout.create_blocks(*this);
244                                 }
245
246                                 return true;
247                         }
248                 }
249         }
250
251         return false;
252 }
253
254 bool Track::snap(Point &pt, float &d) const
255 {
256         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
257
258         for(unsigned i=0; i<eps.size(); ++i)
259         {
260                 Point epp = get_endpoint_position(i);
261                 float dx = pt.x-epp.x;
262                 float dy = pt.y-epp.y;
263                 if(dx*dx+dy*dy<1e-4)
264                 {
265                         pt = epp;
266                         d = rot+eps[i].dir;
267                         return true;
268                 }
269         }
270
271         return false;
272 }
273
274 void Track::break_link(Track &trk)
275 {
276         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
277                 if(*i==&trk)
278                 {
279                         *i = 0;
280                         trk.break_link(*this);
281                         // XXX Creates the blocks twice
282                         layout.create_blocks(*this);
283                         return;
284                 }
285 }
286
287 void Track::break_links()
288 {
289         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
290                 if(Track *trk=*i)
291                 {
292                         *i = 0;
293                         trk->break_link(*this);
294                 }
295 }
296
297 Track *Track::get_link(unsigned i) const
298 {
299         if(i>links.size())
300                 throw InvalidParameterValue("Link index out of range");
301
302         return links[i];
303 }
304
305 TrackPoint Track::get_point(unsigned epi, unsigned path, float d) const
306 {
307         TrackPoint p = type.get_point(epi, path, d);
308         float c = cos(rot);
309         float s = sin(rot);
310
311         p.pos = Point(pos.x+c*p.pos.x-s*p.pos.y, pos.y+s*p.pos.x+c*p.pos.y, pos.z);
312         p.dir += rot;
313         if(type.get_endpoints().size()==2)
314         {
315                 float len = type.get_path_length(path);
316                 float grade = slope/len;
317                 if(epi==0)
318                 {
319                         p.pos.z += grade*d;
320                         p.grade = grade;
321                 }
322                 else
323                 {
324                         p.pos.z += slope-grade*d;
325                         p.grade = -grade;
326                 }
327         }
328
329         return p;
330 }
331
332 TrackPoint Track::get_point(unsigned epi, float d) const
333 {
334         return get_point(epi, active_path, d);
335 }
336
337 void Track::save(list<DataFile::Statement> &st) const
338 {
339         st.push_back((DataFile::Statement("position"), pos.x, pos.y, pos.z));
340         st.push_back((DataFile::Statement("rotation"), rot));
341         st.push_back((DataFile::Statement("slope"), slope));
342         if(turnout_id)
343                 st.push_back((DataFile::Statement("turnout_id"), turnout_id));
344         if(sensor_id)
345                 st.push_back((DataFile::Statement("sensor_id"), sensor_id));
346         if(flex)
347                 st.push_back((DataFile::Statement("flex"), true));
348 }
349
350 void Track::turnout_event(unsigned addr, bool state)
351 {
352         if(!turnout_id)
353                 return;
354
355         if(addr==turnout_id)
356                 active_path = (active_path&2) | (state ? 1 : 0);
357         else if(type.is_double_address() && addr==turnout_id+1)
358                 active_path = (active_path&1) | (state ? 2 : 0);
359         else
360                 return;
361
362         signal_path_changed.emit(active_path);
363 }
364
365
366 Track::Loader::Loader(Track &t):
367         DataFile::BasicLoader<Track>(t)
368 {
369         add("position",   &Loader::position);
370         add("rotation",   &Track::rot);
371         add("slope",      &Track::slope);
372         add("turnout_id", &Loader::turnout_id);
373         add("sensor_id",  &Loader::sensor_id);
374         add("flex",       &Track::flex);
375 }
376
377 void Track::Loader::position(float x, float y, float z)
378 {
379         obj.pos = Point(x, y, z);
380 }
381
382 void Track::Loader::sensor_id(unsigned id)
383 {
384         obj.set_sensor_id(id);
385 }
386
387 void Track::Loader::turnout_id(unsigned id)
388 {
389         obj.set_turnout_id(id);
390 }
391
392 } // namespace Marklin