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