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