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