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