source/libmarklin/track.cpp

   1 /* $Id$
   2
   3 This file is part of the MSP Märklin suite
   4 Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
   5 Distributed under the GPL
   6 */
   7
   8 #include <cmath>
   9 #include "track.h"
  10 #include "tracktype.h"
  11
  12 using namespace std;
  13 using namespace Msp;
  14
  15 #include <iostream>
  16
  17 namespace Marklin {
  18
  19 Track::Track(const TrackType &t):
  20         type(t),
  21         rot(0),
  22         slope(0),
  23         flex(false),
  24         turnout_id(0),
  25         sensor_id(0),
  26         links(t.get_endpoints().size())
  27 { }
  28
  29 Track::~Track()
  30 {
  31         break_links();
  32 }
  33
  34 void Track::set_position(const Point &p)
  35 {
  36         pos=p;
  37 }
  38
  39 void Track::set_rotation(float r)
  40 {
  41         rot=r;
  42         while(rot<0)
  43                 rot+=M_PI*2;
  44         while(rot>M_PI*2)
  45                 rot-=M_PI*2;
  46 }
  47
  48 void Track::set_slope(float s)
  49 {
  50         if(links.size()!=2) return;
  51
  52         slope=s;
  53 }
  54
  55 void Track::set_flex(bool f)
  56 {
  57         flex=f;
  58 }
  59
  60 void Track::set_turnout_id(unsigned i)
  61 {
  62         turnout_id=i;
  63 }
  64
  65 void Track::set_sensor_id(unsigned i)
  66 {
  67         sensor_id=i;
  68 }
  69
  70 int Track::get_endpoint_by_link(const Track &other) const
  71 {
  72         for(unsigned i=0; i<links.size(); ++i)
  73                 if(links[i]==&other)
  74                         return i;
  75
  76         return -1;
  77 }
  78
  79 Point Track::get_endpoint_position(unsigned epi) const
  80 {
  81         const vector<Endpoint> &eps=type.get_endpoints();
  82         if(epi>=eps.size())
  83                 throw InvalidParameterValue("Endpoint index out of range");
  84
  85         const Endpoint &ep=eps[epi];
  86
  87         float c=cos(rot);
  88         float s=sin(rot);
  89
  90         Point p(pos.x+c*ep.pos.x-s*ep.pos.y, pos.y+s*ep.pos.x+c*ep.pos.y, pos.z);
  91         if(eps.size()==2 && epi==1)
  92                 p.z+=slope;
  93         return p;
  94 }
  95
  96 float Track::get_endpoint_direction(unsigned epi) const
  97 {
  98         const vector<Endpoint> &eps=type.get_endpoints();
  99         if(epi>=eps.size())
 100                 throw InvalidParameterValue("Endpoint index out of range");
 101
 102         const Endpoint &ep=eps[epi];
 103
 104         return rot+ep.dir;
 105 }
 106
 107 bool Track::snap_to(Track &other, bool link)
 108 {
 109         float limit=(link && !flex) ? 1e-6 : 1e-4;
 110         const vector<Endpoint> &eps=type.get_endpoints();
 111         const vector<Endpoint> &other_eps=other.get_type().get_endpoints();
 112
 113         for(unsigned i=0; i<eps.size(); ++i)
 114         {
 115                 Point epp=get_endpoint_position(i);
 116
 117                 for(unsigned j=0; j<other_eps.size(); ++j)
 118                 {
 119                         if(other.get_link(j))
 120                                 continue;
 121
 122                         Point epp2=other.get_endpoint_position(j);
 123                         float dx=epp2.x-epp.x;
 124                         float dy=epp2.y-epp.y;
 125                         if(dx*dx+dy*dy<limit)
 126                         {
 127                                 set_rotation(other.rot+other_eps[j].dir-eps[i].dir+M_PI);
 128                                 set_position(Point(epp2.x-(eps[i].pos.x*cos(rot)-eps[i].pos.y*sin(rot)), epp2.y-(eps[i].pos.y*cos(rot)+eps[i].pos.x*sin(rot)), epp2.z));
 129
 130                                 if(link)
 131                                 {
 132                                         if(links[i])
 133                                                 break_link(*links[i]);
 134                                         links[i]=&other;
 135                                         other.links[j]=this;
 136                                 }
 137
 138                                 return true;
 139                         }
 140                 }
 141         }
 142
 143         return false;
 144 }
 145
 146 bool Track::snap(Point &pt, float &d) const
 147 {
 148         const vector<Endpoint> &eps=type.get_endpoints();
 149
 150         for(unsigned i=0; i<eps.size(); ++i)
 151         {
 152                 Point epp=get_endpoint_position(i);
 153                 float dx=pt.x-epp.x;
 154                 float dy=pt.y-epp.y;
 155                 if(dx*dx+dy*dy<1e-4)
 156                 {
 157                         pt=epp;
 158                         d=rot+eps[i].dir;
 159                         return true;
 160                 }
 161         }
 162
 163         return false;
 164 }
 165
 166 void Track::break_link(Track &trk)
 167 {
 168         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 169                 if(*i==&trk)
 170                 {
 171                         *i=0;
 172                         trk.break_link(*this);
 173                         return;
 174                 }
 175 }
 176
 177 void Track::break_links()
 178 {
 179         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 180                 if(Track *trk=*i)
 181                 {
 182                         *i=0;
 183                         trk->break_link(*this);
 184                 }
 185 }
 186
 187 Track *Track::get_link(unsigned i) const
 188 {
 189         if(i>links.size())
 190                 throw InvalidParameterValue("Link index out of range");
 191
 192         return links[i];
 193 }
 194
 195 void Track::check_slope()
 196 {
 197         if(links.size()!=2)
 198                 return;
 199
 200         if(links[0] && links[1])
 201         {
 202                 Point epp0=links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
 203                 Point epp1=links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
 204                 pos.z=epp0.z;
 205                 slope=epp1.z-pos.z;
 206         }
 207         else
 208         {
 209                 slope=0;
 210                 if(links[0])
 211                 {
 212                         Point epp=links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
 213                         pos.z=epp.z;
 214                 }
 215                 else if(links[1])
 216                 {
 217                         Point epp=links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
 218                         pos.z=epp.z;
 219                 }
 220         }
 221 }
 222
 223 int Track::traverse(unsigned i, unsigned route) const
 224 {
 225         const vector<Endpoint> &eps=type.get_endpoints();
 226         if(i>=eps.size())
 227                 throw InvalidParameterValue("Endpoint index out of range");
 228
 229         const Endpoint &ep=eps[i];
 230
 231         if(ep.routes&(1<<route))
 232         {
 233                 // Find the other endpoint for this route
 234                 for(unsigned j=0; j<eps.size(); ++j)
 235                         if((eps[j].routes&(1<<route)) && j!=i)
 236                                 return j;
 237         }
 238         else
 239         {
 240                 // Find an endpoint that's connected to this one and has the requested route
 241                 for(unsigned j=0; j<eps.size(); ++j)
 242                         if((eps[j].routes&(1<<route)) && (eps[j].routes&ep.routes))
 243                                 return j;
 244         }
 245
 246         return -1;
 247 }
 248
 249 Point Track::get_point(unsigned epi, unsigned route, float d) const
 250 {
 251         const vector<Endpoint> &eps=type.get_endpoints();
 252         if(epi>=eps.size())
 253                 throw InvalidParameterValue("Endpoint index out of range");
 254
 255         float x=eps[epi].pos.x;
 256         float y=eps[epi].pos.y;
 257
 258         const vector<TrackPart> &parts=type.get_parts();
 259         const TrackPart *last_part=0;
 260         while(1)
 261         {
 262                 for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
 263                 {
 264                         if((eps[epi].routes&(1<<route)) && i->route!=route)
 265                                 continue;
 266                         if(&*i==last_part)
 267                                 continue;
 268
 269                         vector<Endpoint> part_eps;
 270                         i->collect_endpoints(part_eps);
 271                         for(unsigned j=0; j<part_eps.size(); ++j)
 272                         {
 273                                 float dx=part_eps[j].pos.x-x;
 274                                 float dy=part_eps[j].pos.y-y;
 275                                 if(dx*dx+dy*dy<1e-6)
 276                                 {
 277                                         float plen=i->length;
 278                                         if(i->radius)
 279                                                 plen*=abs(i->radius);
 280                                         if(d<plen)
 281                                         {
 282                                                 if(j==1)
 283                                                         d=plen-d;
 284                                                 Point p=i->get_point(d);
 285                                                 float c=cos(rot);
 286                                                 float s=sin(rot);
 287                                                 return Point(pos.x+c*p.x-s*p.y, pos.y+c*p.y+s*p.x);
 288                                         }
 289                                         else if(part_eps.size()>1)
 290                                         {
 291                                                 d-=plen;
 292                                                 x=part_eps[1-j].pos.x;
 293                                                 y=part_eps[1-j].pos.y;
 294                                                 last_part=&*i;
 295                                                 i=parts.begin();
 296                                                 break;
 297                                         }
 298                                         else
 299                                                 return pos;
 300                                 }
 301                         }
 302                 }
 303
 304                 if(!last_part)
 305                         throw Exception("Internal error (Endpoint does not match any part)");
 306                 else
 307                         return pos;
 308         }
 309 }
 310
 311 Track *Track::copy() const
 312 {
 313         Track *trk=new Track(type);
 314         trk->set_position(pos);
 315         trk->set_rotation(rot);
 316         trk->set_slope(slope);
 317         trk->set_flex(flex);
 318
 319         return trk;
 320 }
 321
 322 /*******************
 323 ** Track::Loader
 324 */
 325
 326 Track::Loader::Loader(Track &t):
 327         track(t)
 328 {
 329         add("position",    &Loader::position);
 330         add("rotation",    &Track::rot);
 331         add("slope",       &Track::slope);
 332         add("turnout_id",  &Track::turnout_id);
 333         add("sensor_id",   &Track::sensor_id);
 334         add("flex",        &Track::flex);
 335 }
 336
 337 void Track::Loader::position(float x, float y, float z)
 338 {
 339         track.pos=Point(x, y, z);
 340 }
 341
 342 } // namespace Marklin