9 #include "vehicletype.h"
16 Vehicle::Vehicle(Layout &l, const VehicleType &t):
25 axles.assign(type.get_fixed_axles().begin(), type.get_fixed_axles().end());
26 bogies.assign(type.get_bogies().begin(), type.get_bogies().end());
27 rods.assign(type.get_rods().begin(), type.get_rods().end());
29 layout.add_vehicle(*this);
38 layout.remove_vehicle(*this);
41 void Vehicle::attach_back(Vehicle &veh)
44 throw attachment_error("already attached");
53 void Vehicle::attach_front(Vehicle &veh)
56 throw attachment_error("already attached");
62 prev->propagate_backward();
65 void Vehicle::detach_back()
68 throw attachment_error("not attached");
74 void Vehicle::detach_front()
77 throw attachment_error("not attached");
83 void Vehicle::place(Track &t, unsigned e, float o, PlaceMode m)
85 track_pos = TrackPosition(&t, e, o);
88 track_pos.advance(-type.get_front_axle_offset());
89 else if(m==FRONT_BUFFER)
90 track_pos.advance(-type.get_length()/2);
92 track_pos.advance(-type.get_back_axle_offset());
93 else if(m==BACK_BUFFER)
94 track_pos.advance(type.get_length()/2);
100 void Vehicle::unplace()
105 track_pos = TrackPosition();
113 void Vehicle::advance(float d)
115 track_pos.advance(d);
118 propagate_position();
121 const Vehicle::Axle &Vehicle::get_fixed_axle(unsigned i) const
124 throw out_of_range("Vehicle::get_fixed_axle");
128 const Vehicle::Bogie &Vehicle::get_bogie(unsigned i) const
131 throw out_of_range("Vehicle::get_bogie");
135 const Vehicle::Axle &Vehicle::get_bogie_axle(unsigned i, unsigned j) const
138 throw out_of_range("Vehicle::get_bogie_axle");
139 if(j>=bogies[i].axles.size())
140 throw out_of_range("Vehicle::get_bogie_axle");
141 return bogies[i].axles[j];
144 const Vehicle::Rod &Vehicle::get_rod(unsigned i) const
147 throw out_of_range("Vehicle::get_rod");
151 void Vehicle::update_position()
157 float wheelbase = axles.front().type->position-axles.back().type->position;
158 tp = get_point(track_pos, wheelbase, -axles.back().type->position/wheelbase);
160 else if(bogies.size()>=2)
162 TrackPosition front = track_pos;
163 front.advance(bogies.front().type->position);
164 TrackPosition back = track_pos;
165 back.advance(bogies.back().type->position);
166 float bogie_spacing = bogies.front().type->position-bogies.back().type->position;
167 adjust_for_distance(front, back, bogie_spacing);
169 const vector<Axle> &front_axles = bogies.front().axles;
170 float wheelbase = front_axles.front().type->position-front_axles.back().type->position;
171 TrackPoint front_point = get_point(front, wheelbase, -front_axles.back().type->position/wheelbase);
173 const vector<Axle> &back_axles = bogies.back().axles;
174 wheelbase = back_axles.front().type->position-back_axles.back().type->position;
175 TrackPoint back_point = get_point(back, wheelbase, -back_axles.back().type->position/wheelbase);
177 tp = get_point(front_point.pos, back_point.pos, -bogies.back().type->position/bogie_spacing);
179 bogies.front().direction = front_point.dir-tp.dir;
180 bogies.back().direction = back_point.dir-tp.dir;
183 tp = track_pos.get_point();
186 check_sensor(type.get_front_axle_offset(), front_sensor);
188 check_sensor(type.get_back_axle_offset(), back_sensor);
191 position.z += layout.get_catalogue().get_rail_elevation();
195 void Vehicle::update_position_from(const Vehicle &veh)
197 int sign = (&veh==prev ? -1 : 1);
199 float tdist = (type.get_length()+veh.type.get_length())/2;
200 float margin = layout.get_catalogue().get_scale();
202 float dist = distance(veh.position, position);
203 if(!track_pos.track || dist<tdist-margin || dist>tdist+margin)
205 track_pos = veh.track_pos;
206 track_pos.advance(sign*tdist);
209 dist = distance(veh.position, position);
212 track_pos.advance(sign*(tdist-dist));
214 turn_axles(sign*(tdist-dist));
217 void Vehicle::propagate_position()
222 propagate_backward();
225 void Vehicle::propagate_forward()
227 prev->update_position_from(*this);
230 prev->propagate_forward();
233 void Vehicle::propagate_backward()
235 next->update_position_from(*this);
238 next->propagate_backward();
241 void Vehicle::check_sensor(float offset, unsigned &sensor)
243 TrackPosition pos = track_pos;
245 unsigned s = pos.track->get_sensor_id();
248 /* Sensor ID under axle has changed. Deduce movement direction by using
249 the sensor ID under the midpoint of the vehicle. */
250 /* XXX This depends on the simulation running fast enough. Something
251 more robust would be preferable. */
252 unsigned old = sensor;
254 unsigned mid = track_pos.track->get_sensor_id();
257 /* There's a sensor and it's different from mid. We've just entered
259 layout.get_driver().set_sensor(sensor, true);
261 /* A sensor was under the axle and it was different from mid. We've
262 just left that sensor. */
263 layout.get_driver().set_sensor(old, false);
267 void Vehicle::turn_axles(float d)
269 for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
270 i->angle += d*2/i->type->wheel_dia;
271 for(vector<Bogie>::iterator i=bogies.begin(); i!=bogies.end(); ++i)
272 for(vector<Axle>::iterator j=i->axles.begin(); j!=i->axles.end(); ++j)
273 j->angle += d*2/j->type->wheel_dia;
278 void Vehicle::update_rods()
280 for(vector<Rod>::iterator i=rods.begin(); i!=rods.end(); ++i)
282 if(i->type->pivot==VehicleType::Rod::BODY)
283 i->position = i->type->pivot_point;
284 else if(i->type->pivot==VehicleType::Rod::AXLE)
286 const Axle &axle = get_fixed_axle(i->type->pivot_index);
287 float c = cos(axle.angle);
288 float s = sin(axle.angle);
289 const Vector &pp = i->type->pivot_point;
290 i->position = Vector(axle.type->position+pp.x*c+pp.z*s, pp.y, axle.type->wheel_dia/2+pp.z*c-pp.x*s);
292 else if(i->type->pivot==VehicleType::Rod::ROD)
294 const Rod &prod = get_rod(i->type->pivot_index);
295 float c = cos(prod.angle);
296 float s = sin(prod.angle);
297 const Vector &pos = prod.position;
298 const Vector &off = i->type->pivot_point;
299 i->position = Vector(pos.x+off.x*c-off.z*s, pos.y+off.y, pos.z+off.z*c+off.x*s);
302 if(i->type->connect_index>=0)
304 Rod &crod = rods[i->type->connect_index];
305 if(i->type->limit==VehicleType::Rod::ROTATE && crod.type->limit==VehicleType::Rod::SLIDE_X)
307 float dx = (crod.position.x+i->type->connect_offset.x)-i->position.x;
308 float dz = (crod.position.z+i->type->connect_offset.z)-i->position.z;
309 float cd = sqrt(i->type->connect_point.x*i->type->connect_point.x+i->type->connect_point.z*i->type->connect_point.z);
310 float ca = atan2(i->type->connect_point.z, i->type->connect_point.x);
311 dx = sqrt(cd*cd-dz*dz)*(dx>0 ? 1 : -1);
312 i->angle = atan2(dz, dx)-ca;
313 crod.position.x = i->position.x+dx-i->type->connect_offset.x;
315 else if(i->type->limit==VehicleType::Rod::ROTATE && crod.type->limit==VehicleType::Rod::ROTATE)
317 float dx = crod.position.x-i->position.x;
318 float dz = crod.position.z-i->position.z;
319 float d = sqrt(dx*dx+dz*dz);
320 float cd1 = sqrt(i->type->connect_point.x*i->type->connect_point.x+i->type->connect_point.z*i->type->connect_point.z);
321 float cd2 = sqrt(i->type->connect_offset.x*i->type->connect_offset.x+i->type->connect_offset.z*i->type->connect_offset.z);
322 float a = (d*d+cd1*cd1-cd2*cd2)/(2*d);
323 float b = sqrt(cd1*cd1-a*a);
324 float sign = (dx*i->type->connect_point.z-dz*i->type->connect_point.x>0 ? 1 : -1);
325 float cx = (dx*a-dz*b*sign)/d;
326 float cz = (dz*a+dx*b*sign)/d;
327 float ca1 = atan2(i->type->connect_point.z, i->type->connect_point.x);
328 float ca2 = atan2(i->type->connect_offset.z, i->type->connect_offset.x);
329 i->angle = atan2(cz, cx)-ca1;
330 crod.angle = atan2(cz-dz, cx-dx)-ca2;
336 void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, float tdist, float ratio) const
338 float margin = 0.01*layout.get_catalogue().get_scale();
342 Vector front_point = front.get_point().pos;
343 Vector back_point = back.get_point().pos;
345 float dx = front_point.x-back_point.x;
346 float dy = front_point.y-back_point.y;
347 float dz = front_point.z-back_point.z;
348 float dist = sqrt(dx*dx+dy*dy+dz*dz);
350 float diff = tdist-dist;
351 if(diff<-margin && adjust_dir<=0)
356 else if(diff>margin && adjust_dir>=0)
364 front.advance(diff*(1-ratio));
365 back.advance(-diff*ratio);
369 TrackPoint Vehicle::get_point(const Vector &front, const Vector &back, float ratio) const
371 float dx = front.x-back.x;
372 float dy = front.y-back.y;
373 float dz = front.z-back.z;
376 tp.pos = Vector(back.x+dx*ratio, back.y+dy*ratio, back.z+dz*ratio);
377 tp.dir = atan2(dy, dx);
382 TrackPoint Vehicle::get_point(const TrackPosition &pos, float tdist, float ratio) const
384 TrackPosition front = pos;
385 front.advance(tdist*(1-ratio));
387 TrackPosition back = pos;
388 back.advance(-tdist*ratio);
390 adjust_for_distance(front, back, tdist, ratio);
391 return get_point(front.get_point().pos, back.get_point().pos, ratio);
395 Vehicle::Axle::Axle(const VehicleType::Axle &t):
401 Vehicle::Bogie::Bogie(const VehicleType::Bogie &t):
405 for(VehicleType::AxleArray::const_iterator i=type->axles.begin(); i!=type->axles.end(); ++i)
410 Vehicle::Rod::Rod(const VehicleType::Rod &t):
416 Vehicle::TrackPosition::TrackPosition():
422 Vehicle::TrackPosition::TrackPosition(Track *t, unsigned e, float o):
428 void Vehicle::TrackPosition::advance(float d)
434 TrackIter iter(track, ep);
437 float path_len = iter->get_type().get_path_length(iter->get_active_path());
448 while(iter && offs<0)
450 iter = iter.flip().reverse();
454 float path_len = iter->get_type().get_path_length(iter->get_active_path());
459 track = iter.track();
465 TrackPoint Vehicle::TrackPosition::get_point() const
468 return track->get_point(ep, offs);