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