source/libr2c2/vehicle.cpp

   1 /* $Id$
   2
   3 This file is part of R²C²
   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 R2C2 {
  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         axle_angles(1),
  31         rods(type.get_rods().size()),
  32         front_sensor(0),
  33         back_sensor(0)
  34 {
  35         layout.add_vehicle(*this);
  36
  37         axle_angles.front().resize(type.get_axles().size(), 0.0f);
  38         const vector<VehicleType::Bogie> &bogies = type.get_bogies();
  39         for(vector<VehicleType::Bogie>::const_iterator i=bogies.begin(); i!=bogies.end(); ++i)
  40                 axle_angles.push_back(vector<float>(i->axles.size(), 0.0f));
  41 }
  42
  43 Vehicle::~Vehicle()
  44 {
  45         if(next)
  46                 detach_back();
  47         if(prev)
  48                 detach_front();
  49         layout.remove_vehicle(*this);
  50 }
  51
  52 void Vehicle::attach_back(Vehicle &veh)
  53 {
  54         if(next || veh.prev)
  55                 throw InvalidState("Already attached");
  56
  57         next = &veh;
  58         veh.prev = this;
  59
  60         if(track_pos.track)
  61                 propagate_backward();
  62 }
  63
  64 void Vehicle::attach_front(Vehicle &veh)
  65 {
  66         if(prev || veh.next)
  67                 throw InvalidState("Already attached");
  68
  69         prev = &veh;
  70         veh.next = this;
  71
  72         if(prev->get_track())
  73                 prev->propagate_backward();
  74 }
  75
  76 void Vehicle::detach_back()
  77 {
  78         if(!next)
  79                 throw InvalidState("Not attached");
  80
  81         next->prev = 0;
  82         next = 0;
  83 }
  84
  85 void Vehicle::detach_front()
  86 {
  87         if(!prev)
  88                 throw InvalidState("Not attached");
  89
  90         prev->next = 0;
  91         prev = 0;
  92 }
  93
  94 void Vehicle::place(Track &t, unsigned e, float o, PlaceMode m)
  95 {
  96         track_pos = TrackPosition(&t, e, o);
  97
  98         if(m==FRONT_AXLE)
  99                 track_pos.advance(-type.get_front_axle_offset());
 100         else if(m==FRONT_BUFFER)
 101                 track_pos.advance(-type.get_length()/2);
 102         else if(m==BACK_AXLE)
 103                 track_pos.advance(-type.get_back_axle_offset());
 104         else if(m==BACK_BUFFER)
 105                 track_pos.advance(type.get_length()/2);
 106
 107         update_position();
 108         propagate_position();
 109 }
 110
 111 void Vehicle::unplace()
 112 {
 113         if(!track_pos.track)
 114                 return;
 115
 116         track_pos = TrackPosition();
 117
 118         if(prev)
 119                 prev->unplace();
 120         if(next)
 121                 next->unplace();
 122 }
 123
 124 void Vehicle::advance(float d)
 125 {
 126         track_pos.advance(d);
 127         update_position();
 128         turn_axles(d);
 129         propagate_position();
 130 }
 131
 132 float Vehicle::get_bogie_direction(unsigned i) const
 133 {
 134         if(i>=bogie_dirs.size())
 135                 throw InvalidParameterValue("Bogie index out of range");
 136         return bogie_dirs[i];
 137 }
 138
 139 float Vehicle::get_axle_angle(unsigned i) const
 140 {
 141         if(i>=axle_angles[0].size())
 142                 throw InvalidParameterValue("Axle index out of range");
 143         return axle_angles[0][i];
 144 }
 145
 146 float Vehicle::get_bogie_axle_angle(unsigned i, unsigned j) const
 147 {
 148         if(i+1>=axle_angles.size())
 149                 throw InvalidParameterValue("Bogie index out of range");
 150         if(j>=axle_angles[i+1].size())
 151                 throw InvalidParameterValue("Axle index out of range");
 152         return axle_angles[i+1][j];
 153 }
 154
 155 const Point &Vehicle::get_rod_position(unsigned i) const
 156 {
 157         if(i>=rods.size())
 158                 throw InvalidParameterValue("Rod index out of range");
 159         return rods[i].position;
 160 }
 161
 162 float Vehicle::get_rod_angle(unsigned i) const
 163 {
 164         if(i>=rods.size())
 165                 throw InvalidParameterValue("Rod index out of range");
 166         return rods[i].angle;
 167 }
 168
 169 void Vehicle::update_position()
 170 {
 171         TrackPoint tp;
 172
 173         const vector<VehicleType::Axle> &axles = type.get_axles();
 174         const vector<VehicleType::Bogie> &bogies = type.get_bogies();
 175         if(axles.size()>=2)
 176         {
 177                 float wheelbase = axles.front().position-axles.back().position;
 178                 tp = get_point(track_pos, wheelbase, -axles.back().position/wheelbase);
 179         }
 180         else if(bogies.size()>=2)
 181         {
 182                 TrackPosition front = track_pos;
 183                 front.advance(bogies.front().position);
 184                 TrackPosition back = track_pos;
 185                 back.advance(bogies.back().position);
 186                 float bogie_spacing = bogies.front().position-bogies.back().position;
 187                 adjust_for_distance(front, back, bogie_spacing);
 188
 189                 const vector<VehicleType::Axle> &front_axles = bogies.front().axles;
 190                 float wheelbase = front_axles.front().position-front_axles.back().position;
 191                 TrackPoint front_point = get_point(front, wheelbase, -front_axles.back().position/wheelbase);
 192
 193                 const vector<VehicleType::Axle> &back_axles = bogies.back().axles;
 194                 wheelbase = back_axles.front().position-back_axles.back().position;
 195                 TrackPoint back_point = get_point(back, wheelbase, -back_axles.back().position/wheelbase);
 196
 197                 tp = get_point(front_point.pos, back_point.pos, -bogies.back().position/bogie_spacing);
 198
 199                 bogie_dirs.front() = front_point.dir-tp.dir;
 200                 bogie_dirs.back() = back_point.dir-tp.dir;
 201         }
 202         else
 203                 tp = track_pos.get_point();
 204
 205         if(!prev)
 206                 check_sensor(type.get_front_axle_offset(), front_sensor);
 207         if(!next)
 208                 check_sensor(type.get_back_axle_offset(), back_sensor);
 209
 210         position = tp.pos;
 211         position.z += layout.get_catalogue().get_rail_elevation();
 212         direction = tp.dir;
 213 }
 214
 215 void Vehicle::update_position_from(const Vehicle &veh)
 216 {
 217         int sign = (&veh==prev ? -1 : 1);
 218
 219         float tdist = (type.get_length()+veh.type.get_length())/2;
 220         float margin = layout.get_catalogue().get_scale();
 221
 222         float dist = distance(veh.position, position);
 223         if(dist<tdist-margin || dist>tdist+margin)
 224         {
 225                 track_pos = veh.track_pos;
 226                 track_pos.advance(sign*tdist);
 227                 update_position();
 228
 229                 dist = distance(veh.position, position);
 230         }
 231
 232         track_pos.advance(sign*(tdist-dist));
 233         update_position();
 234         turn_axles(sign*(tdist-dist));
 235 }
 236
 237 void Vehicle::propagate_position()
 238 {
 239         if(prev)
 240                 propagate_forward();
 241         if(next)
 242                 propagate_backward();
 243 }
 244
 245 void Vehicle::propagate_forward()
 246 {
 247         prev->update_position_from(*this);
 248
 249         if(prev->prev)
 250                 prev->propagate_forward();
 251 }
 252
 253 void Vehicle::propagate_backward()
 254 {
 255         next->update_position_from(*this);
 256
 257         if(next->next)
 258                 next->propagate_backward();
 259 }
 260
 261 void Vehicle::check_sensor(float offset, unsigned &sensor)
 262 {
 263         TrackPosition pos = track_pos;
 264         pos.advance(offset);
 265         unsigned s = pos.track->get_sensor_id();
 266         if(s!=sensor)
 267         {
 268                 /* Sensor ID under axle has changed.  Deduce movement direction by using
 269                 the sensor ID under the midpoint of the vehicle. */
 270                 /* XXX This depends on the simulation running fast enough.  Something
 271                 more robust would be preferable. */
 272                 unsigned old = sensor;
 273                 sensor = s;
 274                 unsigned mid = track_pos.track->get_sensor_id();
 275
 276                 if(s && s!=mid)
 277                         /* There's a sensor and it's different from mid.  We've just entered
 278                         that sensor. */
 279                         layout.get_driver().set_sensor(sensor, true);
 280                 if(old && old!=mid)
 281                         /* A sensor was under the axle and it was different from mid.  We've
 282                         just left that sensor. */
 283                         layout.get_driver().set_sensor(old, false);
 284         }
 285 }
 286
 287 void Vehicle::turn_axles(float d)
 288 {
 289         const vector<VehicleType::Axle> &axles = type.get_axles();
 290         const vector<VehicleType::Bogie> &bogies = type.get_bogies();
 291         for(unsigned i=0; i<axle_angles.size(); ++i)
 292                 for(unsigned j=0; j<axle_angles[i].size(); ++j)
 293                 {
 294                         const VehicleType::Axle &axle = (i==0 ? axles[j] : bogies[i-1].axles[j]);
 295                         axle_angles[i][j] += d*2/axle.wheel_dia;
 296                 }
 297
 298         update_rods();
 299 }
 300
 301 void Vehicle::update_rods()
 302 {
 303         const vector<VehicleType::Rod> &trods = type.get_rods();
 304         for(unsigned i=0; i<trods.size(); ++i)
 305         {
 306                 const VehicleType::Rod &rod = trods[i];
 307                 if(rod.pivot==VehicleType::Rod::BODY)
 308                         rods[i].position = rod.pivot_point;
 309                 else if(rod.pivot==VehicleType::Rod::AXLE)
 310                 {
 311                         const VehicleType::Axle &axle = type.get_axles()[rod.pivot_index];
 312                         float angle = axle_angles[0][rod.pivot_index];
 313                         float c = cos(angle);
 314                         float s = sin(angle);
 315                         const Point &pp = rod.pivot_point;
 316                         rods[i].position = Point(axle.position+pp.x*c+pp.z*s, pp.y, axle.wheel_dia/2+pp.z*c-pp.x*s);
 317                 }
 318                 else if(rod.pivot==VehicleType::Rod::ROD)
 319                 {
 320                         float angle = rods[rod.pivot_index].angle;
 321                         float c = cos(angle);
 322                         float s = sin(angle);
 323                         const Point &pos = rods[rod.pivot_index].position;
 324                         const Point &off = rod.pivot_point;
 325                         rods[i].position = Point(pos.x+off.x*c-off.z*s, pos.y+off.y, pos.z+off.z*c+off.x*s);
 326                 }
 327
 328                 if(rod.connect_index>=0)
 329                 {
 330                         const VehicleType::Rod &crod = trods[rod.connect_index];
 331                         if(rod.limit==VehicleType::Rod::ROTATE && crod.limit==VehicleType::Rod::SLIDE_X)
 332                         {
 333                                 float dx = (rods[rod.connect_index].position.x+rod.connect_offset.x)-rods[i].position.x;
 334                                 float dz = (rods[rod.connect_index].position.z+rod.connect_offset.z)-rods[i].position.z;
 335                                 float cd = sqrt(rod.connect_point.x*rod.connect_point.x+rod.connect_point.z*rod.connect_point.z);
 336                                 float ca = atan2(rod.connect_point.z, rod.connect_point.x);
 337                                 dx = sqrt(cd*cd-dz*dz)*(dx>0 ? 1 : -1);
 338                                 rods[i].angle = atan2(dz, dx)-ca;
 339                                 rods[rod.connect_index].position.x = rods[i].position.x+dx-rod.connect_offset.x;
 340                         }
 341                         else if(rod.limit==VehicleType::Rod::ROTATE && crod.limit==VehicleType::Rod::ROTATE)
 342                         {
 343                                 float dx = rods[rod.connect_index].position.x-rods[i].position.x;
 344                                 float dz = rods[rod.connect_index].position.z-rods[i].position.z;
 345                                 float d = sqrt(dx*dx+dz*dz);
 346                                 float cd1 = sqrt(rod.connect_point.x*rod.connect_point.x+rod.connect_point.z*rod.connect_point.z);
 347                                 float cd2 = sqrt(rod.connect_offset.x*rod.connect_offset.x+rod.connect_offset.z*rod.connect_offset.z);
 348                                 float a = (d*d+cd1*cd1-cd2*cd2)/(2*d);
 349                                 float b = sqrt(cd1*cd1-a*a);
 350                                 float sign = (dx*rod.connect_point.z-dz*rod.connect_point.x>0 ? 1 : -1);
 351                                 float cx = (dx*a-dz*b*sign)/d;
 352                                 float cz = (dz*a+dx*b*sign)/d;
 353                                 float ca1 = atan2(rod.connect_point.z, rod.connect_point.x);
 354                                 float ca2 = atan2(rod.connect_offset.z, rod.connect_offset.x);
 355                                 rods[i].angle = atan2(cz, cx)-ca1;
 356                                 rods[rod.connect_index].angle = atan2(cz-dz, cx-dx)-ca2;
 357                         }
 358                 }
 359         }
 360 }
 361
 362 void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, float tdist, float ratio) const
 363 {
 364         float margin = 0.01*layout.get_catalogue().get_scale();
 365         int adjust_dir = 0;
 366         while(1)
 367         {
 368                 Point front_point = front.get_point().pos;
 369                 Point back_point = back.get_point().pos;
 370
 371                 float dx = front_point.x-back_point.x;
 372                 float dy = front_point.y-back_point.y;
 373                 float dz = front_point.z-back_point.z;
 374                 float dist = sqrt(dx*dx+dy*dy+dz*dz);
 375
 376                 float diff = tdist-dist;
 377                 if(diff<-margin && adjust_dir<=0)
 378                 {
 379                         diff -= margin;
 380                         adjust_dir = -1;
 381                 }
 382                 else if(diff>margin && adjust_dir>=0)
 383                 {
 384                         diff += margin;
 385                         adjust_dir = 1;
 386                 }
 387                 else
 388                         return;
 389
 390                 front.advance(diff*(1-ratio));
 391                 back.advance(-diff*ratio);
 392         }
 393 }
 394
 395 TrackPoint Vehicle::get_point(const Point &front, const Point &back, float ratio) const
 396 {
 397         float dx = front.x-back.x;
 398         float dy = front.y-back.y;
 399         float dz = front.z-back.z;
 400
 401         TrackPoint tp;
 402         tp.pos = Point(back.x+dx*ratio, back.y+dy*ratio, back.z+dz*ratio);
 403         tp.dir = atan2(dy, dx);
 404
 405         return tp;
 406 }
 407
 408 TrackPoint Vehicle::get_point(const TrackPosition &pos, float tdist, float ratio) const
 409 {
 410         TrackPosition front = pos;
 411         front.advance(tdist*(1-ratio));
 412
 413         TrackPosition back = pos;
 414         back.advance(-tdist*ratio);
 415
 416         adjust_for_distance(front, back, tdist, ratio);
 417         return get_point(front.get_point().pos, back.get_point().pos, ratio);
 418 }
 419
 420
 421 Vehicle::TrackPosition::TrackPosition():
 422         track(0),
 423         ep(0),
 424         offs(0)
 425 { }
 426
 427 Vehicle::TrackPosition::TrackPosition(Track *t, unsigned e, float o):
 428         track(t),
 429         ep(e),
 430         offs(o)
 431 { }
 432
 433 void Vehicle::TrackPosition::advance(float d)
 434 {
 435         if(!track)
 436                 return;
 437
 438         offs += d;
 439         TrackIter iter(track, ep);
 440         while(iter)
 441         {
 442                 float path_len = iter->get_type().get_path_length(iter->get_active_path());
 443
 444                 if(offs>path_len)
 445                 {
 446                         offs -= path_len;
 447                         iter = iter.next();
 448                 }
 449                 else
 450                         break;
 451         }
 452
 453         while(iter && offs<0)
 454         {
 455                 iter = iter.flip().reverse();
 456
 457                 if(iter)
 458                 {
 459                         float path_len = iter->get_type().get_path_length(iter->get_active_path());
 460                         offs += path_len;
 461                 }
 462         }
 463
 464         track = iter.track();
 465         ep = iter.entry();
 466         if(!track)
 467                 offs = 0;
 468 }
 469
 470 TrackPoint Vehicle::TrackPosition::get_point() const
 471 {
 472         if(track)
 473                 return track->get_point(ep, offs);
 474         else
 475                 return TrackPoint();
 476 }
 477
 478
 479 Vehicle::Rod::Rod():
 480         angle(0)
 481 { }
 482
 483 } // namespace R2C2