source/libmarklin/track.cpp

   1 #include <cmath>
   2 #include "track.h"
   3 #include "tracktype.h"
   4
   5 using namespace std;
   6 using namespace Msp;
   7
   8 #include <iostream>
   9
  10 namespace Marklin {
  11
  12 Track::Track(const TrackType &t):
  13         type(t),
  14         rot(0),
  15         slope(0),
  16         flex(false),
  17         turnout_id(0),
  18         sensor_id(0),
  19         links(t.get_endpoints().size())
  20 { }
  21
  22 Track::~Track()
  23 {
  24         break_links();
  25 }
  26
  27 void Track::set_position(const Point &p)
  28 {
  29         pos=p;
  30 }
  31
  32 void Track::set_rotation(float r)
  33 {
  34         rot=r;
  35         while(rot<0)
  36                 rot+=M_PI*2;
  37         while(rot>M_PI*2)
  38                 rot-=M_PI*2;
  39 }
  40
  41 void Track::set_slope(float s)
  42 {
  43         if(links.size()!=2) return;
  44
  45         slope=s;
  46 }
  47
  48 void Track::set_flex(bool f)
  49 {
  50         flex=f;
  51 }
  52
  53 void Track::set_turnout_id(unsigned i)
  54 {
  55         turnout_id=i;
  56 }
  57
  58 void Track::set_sensor_id(unsigned i)
  59 {
  60         sensor_id=i;
  61 }
  62
  63 int Track::get_endpoint_by_link(const Track &other) const
  64 {
  65         for(unsigned i=0; i<links.size(); ++i)
  66                 if(links[i]==&other)
  67                         return i;
  68
  69         return -1;
  70 }
  71
  72 Point Track::get_endpoint_position(unsigned epi) const
  73 {
  74         const vector<Endpoint> &eps=type.get_endpoints();
  75         if(epi>=eps.size())
  76                 throw InvalidParameterValue("Endpoint index out of range");
  77
  78         const Endpoint &ep=eps[epi];
  79
  80         float c=cos(rot);
  81         float s=sin(rot);
  82
  83         Point p(pos.x+c*ep.x-s*ep.y, pos.y+s*ep.x+c*ep.y, pos.z);
  84         if(eps.size()==2 && epi==1)
  85                 p.z+=slope;
  86         return p;
  87 }
  88
  89 float Track::get_endpoint_direction(unsigned epi) const
  90 {
  91         const vector<Endpoint> &eps=type.get_endpoints();
  92         if(epi>=eps.size())
  93                 throw InvalidParameterValue("Endpoint index out of range");
  94
  95         const Endpoint &ep=eps[epi];
  96
  97         return rot+ep.dir;
  98 }
  99
 100 bool Track::snap_to(Track &other, bool link)
 101 {
 102         float limit=(link && !flex) ? 1e-6 : 1e-4;
 103         const vector<Endpoint> &eps=type.get_endpoints();
 104         const vector<Endpoint> &other_eps=other.get_type().get_endpoints();
 105
 106         for(unsigned i=0; i<eps.size(); ++i)
 107         {
 108                 Point epp=get_endpoint_position(i);
 109
 110                 for(unsigned j=0; j<other_eps.size(); ++j)
 111                 {
 112                         if(other.get_link(j))
 113                                 continue;
 114
 115                         Point epp2=other.get_endpoint_position(j);
 116                         float dx=epp2.x-epp.x;
 117                         float dy=epp2.y-epp.y;
 118                         if(dx*dx+dy*dy<limit)
 119                         {
 120                                 set_rotation(other.rot+other_eps[j].dir-eps[i].dir+M_PI);
 121                                 set_position(Point(epp2.x-(eps[i].x*cos(rot)-eps[i].y*sin(rot)), epp2.y-(eps[i].y*cos(rot)+eps[i].x*sin(rot)), epp2.z));
 122
 123                                 if(link)
 124                                 {
 125                                         if(links[i])
 126                                                 break_link(*links[i]);
 127                                         links[i]=&other;
 128                                         other.links[j]=this;
 129                                 }
 130
 131                                 return true;
 132                         }
 133                 }
 134         }
 135
 136         return false;
 137 }
 138
 139 bool Track::snap(Point &pt, float &d) const
 140 {
 141         const vector<Endpoint> &eps=type.get_endpoints();
 142
 143         for(unsigned i=0; i<eps.size(); ++i)
 144         {
 145                 Point epp=get_endpoint_position(i);
 146                 float dx=pt.x-epp.x;
 147                 float dy=pt.y-epp.y;
 148                 if(dx*dx+dy*dy<1e-4)
 149                 {
 150                         pt=epp;
 151                         d=rot+eps[i].dir;
 152                         return true;
 153                 }
 154         }
 155
 156         return false;
 157 }
 158
 159 void Track::break_link(Track &trk)
 160 {
 161         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 162                 if(*i==&trk)
 163                 {
 164                         *i=0;
 165                         trk.break_link(*this);
 166                         return;
 167                 }
 168 }
 169
 170 void Track::break_links()
 171 {
 172         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 173                 if(Track *trk=*i)
 174                 {
 175                         *i=0;
 176                         trk->break_link(*this);
 177                 }
 178 }
 179
 180 Track *Track::get_link(unsigned i) const
 181 {
 182         if(i>links.size())
 183                 throw InvalidParameterValue("Link index out of range");
 184
 185         return links[i];
 186 }
 187
 188 void Track::check_slope()
 189 {
 190         if(links.size()!=2)
 191                 return;
 192
 193         if(links[0] && links[1])
 194         {
 195                 Point epp0=links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
 196                 Point epp1=links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
 197                 pos.z=epp0.z;
 198                 slope=epp1.z-pos.z;
 199         }
 200         else
 201         {
 202                 slope=0;
 203                 if(links[0])
 204                 {
 205                         Point epp=links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
 206                         pos.z=epp.z;
 207                 }
 208                 else if(links[1])
 209                 {
 210                         Point epp=links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
 211                         pos.z=epp.z;
 212                 }
 213         }
 214 }
 215
 216 int Track::traverse(unsigned i, unsigned route) const
 217 {
 218         const vector<Endpoint> &eps=type.get_endpoints();
 219         if(i>=eps.size())
 220                 throw InvalidParameterValue("Endpoint index out of range");
 221
 222         const Endpoint &ep=eps[i];
 223
 224         if(ep.routes&(1<<route))
 225         {
 226                 // Find the other endpoint for this route
 227                 for(unsigned j=0; j<eps.size(); ++j)
 228                         if((eps[j].routes&(1<<route)) && j!=i)
 229                                 return j;
 230         }
 231         else
 232         {
 233                 // Find an endpoint that's connected to this one and has the requested route
 234                 for(unsigned j=0; j<eps.size(); ++j)
 235                         if((eps[j].routes&(1<<route)) && (eps[j].routes&ep.routes))
 236                                 return j;
 237         }
 238
 239         return -1;
 240 }
 241
 242 Track *Track::copy() const
 243 {
 244         Track *trk=new Track(type);
 245         trk->set_position(pos);
 246         trk->set_rotation(rot);
 247         trk->set_slope(slope);
 248         trk->set_flex(flex);
 249
 250         return trk;
 251 }
 252
 253 /*******************
 254 ** Track::Loader
 255 */
 256
 257 Track::Loader::Loader(Track &t):
 258         track(t)
 259 {
 260         add("position",    &Loader::position);
 261         add("rotation",    &Track::rot);
 262         add("slope",       &Track::slope);
 263         add("turnout_id",  &Track::turnout_id);
 264         add("sensor_id",   &Track::sensor_id);
 265         add("flex",        &Track::flex);
 266 }
 267
 268 void Track::Loader::position(float x, float y, float z)
 269 {
 270         track.pos=Point(x, y, z);
 271 }
 272
 273 } // namespace Marklin