]> git.tdb.fi Git - r2c2.git/blob - source/libmarklin/vehicle.cpp
Fix a segfault in removing vehicles when creating a new train
[r2c2.git] / source / libmarklin / vehicle.cpp
1 /* $Id$
2
3 This file is part of the MSP Märklin suite
4 Copyright © 2010  Mikkosoft Productions, Mikko Rasa
5 Distributed under the GPL
6 */
7
8 #include <cmath>
9 #include "catalogue.h"
10 #include "driver.h"
11 #include "layout.h"
12 #include "track.h"
13 #include "trackiter.h"
14 #include "tracktype.h"
15 #include "vehicle.h"
16 #include "vehicletype.h"
17
18 using namespace std;
19 using namespace Msp;
20
21 namespace Marklin {
22
23 Vehicle::Vehicle(Layout &l, const VehicleType &t):
24         layout(l),
25         type(t),
26         next(0),
27         prev(0),
28         direction(0),
29         bogie_dirs(type.get_bogies().size()),
30         front_sensor(0),
31         back_sensor(0)
32 {
33         layout.add_vehicle(*this);
34 }
35
36 Vehicle::~Vehicle()
37 {
38         if(next)
39                 detach_back();
40         if(prev)
41                 detach_front();
42         layout.remove_vehicle(*this);
43 }
44
45 void Vehicle::attach_back(Vehicle &veh)
46 {
47         if(next || veh.prev)
48                 throw InvalidState("Already attached");
49
50         next = &veh;
51         veh.prev = this;
52
53         if(track_pos.track)
54                 propagate_backward();
55 }
56
57 void Vehicle::attach_front(Vehicle &veh)
58 {
59         if(prev || veh.next)
60                 throw InvalidState("Already attached");
61
62         prev = &veh;
63         veh.next = this;
64
65         if(prev->get_track())
66                 prev->propagate_backward();
67 }
68
69 void Vehicle::detach_back()
70 {
71         if(!next)
72                 throw InvalidState("Not attached");
73
74         next->prev = 0;
75         next = 0;
76 }
77
78 void Vehicle::detach_front()
79 {
80         if(!prev)
81                 throw InvalidState("Not attached");
82
83         prev->next = 0;
84         prev = 0;
85 }
86
87 void Vehicle::place(Track &t, unsigned e, float o, PlaceMode m)
88 {
89         track_pos = TrackPosition(&t, e, o);
90
91         if(m==FRONT_AXLE)
92                 track_pos.advance(-type.get_front_axle_offset());
93         else if(m==FRONT_BUFFER)
94                 track_pos.advance(-type.get_length()/2);
95         else if(m==BACK_AXLE)
96                 track_pos.advance(-type.get_back_axle_offset());
97         else if(m==BACK_BUFFER)
98                 track_pos.advance(type.get_length()/2);
99
100         update_position();
101         propagate_position();
102 }
103
104 void Vehicle::unplace()
105 {
106         if(!track_pos.track)
107                 return;
108
109         track_pos = TrackPosition();
110
111         if(prev)
112                 prev->unplace();
113         if(next)
114                 next->unplace();
115 }
116
117 void Vehicle::advance(float d)
118 {
119         track_pos.advance(d);
120         update_position();
121         propagate_position();
122 }
123
124 float Vehicle::get_bogie_direction(unsigned i) const
125 {
126         if(i>=bogie_dirs.size())
127                 throw InvalidParameterValue("Bogie index out of range");
128         return bogie_dirs[i];
129 }
130
131 void Vehicle::update_position()
132 {
133         TrackPoint tp;
134
135         const vector<VehicleType::Axle> &axles = type.get_axles();
136         const vector<VehicleType::Bogie> &bogies = type.get_bogies();
137         if(axles.size()>=2)
138         {
139                 float wheelbase = axles.front().position-axles.back().position;
140                 tp = get_point(track_pos, wheelbase, -axles.back().position/wheelbase);
141         }
142         else if(bogies.size()>=2)
143         {
144                 TrackPosition front = track_pos;
145                 front.advance(bogies.front().position);
146                 TrackPosition back = track_pos;
147                 back.advance(bogies.back().position);
148                 float bogie_spacing = bogies.front().position-bogies.back().position;
149                 adjust_for_distance(front, back, bogie_spacing);
150
151                 const vector<VehicleType::Axle> &front_axles = bogies.front().axles;
152                 float wheelbase = front_axles.front().position-front_axles.back().position;
153                 TrackPoint front_point = get_point(front, wheelbase, -front_axles.back().position/wheelbase);
154
155                 const vector<VehicleType::Axle> &back_axles = bogies.back().axles;
156                 wheelbase = back_axles.front().position-back_axles.back().position;
157                 TrackPoint back_point = get_point(back, wheelbase, -back_axles.back().position/wheelbase);
158
159                 tp = get_point(front_point.pos, back_point.pos, -bogies.back().position/bogie_spacing);
160
161                 bogie_dirs.front() = front_point.dir-tp.dir;
162                 bogie_dirs.back() = back_point.dir-tp.dir;
163         }
164         else
165                 tp = track_pos.get_point();
166
167         if(!prev)
168                 check_sensor(type.get_front_axle_offset(), front_sensor);
169         if(!next)
170                 check_sensor(type.get_back_axle_offset(), back_sensor);
171
172         position = tp.pos;
173         position.z += layout.get_catalogue().get_rail_elevation();
174         direction = tp.dir;
175 }
176
177 void Vehicle::update_position_from(const Vehicle &veh)
178 {
179         int sign = (&veh==prev ? -1 : 1);
180
181         float tdist = (type.get_length()+veh.type.get_length())/2;
182         float margin = layout.get_catalogue().get_scale();
183
184         float dist = distance(veh.position, position);
185         if(dist<tdist-margin || dist>tdist+margin)
186         {
187                 track_pos = veh.track_pos;
188                 track_pos.advance(sign*tdist);
189                 update_position();
190
191                 dist = distance(veh.position, position);
192         }
193
194         track_pos.advance(sign*(tdist-dist));
195         update_position();
196 }
197
198 void Vehicle::propagate_position()
199 {
200         if(prev)
201                 propagate_forward();
202         if(next)
203                 propagate_backward();
204 }
205
206 void Vehicle::propagate_forward()
207 {
208         prev->update_position_from(*this);
209
210         if(prev->prev)
211                 prev->propagate_forward();
212 }
213
214 void Vehicle::propagate_backward()
215 {
216         next->update_position_from(*this);
217
218         if(next->next)
219                 next->propagate_backward();
220 }
221
222 void Vehicle::check_sensor(float offset, unsigned &sensor)
223 {
224         TrackPosition pos = track_pos;
225         pos.advance(offset);
226         unsigned s = pos.track->get_sensor_id();
227         if(s!=sensor)
228         {
229                 /* Sensor ID under axle has changed.  Deduce movement direction by using
230                 the sensor ID under the midpoint of the vehicle. */
231                 /* XXX This depends on the simulation running fast enough.  Something
232                 more robust would be preferable. */
233                 unsigned old = sensor;
234                 sensor = s;
235                 unsigned mid = track_pos.track->get_sensor_id();
236
237                 if(s && s!=mid)
238                         /* There's a sensor and it's different from mid.  We've just entered
239                         that sensor. */
240                         layout.get_driver().set_sensor(sensor, true);
241                 if(old && old!=mid)
242                         /* A sensor was under the axle and it was different from mid.  We've
243                         just left that sensor. */
244                         layout.get_driver().set_sensor(old, false);
245         }
246 }
247
248 void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, float tdist, float ratio) const
249 {
250         float margin = 0.01*layout.get_catalogue().get_scale();
251         int adjust_dir = 0;
252         while(1)
253         {
254                 Point front_point = front.get_point().pos;
255                 Point back_point = back.get_point().pos;
256
257                 float dx = front_point.x-back_point.x;
258                 float dy = front_point.y-back_point.y;
259                 float dz = front_point.z-back_point.z;
260                 float dist = sqrt(dx*dx+dy*dy+dz*dz);
261
262                 float diff = tdist-dist;
263                 if(diff<-margin && adjust_dir<=0)
264                 {
265                         diff -= margin;
266                         adjust_dir = -1;
267                 }
268                 else if(diff>margin && adjust_dir>=0)
269                 {
270                         diff += margin;
271                         adjust_dir = 1;
272                 }
273                 else
274                         return;
275
276                 front.advance(diff*(1-ratio));
277                 back.advance(-diff*ratio);
278         }
279 }
280
281 TrackPoint Vehicle::get_point(const Point &front, const Point &back, float ratio) const
282 {
283         float dx = front.x-back.x;
284         float dy = front.y-back.y;
285         float dz = front.z-back.z;
286
287         TrackPoint tp;
288         tp.pos = Point(back.x+dx*ratio, back.y+dy*ratio, back.z+dz*ratio);
289         tp.dir = atan2(dy, dx);
290
291         return tp;
292 }
293
294 TrackPoint Vehicle::get_point(const TrackPosition &pos, float tdist, float ratio) const
295 {
296         TrackPosition front = pos;
297         front.advance(tdist*(1-ratio));
298
299         TrackPosition back = pos;
300         back.advance(-tdist*ratio);
301
302         adjust_for_distance(front, back, tdist, ratio);
303         return get_point(front.get_point().pos, back.get_point().pos, ratio);
304 }
305
306
307 Vehicle::TrackPosition::TrackPosition():
308         track(0),
309         ep(0),
310         offs(0)
311 { }
312
313 Vehicle::TrackPosition::TrackPosition(Track *t, unsigned e, float o):
314         track(t),
315         ep(e),
316         offs(o)
317 { }
318
319 void Vehicle::TrackPosition::advance(float d)
320 {
321         if(!track)
322                 return;
323
324         offs += d;
325         TrackIter iter(track, ep);
326         while(iter)
327         {
328                 float path_len = iter->get_type().get_path_length(iter->get_active_path());
329
330                 if(offs>path_len)
331                 {
332                         offs -= path_len;
333                         iter = iter.next();
334                 }
335                 else
336                         break;
337         }
338
339         while(iter && offs<0)
340         {
341                 iter = iter.flip().reverse();
342
343                 if(iter)
344                 {
345                         float path_len = iter->get_type().get_path_length(iter->get_active_path());
346                         offs += path_len;
347                 }
348         }
349
350         track = iter.track();
351         ep = iter.entry();
352         if(!track)
353                 offs = 0;
354 }
355
356 TrackPoint Vehicle::TrackPosition::get_point() const
357 {
358         if(track)
359                 return track->get_point(ep, offs);
360         else
361                 return TrackPoint();
362 }
363
364 } // namespace Marklin