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