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