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(0),
  26         sensor_id(0),
  27         links(t.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         if(layout.has_driver() && turnout_id)
 107         {
 108                 layout.get_driver().add_turnout(turnout_id);
 109                 if(type.is_double_address())
 110                         layout.get_driver().add_turnout(turnout_id+1);
 111         }
 112 }
 113
 114 void Track::set_sensor_id(unsigned i)
 115 {
 116         if(type.is_turnout())
 117                 throw InvalidState("Can't set sensor on a turnout");
 118
 119         sensor_id = i;
 120         layout.create_blocks(*this);
 121         if(layout.has_driver() && sensor_id)
 122                 layout.get_driver().add_sensor(sensor_id);
 123 }
 124
 125 void Track::set_active_path(unsigned p)
 126 {
 127         if(!turnout_id)
 128                 throw InvalidState("Not a turnout");
 129         if(!(type.get_paths()&(1<<p)))
 130                 throw InvalidParameterValue("Invalid path");
 131
 132         layout.get_driver().set_turnout(turnout_id, p&1);
 133         if(type.is_double_address())
 134                 layout.get_driver().set_turnout(turnout_id+1, p&2);
 135         else if(type.get_n_paths()>2)
 136                 active_path = (active_path&1) | (p&2);
 137 }
 138
 139 int Track::get_endpoint_by_link(const Track &other) const
 140 {
 141         for(unsigned i=0; i<links.size(); ++i)
 142                 if(links[i]==&other)
 143                         return i;
 144
 145         return -1;
 146 }
 147
 148 Point Track::get_endpoint_position(unsigned epi) const
 149 {
 150         const vector<Endpoint> &eps = type.get_endpoints();
 151         if(epi>=eps.size())
 152                 throw InvalidParameterValue("Endpoint index out of range");
 153
 154         const Endpoint &ep = eps[epi];
 155
 156         float c = cos(rot);
 157         float s = sin(rot);
 158
 159         Point p(pos.x+c*ep.pos.x-s*ep.pos.y, pos.y+s*ep.pos.x+c*ep.pos.y, pos.z);
 160         if(eps.size()==2 && epi==1)
 161                 p.z += slope;
 162         return p;
 163 }
 164
 165 float Track::get_endpoint_direction(unsigned epi) const
 166 {
 167         const vector<Endpoint> &eps = type.get_endpoints();
 168         if(epi>=eps.size())
 169                 throw InvalidParameterValue("Endpoint index out of range");
 170
 171         const Endpoint &ep = eps[epi];
 172
 173         return rot+ep.dir;
 174 }
 175
 176 bool Track::snap_to(Track &other, bool link)
 177 {
 178         float limit = (link && !flex) ? 1e-6 : 1e-4;
 179         const vector<Endpoint> &eps = type.get_endpoints();
 180         const vector<Endpoint> &other_eps = other.get_type().get_endpoints();
 181
 182         for(unsigned i=0; i<eps.size(); ++i)
 183         {
 184                 Point epp = get_endpoint_position(i);
 185
 186                 for(unsigned j=0; j<other_eps.size(); ++j)
 187                 {
 188                         if(other.get_link(j))
 189                                 continue;
 190
 191                         Point epp2 = other.get_endpoint_position(j);
 192                         float dx = epp2.x-epp.x;
 193                         float dy = epp2.y-epp.y;
 194                         if(dx*dx+dy*dy<limit)
 195                         {
 196                                 set_rotation(other.rot+other_eps[j].dir-eps[i].dir+M_PI);
 197                                 Point p(epp2.x-(eps[i].pos.x*cos(rot)-eps[i].pos.y*sin(rot)),
 198                                         epp2.y-(eps[i].pos.y*cos(rot)+eps[i].pos.x*sin(rot)),
 199                                         epp2.z);
 200                                 if(eps.size()==2 && i==1)
 201                                         p.z -= slope;
 202                                 set_position(p);
 203
 204                                 if(link)
 205                                 {
 206                                         if(links[i])
 207                                                 break_link(*links[i]);
 208                                         links[i] = &other;
 209                                         other.links[j] = this;
 210                                         layout.create_blocks(*this);
 211                                 }
 212
 213                                 return true;
 214                         }
 215                 }
 216         }
 217
 218         return false;
 219 }
 220
 221 bool Track::snap(Point &pt, float &d) const
 222 {
 223         const vector<Endpoint> &eps = type.get_endpoints();
 224
 225         for(unsigned i=0; i<eps.size(); ++i)
 226         {
 227                 Point epp = get_endpoint_position(i);
 228                 float dx = pt.x-epp.x;
 229                 float dy = pt.y-epp.y;
 230                 if(dx*dx+dy*dy<1e-4)
 231                 {
 232                         pt = epp;
 233                         d = rot+eps[i].dir;
 234                         return true;
 235                 }
 236         }
 237
 238         return false;
 239 }
 240
 241 void Track::break_link(Track &trk)
 242 {
 243         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 244                 if(*i==&trk)
 245                 {
 246                         *i = 0;
 247                         trk.break_link(*this);
 248                         // XXX Creates the blocks twice
 249                         layout.create_blocks(*this);
 250                         return;
 251                 }
 252 }
 253
 254 void Track::break_links()
 255 {
 256         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 257                 if(Track *trk=*i)
 258                 {
 259                         *i = 0;
 260                         trk->break_link(*this);
 261                 }
 262 }
 263
 264 Track *Track::get_link(unsigned i) const
 265 {
 266         if(i>links.size())
 267                 throw InvalidParameterValue("Link index out of range");
 268
 269         return links[i];
 270 }
 271
 272 unsigned Track::traverse(unsigned i, unsigned path) const
 273 {
 274         const vector<Endpoint> &eps = type.get_endpoints();
 275         if(i>=eps.size())
 276                 throw InvalidParameterValue("Endpoint index out of range");
 277
 278         const Endpoint &ep = eps[i];
 279
 280         if(ep.paths&(1<<path))
 281         {
 282                 // Find the other endpoint for this path
 283                 for(unsigned j=0; j<eps.size(); ++j)
 284                         if((eps[j].paths&(1<<path)) && j!=i)
 285                                 return j;
 286         }
 287         else
 288         {
 289                 // Find an endpoint that's connected to this one and has the requested path
 290                 for(unsigned j=0; j<eps.size(); ++j)
 291                         if((eps[j].paths&(1<<path)) && (eps[j].paths&ep.paths))
 292                                 return j;
 293         }
 294
 295         throw Exception("Track endpoint did not have a counterpart");
 296 }
 297
 298 unsigned Track::traverse(unsigned i) const
 299 {
 300         return traverse(i, active_path);
 301 }
 302
 303 TrackPoint Track::get_point(unsigned epi, unsigned path, float d) const
 304 {
 305         TrackPoint p = type.get_point(epi, path, d);
 306         float c = cos(rot);
 307         float s = sin(rot);
 308
 309         p.pos = Point(pos.x+c*p.pos.x-s*p.pos.y, pos.y+s*p.pos.x+c*p.pos.y, 0);
 310         p.dir += rot;
 311         if(type.get_endpoints().size()==2)
 312         {
 313                 float len = type.get_path_length(path);
 314                 float grade = slope/len;
 315                 if(epi==0)
 316                 {
 317                         p.pos.z = pos.z+grade*d;
 318                         p.grade = grade;
 319                 }
 320                 else
 321                 {
 322                         p.pos.z = pos.z+slope-grade*d;
 323                         p.grade = -grade;
 324                 }
 325         }
 326
 327         return p;
 328 }
 329
 330 TrackPoint Track::get_point(unsigned epi, float d) const
 331 {
 332         return get_point(epi, active_path, d);
 333 }
 334
 335 void Track::save(list<DataFile::Statement> &st) const
 336 {
 337         st.push_back((DataFile::Statement("position"), pos.x, pos.y, pos.z));
 338         st.push_back((DataFile::Statement("rotation"), rot));
 339         st.push_back((DataFile::Statement("slope"), slope));
 340         if(turnout_id)
 341                 st.push_back((DataFile::Statement("turnout_id"), turnout_id));
 342         if(sensor_id)
 343                 st.push_back((DataFile::Statement("sensor_id"), sensor_id));
 344         if(flex)
 345                 st.push_back((DataFile::Statement("flex"), true));
 346 }
 347
 348 void Track::turnout_event(unsigned addr, bool state)
 349 {
 350         if(!turnout_id)
 351                 return;
 352
 353         if(addr==turnout_id)
 354                 active_path = (active_path&2) | (state ? 1 : 0);
 355         else if(type.is_double_address() && addr==turnout_id+1)
 356                 active_path = (active_path&1) | (state ? 2 : 0);
 357 }
 358
 359
 360 Track::Loader::Loader(Track &t):
 361         DataFile::BasicLoader<Track>(t)
 362 {
 363         add("position",   &Loader::position);
 364         add("rotation",   &Track::rot);
 365         add("slope",      &Track::slope);
 366         add("turnout_id", &Loader::turnout_id);
 367         add("sensor_id",  &Loader::sensor_id);
 368         add("flex",       &Track::flex);
 369 }
 370
 371 void Track::Loader::position(float x, float y, float z)
 372 {
 373         obj.pos = Point(x, y, z);
 374 }
 375
 376 void Track::Loader::sensor_id(unsigned id)
 377 {
 378         obj.set_sensor_id(id);
 379 }
 380
 381 void Track::Loader::turnout_id(unsigned id)
 382 {
 383         obj.set_turnout_id(id);
 384 }
 385
 386 } // namespace Marklin