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