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