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