source/libmarklin/track.cpp

   1 /* $Id$
   2
   3 This file is part of the MSP Märklin suite
   4 Copyright © 2006-2010  Mikkosoft Productions, Mikko Rasa
   5 Distributed under the GPL
   6 */
   7
   8 #include <cmath>
   9 #include "driver.h"
  10 #include "layout.h"
  11 #include "track.h"
  12 #include "tracktype.h"
  13
  14 using namespace std;
  15 using namespace Msp;
  16
  17 namespace Marklin {
  18
  19 Track::Track(Layout &l, const TrackType &t):
  20         layout(l),
  21         type(t),
  22         rot(0),
  23         slope(0),
  24         flex(false),
  25         turnout_id(type.is_turnout() ? layout.allocate_turnout_id(type.is_double_address()) : 0),
  26         sensor_id(0),
  27         links(type.get_endpoints().size()),
  28         active_path(0)
  29 {
  30         layout.add_track(*this);
  31
  32         if(layout.has_driver())
  33                 layout.get_driver().signal_turnout.connect(sigc::mem_fun(this, &Track::turnout_event));
  34
  35         for(unsigned paths = type.get_paths(); !(paths&1); ++active_path, paths>>=1) ;
  36 }
  37
  38 Track::~Track()
  39 {
  40         break_links();
  41         layout.remove_track(*this);
  42 }
  43
  44 void Track::set_position(const Point &p)
  45 {
  46         pos = p;
  47 }
  48
  49 void Track::set_rotation(float r)
  50 {
  51         rot = r;
  52         while(rot<0)
  53                 rot += M_PI*2;
  54         while(rot>M_PI*2)
  55                 rot -= M_PI*2;
  56 }
  57
  58 void Track::set_slope(float s)
  59 {
  60         if(links.size()!=2)
  61                 return;
  62
  63         slope = s;
  64 }
  65
  66 void Track::set_flex(bool f)
  67 {
  68         flex = f;
  69 }
  70
  71 void Track::check_slope()
  72 {
  73         if(links.size()!=2)
  74                 return;
  75
  76         if(links[0] && links[1])
  77         {
  78                 Point epp0 = links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
  79                 Point epp1 = links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
  80                 pos.z = epp0.z;
  81                 slope = epp1.z-pos.z;
  82         }
  83         else
  84         {
  85                 slope = 0;
  86                 if(links[0])
  87                 {
  88                         Point epp = links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
  89                         pos.z = epp.z;
  90                 }
  91                 else if(links[1])
  92                 {
  93                         Point epp = links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
  94                         pos.z = epp.z;
  95                 }
  96         }
  97 }
  98
  99 void Track::set_turnout_id(unsigned i)
 100 {
 101         if(!type.is_turnout())
 102                 throw InvalidState("Not a turnout");
 103
 104         turnout_id = i;
 105         layout.create_blocks(*this);
 106         layout.update_routes();
 107         if(layout.has_driver() && turnout_id)
 108         {
 109                 layout.get_driver().add_turnout(turnout_id);
 110                 if(type.is_double_address())
 111                         layout.get_driver().add_turnout(turnout_id+1);
 112         }
 113 }
 114
 115 void Track::set_sensor_id(unsigned i)
 116 {
 117         if(type.is_turnout())
 118                 throw InvalidState("Can't set sensor on a turnout");
 119
 120         sensor_id = i;
 121         layout.create_blocks(*this);
 122         if(layout.has_driver() && sensor_id)
 123                 layout.get_driver().add_sensor(sensor_id);
 124 }
 125
 126 void Track::set_active_path(unsigned p)
 127 {
 128         if(!turnout_id)
 129                 throw InvalidState("Not a turnout");
 130         if(!(type.get_paths()&(1<<p)))
 131                 throw InvalidParameterValue("Invalid path");
 132
 133         layout.get_driver().set_turnout(turnout_id, p&1);
 134         if(type.is_double_address())
 135                 layout.get_driver().set_turnout(turnout_id+1, p&2);
 136         else if(type.get_n_paths()>2)
 137                 active_path = (active_path&1) | (p&2);
 138 }
 139
 140 int Track::get_endpoint_by_link(Track &other) const
 141 {
 142         for(unsigned i=0; i<links.size(); ++i)
 143                 if(links[i]==&other)
 144                         return i;
 145
 146         return -1;
 147 }
 148
 149 Point Track::get_endpoint_position(unsigned epi) const
 150 {
 151         const vector<Endpoint> &eps = type.get_endpoints();
 152         if(epi>=eps.size())
 153                 throw InvalidParameterValue("Endpoint index out of range");
 154
 155         const Endpoint &ep = eps[epi];
 156
 157         float c = cos(rot);
 158         float s = sin(rot);
 159
 160         Point p(pos.x+c*ep.pos.x-s*ep.pos.y, pos.y+s*ep.pos.x+c*ep.pos.y, pos.z);
 161         if(eps.size()==2 && epi==1)
 162                 p.z += slope;
 163         return p;
 164 }
 165
 166 float Track::get_endpoint_direction(unsigned epi) const
 167 {
 168         const vector<Endpoint> &eps = type.get_endpoints();
 169         if(epi>=eps.size())
 170                 throw InvalidParameterValue("Endpoint index out of range");
 171
 172         const Endpoint &ep = eps[epi];
 173
 174         return rot+ep.dir;
 175 }
 176
 177 bool Track::snap_to(Track &other, bool link)
 178 {
 179         float limit = (link && !flex && !other.get_flex()) ? 1e-6 : 1e-4;
 180         const vector<Endpoint> &eps = type.get_endpoints();
 181         const vector<Endpoint> &other_eps = other.get_type().get_endpoints();
 182
 183         for(unsigned i=0; i<eps.size(); ++i)
 184         {
 185                 Point epp = get_endpoint_position(i);
 186
 187                 for(unsigned j=0; j<other_eps.size(); ++j)
 188                 {
 189                         if(other.get_link(j))
 190                                 continue;
 191
 192                         Point epp2 = other.get_endpoint_position(j);
 193                         float dx = epp2.x-epp.x;
 194                         float dy = epp2.y-epp.y;
 195                         if(dx*dx+dy*dy<limit)
 196                         {
 197                                 if(!link || (!flex && !other.get_flex()))
 198                                 {
 199                                         set_rotation(other.rot+other_eps[j].dir-eps[i].dir+M_PI);
 200                                         Point p(epp2.x-(eps[i].pos.x*cos(rot)-eps[i].pos.y*sin(rot)),
 201                                                 epp2.y-(eps[i].pos.y*cos(rot)+eps[i].pos.x*sin(rot)),
 202                                                 epp2.z);
 203                                         if(eps.size()==2 && i==1)
 204                                                 p.z -= slope;
 205                                         set_position(p);
 206                                 }
 207
 208                                 if(link)
 209                                 {
 210                                         if(links[i])
 211                                                 break_link(*links[i]);
 212                                         links[i] = &other;
 213                                         other.links[j] = this;
 214                                         layout.create_blocks(*this);
 215                                 }
 216
 217                                 return true;
 218                         }
 219                 }
 220         }
 221
 222         return false;
 223 }
 224
 225 bool Track::snap(Point &pt, float &d) const
 226 {
 227         const vector<Endpoint> &eps = type.get_endpoints();
 228
 229         for(unsigned i=0; i<eps.size(); ++i)
 230         {
 231                 Point epp = get_endpoint_position(i);
 232                 float dx = pt.x-epp.x;
 233                 float dy = pt.y-epp.y;
 234                 if(dx*dx+dy*dy<1e-4)
 235                 {
 236                         pt = epp;
 237                         d = rot+eps[i].dir;
 238                         return true;
 239                 }
 240         }
 241
 242         return false;
 243 }
 244
 245 void Track::break_link(Track &trk)
 246 {
 247         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 248                 if(*i==&trk)
 249                 {
 250                         *i = 0;
 251                         trk.break_link(*this);
 252                         // XXX Creates the blocks twice
 253                         layout.create_blocks(*this);
 254                         return;
 255                 }
 256 }
 257
 258 void Track::break_links()
 259 {
 260         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 261                 if(Track *trk=*i)
 262                 {
 263                         *i = 0;
 264                         trk->break_link(*this);
 265                 }
 266 }
 267
 268 Track *Track::get_link(unsigned i) const
 269 {
 270         if(i>links.size())
 271                 throw InvalidParameterValue("Link index out of range");
 272
 273         return links[i];
 274 }
 275
 276 unsigned Track::traverse(unsigned i, unsigned path) const
 277 {
 278         const vector<Endpoint> &eps = type.get_endpoints();
 279         if(i>=eps.size())
 280                 throw InvalidParameterValue("Endpoint index out of range");
 281
 282         const Endpoint &ep = eps[i];
 283
 284         if(ep.paths&(1<<path))
 285         {
 286                 // Find the other endpoint for this path
 287                 for(unsigned j=0; j<eps.size(); ++j)
 288                         if((eps[j].paths&(1<<path)) && j!=i)
 289                                 return j;
 290         }
 291         else
 292         {
 293                 // Find an endpoint that's connected to this one and has the requested path
 294                 for(unsigned j=0; j<eps.size(); ++j)
 295                         if((eps[j].paths&(1<<path)) && (eps[j].paths&ep.paths))
 296                                 return j;
 297         }
 298
 299         throw Exception("Track endpoint did not have a counterpart");
 300 }
 301
 302 unsigned Track::traverse(unsigned i) const
 303 {
 304         return traverse(i, active_path);
 305 }
 306
 307 TrackPoint Track::get_point(unsigned epi, unsigned path, float d) const
 308 {
 309         TrackPoint p = type.get_point(epi, path, d);
 310         float c = cos(rot);
 311         float s = sin(rot);
 312
 313         p.pos = Point(pos.x+c*p.pos.x-s*p.pos.y, pos.y+s*p.pos.x+c*p.pos.y, pos.z);
 314         p.dir += rot;
 315         if(type.get_endpoints().size()==2)
 316         {
 317                 float len = type.get_path_length(path);
 318                 float grade = slope/len;
 319                 if(epi==0)
 320                 {
 321                         p.pos.z += grade*d;
 322                         p.grade = grade;
 323                 }
 324                 else
 325                 {
 326                         p.pos.z += slope-grade*d;
 327                         p.grade = -grade;
 328                 }
 329         }
 330
 331         return p;
 332 }
 333
 334 TrackPoint Track::get_point(unsigned epi, float d) const
 335 {
 336         return get_point(epi, active_path, d);
 337 }
 338
 339 void Track::save(list<DataFile::Statement> &st) const
 340 {
 341         st.push_back((DataFile::Statement("position"), pos.x, pos.y, pos.z));
 342         st.push_back((DataFile::Statement("rotation"), rot));
 343         st.push_back((DataFile::Statement("slope"), slope));
 344         if(turnout_id)
 345                 st.push_back((DataFile::Statement("turnout_id"), turnout_id));
 346         if(sensor_id)
 347                 st.push_back((DataFile::Statement("sensor_id"), sensor_id));
 348         if(flex)
 349                 st.push_back((DataFile::Statement("flex"), true));
 350 }
 351
 352 void Track::turnout_event(unsigned addr, bool state)
 353 {
 354         if(!turnout_id)
 355                 return;
 356
 357         if(addr==turnout_id)
 358                 active_path = (active_path&2) | (state ? 1 : 0);
 359         else if(type.is_double_address() && addr==turnout_id+1)
 360                 active_path = (active_path&1) | (state ? 2 : 0);
 361 }
 362
 363
 364 Track::Loader::Loader(Track &t):
 365         DataFile::BasicLoader<Track>(t)
 366 {
 367         add("position",   &Loader::position);
 368         add("rotation",   &Track::rot);
 369         add("slope",      &Track::slope);
 370         add("turnout_id", &Loader::turnout_id);
 371         add("sensor_id",  &Loader::sensor_id);
 372         add("flex",       &Track::flex);
 373 }
 374
 375 void Track::Loader::position(float x, float y, float z)
 376 {
 377         obj.pos = Point(x, y, z);
 378 }
 379
 380 void Track::Loader::sensor_id(unsigned id)
 381 {
 382         obj.set_sensor_id(id);
 383 }
 384
 385 void Track::Loader::turnout_id(unsigned id)
 386 {
 387         obj.set_turnout_id(id);
 388 }
 389
 390 } // namespace Marklin