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
  18 namespace Marklin {
  19
  20 Vehicle::Vehicle(Layout &l, const VehicleType &t):
  21         layout(l),
  22         type(t),
  23         next(0),
  24         prev(0),
  25         direction(0)
  26 {
  27         layout.add_vehicle(*this);
  28 }
  29
  30 Vehicle::~Vehicle()
  31 {
  32         layout.remove_vehicle(*this);
  33 }
  34
  35 void Vehicle::place(Track *t, unsigned e, float o, PlaceMode m)
  36 {
  37         track_pos = TrackPosition(t, e, o);
  38
  39         if(m==FRONT_AXLE)
  40         {
  41                 float front = type.get_length()/2;
  42                 if(!type.get_axles().empty())
  43                         front = type.get_axles().front().position;
  44                 if(!type.get_bogies().empty())
  45                 {
  46                         const VehicleType::Bogie &bogie = type.get_bogies().front();
  47                         front = max(front, bogie.position+bogie.axles.front().position);
  48                 }
  49                 track_pos.advance(-front);
  50         }
  51         else if(m==FRONT_BUFFER)
  52                 track_pos.advance(-type.get_length()/2);
  53         else if(m==BACK_AXLE)
  54         {
  55                 float back = type.get_length()/2;
  56                 if(!type.get_axles().empty())
  57                         back = type.get_axles().back().position;
  58                 if(!type.get_bogies().empty())
  59                 {
  60                         const VehicleType::Bogie &bogie = type.get_bogies().back();
  61                         back = min(back, bogie.position+bogie.axles.back().position);
  62                 }
  63                 track_pos.advance(-back);
  64         }
  65         else if(m==BACK_BUFFER)
  66                 track_pos.advance(type.get_length()/2);
  67
  68         update_position();
  69 }
  70
  71 void Vehicle::advance(float d)
  72 {
  73         track_pos.advance(d);
  74         update_position();
  75 }
  76
  77 void Vehicle::update_position()
  78 {
  79         TrackPoint tp;
  80
  81         const vector<VehicleType::Axle> &axles = type.get_axles();
  82         if(axles.size()>=2)
  83                 tp = get_position(axles.front().position, axles.back().position, track_pos);
  84         else
  85         {
  86                 const vector<VehicleType::Bogie> &bogies = type.get_bogies();
  87                 if(bogies.size()>=2)
  88                         // XXX Calculate bogie positions correctly
  89                         tp = get_position(bogies.front().position, bogies.back().position, track_pos);
  90                 else
  91                         tp = track_pos.get_point();
  92         }
  93         position = tp.pos;
  94         direction = tp.dir;
  95 }
  96
  97 TrackPoint Vehicle::get_position(float front, float back, const TrackPosition &tpos)
  98 {
  99         TrackPosition front_pos = tpos;
 100         front_pos.advance(front);
 101
 102         TrackPosition back_pos = tpos;
 103         back_pos.advance(back);
 104
 105         float target_dist = front-back;
 106
 107         while(1)
 108         {
 109                 Point front_point = front_pos.get_point().pos;
 110                 Point back_point = back_pos.get_point().pos;
 111
 112                 float dx = front_point.x-back_point.x;
 113                 float dy = front_point.y-back_point.y;
 114                 float dz = front_point.z-back_point.z;
 115                 float dist = sqrt(dx*dx+dy*dy+dz*dz);
 116
 117                 if(dist<target_dist)
 118                 {
 119                         float adjust = target_dist-dist+0.01*layout.get_catalogue().get_scale();
 120                         front_pos.advance(adjust/2);
 121                         back_pos.advance(-adjust/2);
 122                 }
 123                 else
 124                 {
 125                         float f = -back/target_dist;
 126                         TrackPoint pt;
 127                         pt.pos = Point(back_point.x+dx*f, back_point.y+dy*f, back_point.z+dz*f);
 128                         pt.dir = atan2(dy, dx);
 129                         return pt;
 130                 }
 131         }
 132 }
 133
 134
 135 Vehicle::TrackPosition::TrackPosition():
 136         track(0),
 137         ep(0),
 138         offs(0)
 139 { }
 140
 141 Vehicle::TrackPosition::TrackPosition(Track *t, unsigned e, float o):
 142         track(t),
 143         ep(e),
 144         offs(o)
 145 { }
 146
 147 void Vehicle::TrackPosition::advance(float d)
 148 {
 149         if(!track)
 150                 return;
 151
 152         unsigned path = track->get_active_path();
 153
 154         offs += d;
 155         float path_len = track->get_type().get_path_length(path);
 156         while(track && offs>=path_len)
 157         {
 158                 unsigned out = track->traverse(ep, path);
 159                 Track *next = track->get_link(out);
 160
 161                 if(next)
 162                 {
 163                         ep = next->get_endpoint_by_link(*track);
 164                         track = next;
 165
 166                         offs -= path_len;
 167                         path = track->get_active_path();
 168                         path_len = track->get_type().get_path_length(path);
 169                 }
 170                 else
 171                         track = 0;
 172         }
 173
 174         while(track && offs<0)
 175         {
 176                 Track *prev = track->get_link(ep);
 177                 if(prev)
 178                 {
 179                         unsigned in = prev->get_endpoint_by_link(*track);
 180                         track = prev;
 181
 182                         path = track->get_active_path();
 183                         ep = track->traverse(in, path);
 184
 185                         path_len = track->get_type().get_path_length(path);
 186                         offs += path_len;
 187                 }
 188                 else
 189                         track = 0;
 190         }
 191
 192         if(!track)
 193         {
 194                 ep = 0;
 195                 offs = 0;
 196         }
 197 }
 198
 199 TrackPoint Vehicle::TrackPosition::get_point() const
 200 {
 201         if(track)
 202                 return track->get_point(ep, offs);
 203         else
 204                 return TrackPoint();
 205 }
 206
 207 } // namespace Marklin