source/libr2c2/vehicle.cpp

   1 #include <cmath>
   2 #include "catalogue.h"
   3 #include "driver.h"
   4 #include "layout.h"
   5 #include "track.h"
   6 #include "trackappearance.h"
   7 #include "trackiter.h"
   8 #include "tracktype.h"
   9 #include "vehicle.h"
  10 #include "vehicletype.h"
  11
  12 using namespace std;
  13 using namespace Msp;
  14
  15 namespace R2C2 {
  16
  17 Vehicle::Vehicle(Layout &l, const VehicleType &t):
  18         Object(l),
  19         type(t),
  20         train(0),
  21         next(0),
  22         prev(0),
  23         placement(type),
  24         front_sensor(0),
  25         back_sensor(0)
  26 {
  27         axles.assign(type.get_axles().begin(), type.get_axles().end());
  28         for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
  29                 if(!i->type->bogie)
  30                         fixed_axles.push_back(&*i);
  31         bogies.assign(type.get_bogies().begin(), type.get_bogies().end());
  32         rods.assign(type.get_rods().begin(), type.get_rods().end());
  33         for(vector<Bogie>::iterator i=bogies.begin(); i!=bogies.end(); ++i)
  34                 for(unsigned j=0; j<i->axles.size(); ++j)
  35                         i->axles[j] = &axles[i->type->first_axle+j];
  36
  37         update_rods();
  38
  39         layout.add(*this);
  40 }
  41
  42 Vehicle::~Vehicle()
  43 {
  44         if(next)
  45                 detach_back();
  46         if(prev)
  47                 detach_front();
  48         layout.remove(*this);
  49 }
  50
  51 Vehicle *Vehicle::clone(Layout *to_layout) const
  52 {
  53         Vehicle *veh = new Vehicle((to_layout ? *to_layout : layout), type);
  54         veh->set_position(position);
  55         veh->set_rotation(rotation);
  56         return veh;
  57 }
  58
  59 void Vehicle::set_train(Train *t)
  60 {
  61         train = t;
  62 }
  63
  64 void Vehicle::attach_back(Vehicle &veh)
  65 {
  66         if(next || veh.prev)
  67                 throw attachment_error("already attached");
  68
  69         next = &veh;
  70         veh.prev = this;
  71
  72         if(is_placed())
  73                 propagate_backward();
  74 }
  75
  76 void Vehicle::attach_front(Vehicle &veh)
  77 {
  78         if(prev || veh.next)
  79                 throw attachment_error("already attached");
  80
  81         prev = &veh;
  82         veh.next = this;
  83
  84         if(prev->is_placed())
  85                 prev->propagate_backward();
  86 }
  87
  88 void Vehicle::detach_back()
  89 {
  90         if(!next)
  91                 throw attachment_error("not attached");
  92
  93         next->prev = 0;
  94         next = 0;
  95 }
  96
  97 void Vehicle::detach_front()
  98 {
  99         if(!prev)
 100                 throw attachment_error("not attached");
 101
 102         prev->next = 0;
 103         prev = 0;
 104 }
 105
 106 void Vehicle::place(const TrackOffsetIter &t, VehiclePlacement::Anchor a)
 107 {
 108         if(!t)
 109                 throw invalid_argument("Vehicle::place");
 110         float gauge_ratio = t->get_type().get_gauge()/type.get_gauge();
 111         if(gauge_ratio<0.99 || gauge_ratio>1.01)
 112                 throw logic_error("Incompatible gauge");
 113
 114         placement.place(t, a);
 115
 116         update_position(0);
 117         propagate_position();
 118 }
 119
 120 void Vehicle::unplace()
 121 {
 122         if(!placement.is_placed())
 123                 return;
 124
 125         placement.unplace();
 126
 127         if(prev)
 128                 prev->unplace();
 129         if(next)
 130                 next->unplace();
 131 }
 132
 133 void Vehicle::advance(float d)
 134 {
 135         placement.advance(d);
 136         turn_axles(d);
 137         update_position(d<0 ? -1 : 1);
 138         propagate_position();
 139 }
 140
 141 const Vehicle::Axle &Vehicle::get_axle(unsigned i) const
 142 {
 143         if(i>=axles.size())
 144                 throw out_of_range("Vehicle::get_axle");
 145         return axles[i];
 146 }
 147
 148 const Vehicle::Axle &Vehicle::get_fixed_axle(unsigned i) const
 149 {
 150         if(i>=fixed_axles.size())
 151                 throw out_of_range("Vehicle::get_fixed_axle");
 152         return *fixed_axles[i];
 153 }
 154
 155 const Vehicle::Bogie &Vehicle::get_bogie(unsigned i) const
 156 {
 157         if(i>=bogies.size())
 158                 throw out_of_range("Vehicle::get_bogie");
 159         return bogies[i];
 160 }
 161
 162 const Vehicle::Axle &Vehicle::get_bogie_axle(unsigned i, unsigned j) const
 163 {
 164         if(i>=bogies.size())
 165                 throw out_of_range("Vehicle::get_bogie_axle");
 166         if(j>=bogies[i].axles.size())
 167                 throw out_of_range("Vehicle::get_bogie_axle");
 168         return *bogies[i].axles[j];
 169 }
 170
 171 const Vehicle::Rod &Vehicle::get_rod(unsigned i) const
 172 {
 173         if(i>=rods.size())
 174                 throw out_of_range("Vehicle::get_rod");
 175         return rods[i];
 176 }
 177
 178 void Vehicle::update_position(int sign)
 179 {
 180         OrientedPoint p = placement.get_point();
 181         position = p.position;
 182         // TODO Move the z adjustment to VehiclePlacement
 183         position.z += placement.get_position(VehiclePlacement::FRONT_AXLE)->get_type().get_appearance().get_rail_elevation();
 184         rotation = p.rotation;
 185         tilt = p.tilt;
 186
 187         if(bogies.size()>=2)
 188         {
 189                 OrientedPoint front_point = placement.get_bogie_point(bogies.front().type->index);
 190                 bogies.front().direction = front_point.rotation-p.rotation;
 191
 192                 OrientedPoint back_point = placement.get_bogie_point(bogies.back().type->index);
 193                 bogies.back().direction = back_point.rotation-p.rotation;
 194         }
 195
 196         if(!prev)
 197                 check_sensor(placement.get_position(VehiclePlacement::FRONT_AXLE), front_sensor, sign<0);
 198         if(!next)
 199                 check_sensor(placement.get_position(VehiclePlacement::BACK_AXLE), back_sensor, sign>0);
 200
 201         signal_moved.emit();
 202 }
 203
 204 void Vehicle::update_position_from(const Vehicle &veh)
 205 {
 206         int sign = (&veh==prev ? -1 : 1);
 207
 208         float tdist = (type.get_length()+veh.type.get_length())/2;
 209         float margin = layout.get_catalogue().get_scale();
 210
 211         float dist = distance(veh.position, position);
 212         if(!is_placed() || dist<tdist-margin || dist>tdist+margin)
 213         {
 214                 if(sign<0)
 215                         placement.place_after(veh.placement);
 216                 else
 217                         placement.place_before(veh.placement);
 218                 update_position(0);
 219
 220                 dist = distance(veh.position, position);
 221         }
 222
 223         float d = sign*(tdist-dist);
 224         placement.advance(d);
 225         update_position(d<0 ? -1 : 1);
 226         turn_axles(d);
 227 }
 228
 229 void Vehicle::propagate_position()
 230 {
 231         if(prev)
 232                 propagate_forward();
 233         if(next)
 234                 propagate_backward();
 235 }
 236
 237 void Vehicle::propagate_forward()
 238 {
 239         prev->update_position_from(*this);
 240
 241         if(prev->prev)
 242                 prev->propagate_forward();
 243 }
 244
 245 void Vehicle::propagate_backward()
 246 {
 247         next->update_position_from(*this);
 248
 249         if(next->next)
 250                 next->propagate_backward();
 251 }
 252
 253 void Vehicle::check_sensor(const TrackOffsetIter &t, unsigned &sensor, bool release)
 254 {
 255         unsigned s = t->get_sensor_address();
 256         if(s!=sensor)
 257         {
 258                 unsigned old = sensor;
 259                 sensor = s;
 260                 if(release)
 261                         layout.get_driver().set_sensor(old, false);
 262                 else
 263                         layout.get_driver().set_sensor(sensor, true);
 264         }
 265 }
 266
 267 void Vehicle::turn_axles(float d)
 268 {
 269         for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
 270                 i->angle += Angle::from_radians(d*2/i->type->wheel_dia);
 271
 272         update_rods();
 273 }
 274
 275 void Vehicle::update_rods()
 276 {
 277         if(rods.empty())
 278                 return;
 279
 280         for(unsigned n=0; n<10; ++n)
 281         {
 282                 float max_d = 0;
 283                 for(vector<Rod>::iterator i=rods.begin(); i!=rods.end(); ++i)
 284                 {
 285                         const vector<VehicleType::RodConstraint> &constraints = i->type->constraints;
 286                         for(vector<VehicleType::RodConstraint>::const_iterator j=constraints.begin(); j!=constraints.end(); ++j)
 287                         {
 288                                 float d = resolve_rod_constraint(*i, *j);
 289                                 max_d = max(d, max_d);
 290                         }
 291                 }
 292
 293                 if(max_d<0.0001)
 294                         break;
 295         }
 296 }
 297
 298 float Vehicle::resolve_rod_constraint(Rod &rod, const VehicleType::RodConstraint &cns)
 299 {
 300         Vector target;
 301         if(cns.target==VehicleType::RodConstraint::BODY)
 302                 target = cns.target_position;
 303         else if(cns.target==VehicleType::RodConstraint::BOGIE)
 304                 ;  // TODO currently rods must lie in the xz plane of the body
 305         else if(cns.target==VehicleType::RodConstraint::AXLE)
 306         {
 307                 const Axle &axle = get_axle(cns.target_index);
 308                 target = Vector(axle.type->position, 0, axle.type->wheel_dia/2);
 309                 target += Transform::rotation(axle.angle, Vector(0, 1, 0)).transform(cns.target_position);
 310         }
 311         else if(cns.target==VehicleType::RodConstraint::ROD)
 312         {
 313                 const Rod &trod = get_rod(cns.target_index);
 314                 target = trod.position;
 315                 target += Transform::rotation(trod.angle, Vector(0, -1, 0)).transform(cns.target_position);
 316         }
 317
 318         Vector old_position = rod.position;
 319         if(cns.type==VehicleType::RodConstraint::MOVE)
 320                 rod.position = target-Transform::rotation(rod.angle, Vector(0, -1, 0)).transform(cns.local_position);
 321         else if(cns.type==VehicleType::RodConstraint::SLIDE)
 322         {
 323                 Vector d = rod.position-target;
 324                 rod.position = target+cns.axis*dot(d, cns.axis);
 325                 rod.angle = Angle::zero();
 326         }
 327         else if(cns.type==VehicleType::RodConstraint::ROTATE)
 328         {
 329                 Angle old_angle = rod.angle;
 330                 Vector d = target-rod.position;
 331                 rod.angle = Geometry::atan2<float>(d.z, d.x);
 332                 if(cns.local_position.x || cns.local_position.z)
 333                         rod.angle -= Geometry::atan2<float>(cns.local_position.z, cns.local_position.x);
 334                 return abs(rod.angle-old_angle).radians()*cns.local_position.norm();
 335         }
 336
 337         return distance(old_position, rod.position);
 338 }
 339
 340 unsigned Vehicle::get_n_link_slots() const
 341 {
 342         return 2;
 343 }
 344
 345 Vehicle *Vehicle::get_link(unsigned i) const
 346 {
 347         if(i>=2)
 348                 throw out_of_range("Vehicle::get_link");
 349
 350         return (i==0 ? prev : next);
 351 }
 352
 353 int Vehicle::get_link_slot(const Object &other) const
 354 {
 355         if(&other==prev)
 356                 return 0;
 357         else if(&other==next)
 358                 return 1;
 359         else
 360                 return -1;
 361 }
 362
 363 bool Vehicle::collide_ray(const Ray &ray, float *d) const
 364 {
 365         if(is_placed())
 366                 return Object::collide_ray(ray, d);
 367         else
 368                 return false;
 369 }
 370
 371
 372 Vehicle::Axle::Axle(const VehicleType::Axle &t):
 373         type(&t)
 374 { }
 375
 376
 377 Vehicle::Bogie::Bogie(const VehicleType::Bogie &t):
 378         type(&t),
 379         axles(t.axles.size())
 380 { }
 381
 382
 383 Vehicle::Rod::Rod(const VehicleType::Rod &t):
 384         type(&t),
 385         position(t.initial_position)
 386 { }
 387
 388 } // namespace R2C2