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