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