3 This file is part of the MSP Märklin suite
4 Copyright © 2010 Mikkosoft Productions, Mikko Rasa
5 Distributed under the GPL
13 #include "tracktype.h"
15 #include "vehicletype.h"
22 Vehicle::Vehicle(Layout &l, const VehicleType &t):
28 bogie_dirs(type.get_bogies().size()),
32 layout.add_vehicle(*this);
41 layout.remove_vehicle(*this);
44 void Vehicle::attach_back(Vehicle &veh)
47 throw InvalidState("Already attached");
56 void Vehicle::attach_front(Vehicle &veh)
59 throw InvalidState("Already attached");
65 prev->propagate_backward();
68 void Vehicle::detach_back()
71 throw InvalidState("Not attached");
77 void Vehicle::detach_front()
80 throw InvalidState("Not attached");
86 void Vehicle::place(Track *t, unsigned e, float o, PlaceMode m)
88 track_pos = TrackPosition(t, e, o);
91 track_pos.advance(-type.get_front_axle_offset());
92 else if(m==FRONT_BUFFER)
93 track_pos.advance(-type.get_length()/2);
95 track_pos.advance(-type.get_back_axle_offset());
96 else if(m==BACK_BUFFER)
97 track_pos.advance(type.get_length()/2);
100 propagate_position();
103 void Vehicle::advance(float d)
105 track_pos.advance(d);
107 propagate_position();
110 float Vehicle::get_bogie_direction(unsigned i) const
112 if(i>=bogie_dirs.size())
113 throw InvalidParameterValue("Bogie index out of range");
114 return bogie_dirs[i];
117 void Vehicle::update_position()
121 const vector<VehicleType::Axle> &axles = type.get_axles();
122 const vector<VehicleType::Bogie> &bogies = type.get_bogies();
125 float wheelbase = axles.front().position-axles.back().position;
126 tp = get_point(track_pos, wheelbase, -axles.back().position/wheelbase);
128 else if(bogies.size()>=2)
130 TrackPosition front = track_pos;
131 front.advance(bogies.front().position);
132 TrackPosition back = track_pos;
133 back.advance(bogies.back().position);
134 float bogie_spacing = bogies.front().position-bogies.back().position;
135 adjust_for_distance(front, back, bogie_spacing);
137 const vector<VehicleType::Axle> &front_axles = bogies.front().axles;
138 float wheelbase = front_axles.front().position-front_axles.back().position;
139 TrackPoint front_point = get_point(front, wheelbase, -front_axles.back().position/wheelbase);
141 const vector<VehicleType::Axle> &back_axles = bogies.back().axles;
142 wheelbase = back_axles.front().position-back_axles.back().position;
143 TrackPoint back_point = get_point(back, wheelbase, -back_axles.back().position/wheelbase);
145 tp = get_point(front_point.pos, back_point.pos, -bogies.back().position/bogie_spacing);
147 bogie_dirs.front() = front_point.dir-tp.dir;
148 bogie_dirs.back() = back_point.dir-tp.dir;
151 tp = track_pos.get_point();
154 check_sensor(type.get_front_axle_offset(), front_sensor);
156 check_sensor(type.get_back_axle_offset(), back_sensor);
159 position.z += layout.get_catalogue().get_rail_elevation();
163 void Vehicle::update_position_from(const Vehicle &veh)
165 int sign = (&veh==prev ? -1 : 1);
167 float tdist = (type.get_length()+veh.type.get_length())/2;
168 float margin = layout.get_catalogue().get_scale();
170 float dist = distance(veh.position, position);
171 if(dist<tdist-margin || dist>tdist+margin)
173 track_pos = veh.track_pos;
174 track_pos.advance(sign*tdist);
177 dist = distance(veh.position, position);
180 track_pos.advance(sign*(tdist-dist));
184 void Vehicle::propagate_position()
189 propagate_backward();
192 void Vehicle::propagate_forward()
194 prev->update_position_from(*this);
197 prev->propagate_forward();
200 void Vehicle::propagate_backward()
202 next->update_position_from(*this);
205 next->propagate_backward();
208 void Vehicle::check_sensor(float offset, unsigned &sensor)
210 TrackPosition pos = track_pos;
212 unsigned s = pos.track->get_sensor_id();
215 /* Sensor ID under axle has changed. Deduce movement direction by using
216 the sensor ID under the midpoint of the vehicle. */
217 /* XXX This depends on the simulation running fast enough. Something
218 more robust would be preferable. */
219 unsigned old = sensor;
221 unsigned mid = track_pos.track->get_sensor_id();
224 /* There's a sensor and it's different from mid. We've just entered
226 layout.get_driver().set_sensor(sensor, true);
228 /* A sensor was under the axle and it was different from mid. We've
229 just left that sensor. */
230 layout.get_driver().set_sensor(old, false);
234 void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, float tdist, float ratio) const
236 float margin = 0.01*layout.get_catalogue().get_scale();
240 Point front_point = front.get_point().pos;
241 Point back_point = back.get_point().pos;
243 float dx = front_point.x-back_point.x;
244 float dy = front_point.y-back_point.y;
245 float dz = front_point.z-back_point.z;
246 float dist = sqrt(dx*dx+dy*dy+dz*dz);
248 float diff = tdist-dist;
249 if(diff<-margin && adjust_dir<=0)
254 else if(diff>margin && adjust_dir>=0)
262 front.advance(diff*(1-ratio));
263 back.advance(-diff*ratio);
267 TrackPoint Vehicle::get_point(const Point &front, const Point &back, float ratio) const
269 float dx = front.x-back.x;
270 float dy = front.y-back.y;
271 float dz = front.z-back.z;
274 tp.pos = Point(back.x+dx*ratio, back.y+dy*ratio, back.z+dz*ratio);
275 tp.dir = atan2(dy, dx);
280 TrackPoint Vehicle::get_point(const TrackPosition &pos, float tdist, float ratio) const
282 TrackPosition front = pos;
283 front.advance(tdist*(1-ratio));
285 TrackPosition back = pos;
286 back.advance(-tdist*ratio);
288 adjust_for_distance(front, back, tdist, ratio);
289 return get_point(front.get_point().pos, back.get_point().pos, ratio);
293 Vehicle::TrackPosition::TrackPosition():
299 Vehicle::TrackPosition::TrackPosition(Track *t, unsigned e, float o):
305 void Vehicle::TrackPosition::advance(float d)
310 unsigned path = track->get_active_path();
313 float path_len = track->get_type().get_path_length(path);
314 while(track && offs>=path_len)
316 unsigned out = track->traverse(ep, path);
317 Track *next = track->get_link(out);
321 ep = next->get_endpoint_by_link(*track);
325 path = track->get_active_path();
326 path_len = track->get_type().get_path_length(path);
332 while(track && offs<0)
334 Track *prev = track->get_link(ep);
337 unsigned in = prev->get_endpoint_by_link(*track);
340 path = track->get_active_path();
341 ep = track->traverse(in, path);
343 path_len = track->get_type().get_path_length(path);
357 TrackPoint Vehicle::TrackPosition::get_point() const
360 return track->get_point(ep, offs);
365 } // namespace Marklin