source/libmarklin/track.cpp

   1 #include <cmath>
   2 #include "track.h"
   3
   4 using namespace std;
   5 using namespace Msp;
   6
   7 #include <iostream>
   8
   9 namespace Marklin {
  10
  11 Track::Track(unsigned a):
  12         art_nr(a),
  13         rot(0),
  14         slope(0),
  15         flex(false),
  16         turnout_id(0),
  17         sensor_id(0)
  18 { }
  19
  20 Track::~Track()
  21 {
  22         for(EndpointSeq::iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
  23                 if(i->link)
  24                 {
  25                         Track *trk=i->link;
  26                         i->link=0;
  27                         trk->break_link(*this);
  28                 }
  29 }
  30
  31 void Track::set_position(const Point &p)
  32 {
  33         pos=p;
  34 }
  35
  36 void Track::set_rotation(float r)
  37 {
  38         rot=r;
  39         while(rot<0)
  40                 rot+=M_PI*2;
  41         while(rot>M_PI*2)
  42                 rot-=M_PI*2;
  43 }
  44
  45 void Track::set_slope(float s)
  46 {
  47         if(endpoints.size()!=2) return;
  48
  49         slope=s;
  50         endpoints.back().pos.z=slope;
  51 }
  52
  53 const Track::Endpoint *Track::get_endpoint_by_link(Track *other) const
  54 {
  55         for(EndpointSeq::const_iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
  56                 if(i->link==other)
  57                         return &*i;
  58
  59         return 0;
  60 }
  61
  62 Point Track::get_endpoint_position(const Endpoint &ep) const
  63 {
  64         float c=cos(rot);
  65         float s=sin(rot);
  66         return Point(pos.x+c*ep.pos.x-s*ep.pos.y, pos.y+s*ep.pos.x+c*ep.pos.y, pos.z+ep.pos.z);
  67 }
  68
  69 float Track::get_length() const
  70 {
  71         float len=parts.front().length;
  72         if(parts.front().radius)
  73                 len*=parts.front().radius;
  74         return len;
  75 }
  76
  77 float Track::get_total_length() const
  78 {
  79         float len=0;
  80         for(PartSeq::const_iterator i=parts.begin(); i!=parts.end(); ++i)
  81         {
  82                 float l=i->length;
  83                 if(i->radius)
  84                         l*=i->radius;
  85                 len+=l;
  86         }
  87         return len;
  88 }
  89
  90 unsigned Track::get_n_routes() const
  91 {
  92         unsigned n=1;
  93         for(PartSeq::const_iterator i=parts.begin(); i!=parts.end(); ++i)
  94                 if(i->route>=n)
  95                         n=i->route+1;
  96         return n;
  97 }
  98
  99 bool Track::snap_to(Track &other, bool link)
 100 {
 101         float limit=(link && !flex) ? 1e-6 : 1e-4;
 102         for(EndpointSeq::iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
 103         {
 104                 Point epp=get_endpoint_position(*i);
 105                 for(EndpointSeq::iterator j=other.endpoints.begin(); j!=other.endpoints.end(); ++j)
 106                 {
 107                         if(j->link)
 108                                 continue;
 109
 110                         Point epp2=other.get_endpoint_position(*j);
 111                         float dx=epp2.x-epp.x;
 112                         float dy=epp2.y-epp.y;
 113                         if(dx*dx+dy*dy<limit)
 114                         {
 115                                 set_rotation(other.rot+j->rot-i->rot+M_PI);
 116                                 set_position(Point(epp2.x-(i->pos.x*cos(rot)-i->pos.y*sin(rot)), epp2.y-(i->pos.y*cos(rot)+i->pos.x*sin(rot)), other.pos.z+j->pos.z-i->pos.z));
 117                                 if(link)
 118                                 {
 119                                         if(i->link)
 120                                                 break_link(*i->link);
 121                                         i->link=&other;
 122                                         j->link=this;
 123                                 }
 124                                 return true;
 125                         }
 126                 }
 127         }
 128
 129         return false;
 130 }
 131
 132 bool Track::snap(Point &pt, float &d) const
 133 {
 134         for(EndpointSeq::const_iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
 135         {
 136                 Point epp=get_endpoint_position(*i);
 137                 float dx=pt.x-epp.x;
 138                 float dy=pt.y-epp.y;
 139                 if(dx*dx+dy*dy<1e-4)
 140                 {
 141                         pt=epp;
 142                         d=rot+i->rot;
 143                         return true;
 144                 }
 145         }
 146
 147         return false;
 148 }
 149
 150 void Track::break_link(Track &trk)
 151 {
 152         for(EndpointSeq::iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
 153                 if(i->link==&trk)
 154                 {
 155                         i->link=0;
 156                         trk.break_link(*this);
 157                         return;
 158                 }
 159 }
 160
 161 void Track::break_links()
 162 {
 163         for(EndpointSeq::iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
 164                 if(i->link)
 165                 {
 166                         Track *trk=i->link;
 167                         i->link=0;
 168                         trk->break_link(*this);
 169                 }
 170 }
 171
 172 void Track::check_slope()
 173 {
 174         if(endpoints.size()!=2)
 175                 return;
 176
 177         Track *link1=endpoints.front().link;
 178         Track *link2=endpoints.back().link;
 179         if(link1 && link2)
 180         {
 181                 const Endpoint *ep1=link1->get_endpoint_by_link(this);
 182                 const Endpoint *ep2=link2->get_endpoint_by_link(this);
 183                 pos.z=link1->pos.z+ep1->pos.z;
 184                 slope=(link2->pos.z+ep2->pos.z)-pos.z;
 185         }
 186         else
 187         {
 188                 slope=0;
 189                 if(link1)
 190                 {
 191                         const Endpoint *ep=link1->get_endpoint_by_link(this);
 192                         pos.z=link1->pos.z+ep->pos.z;
 193                 }
 194                 else if(link2)
 195                 {
 196                         const Endpoint *ep=link2->get_endpoint_by_link(this);
 197                         pos.z=link2->pos.z+ep->pos.z;
 198                 }
 199         }
 200
 201         endpoints.back().pos.z=slope;
 202 }
 203
 204 const Track::Endpoint *Track::traverse(const Endpoint *ep, unsigned route) const
 205 {
 206         if(ep->routes&(1<<route))
 207         {
 208                 // Find the other endpoint for this route
 209                 for(EndpointSeq::const_iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
 210                         if((i->routes&(1<<route)) && &*i!=ep)
 211                                 return &*i;
 212         }
 213         else
 214         {
 215                 // Find an endpoint that's connected to this one and has the requested route
 216                 for(EndpointSeq::const_iterator i=endpoints.begin(); i!=endpoints.end(); ++i)
 217                         if((i->routes&(1<<route)) && (i->routes&ep->routes))
 218                                 return &*i;
 219         }
 220
 221         return 0;
 222 }
 223
 224 Track *Track::copy() const
 225 {
 226         Track *trk=new Track(*this);
 227         for(EndpointSeq::iterator i=trk->endpoints.begin(); i!=trk->endpoints.end(); ++i)
 228                 i->link=0;
 229         trk->turnout_id=0;
 230         trk->sensor_id=0;
 231
 232         return trk;
 233 }
 234
 235 /*** private ***/
 236
 237 void Track::collect_endpoints()
 238 {
 239         endpoints.clear();
 240
 241         for(PartSeq::iterator i=parts.begin(); i!=parts.end(); ++i)
 242                 i->collect_endpoints(endpoints);
 243         endpoints.back().pos.z=slope;
 244
 245         for(EndpointSeq::iterator i=endpoints.begin(); i!=endpoints.end();)
 246         {
 247                 bool rm=false;
 248                 for(EndpointSeq::iterator j=i; j!=endpoints.end();)
 249                 {
 250                         if(j==i)
 251                         {
 252                                 ++j;
 253                                 continue;
 254                         }
 255                         float dx=i->pos.x-j->pos.x;
 256                         float dy=i->pos.y-j->pos.y;
 257                         if(dx*dx+dy*dy<0.0001)
 258                         {
 259                                 float da=i->rot-j->rot;
 260                                 if(da<-M_PI)
 261                                         da+=M_PI*2;
 262                                 if(da>M_PI)
 263                                         da-=M_PI*2;
 264                                 if(da<-3 || da>3)
 265                                         rm=true;
 266                                 i->routes|=j->routes;
 267                                 j=endpoints.erase(j);
 268                         }
 269                         else
 270                                 ++j;
 271                 }
 272                 if(rm)
 273                         i=endpoints.erase(i);
 274                 else
 275                         ++i;
 276         }
 277 }
 278
 279 /*******************
 280 ** Track::Loader
 281 */
 282
 283 Track::Loader::Loader(Track &t):
 284         track(t)
 285 {
 286         add("description", &Track::description);
 287         add("part",        &Loader::part);
 288         add("position",    &Loader::position);
 289         add("rotation",    &Track::rot);
 290         add("slope",       &Track::slope);
 291         add("turnout_id",  &Track::turnout_id);
 292         add("sensor_id",   &Track::sensor_id);
 293         add("flex",        &Track::flex);
 294 }
 295
 296 Track::Loader::~Loader()
 297 {
 298         track.collect_endpoints();
 299 }
 300
 301 void Track::Loader::part()
 302 {
 303         Part p;
 304         load_sub(p);
 305         track.parts.push_back(p);
 306 }
 307
 308 void Track::Loader::position(float x, float y, float z)
 309 {
 310         track.pos=Point(x, y, z);
 311 }
 312
 313 /*******************
 314 ** Track::Part
 315 */
 316
 317 Track::Part::Part():
 318         x(0),
 319         y(0),
 320         dir(0),
 321         length(0),
 322         radius(0),
 323         route(0),
 324         dead_end(false)
 325 { }
 326
 327 void Track::Part::collect_endpoints(EndpointSeq &epl)
 328 {
 329         epl.push_back(Endpoint(Point(x, y, 0), dir+M_PI, 1<<route));
 330         if(dead_end)
 331                 return;
 332         else if(radius)
 333         {
 334                 float a=(radius<0)?-length:length;
 335                 float c=cos(a);
 336                 float s=sin(a);
 337                 float rx=radius*sin(dir);
 338                 float ry=-radius*cos(dir);
 339                 epl.push_back(Endpoint(Point(x+c*rx-s*ry-rx, y+c*ry+s*rx-ry, 0), dir+a, 1<<route));
 340         }
 341         else
 342                 epl.push_back(Endpoint(Point(x+cos(dir)*length, y+sin(dir)*length, 0), dir, 1<<route));
 343 }
 344
 345 Track::Part::Loader::Loader(Part &p):
 346         part(p)
 347 {
 348         add("start",    &Loader::start);
 349         add("length",   &Part::length);
 350         add("radius",   &Part::radius);
 351         add("route",    &Part::route);
 352         add("dead_end", &Part::dead_end);
 353 }
 354
 355 Track::Part::Loader::~Loader()
 356 {
 357         if(part.radius)
 358         {
 359                 part.length*=M_PI/180;
 360                 part.radius/=1000;
 361         }
 362         else
 363                 part.length/=1000;
 364
 365         part.x/=1000;
 366         part.y/=1000;
 367         part.dir*=M_PI/180;
 368 }
 369
 370 void Track::Part::Loader::start(float x, float y, float d)
 371 {
 372         part.x=x;
 373         part.y=y;
 374         part.dir=d;
 375 }
 376
 377 } // namespace Marklin