source/libr2c2/track.cpp

   1 #include <cmath>
   2 #include <msp/core/maputils.h>
   3 #include <msp/strings/format.h>
   4 #include "block.h"
   5 #include "catalogue.h"
   6 #include "driver.h"
   7 #include "layout.h"
   8 #include "track.h"
   9 #include "trackattachment.h"
  10 #include "tracktype.h"
  11
  12 using namespace std;
  13 using namespace Msp;
  14
  15 namespace {
  16
  17 struct AttachmentCompare
  18 {
  19         unsigned entry;
  20
  21         AttachmentCompare(unsigned e): entry(e) { }
  22
  23         bool operator()(const R2C2::TrackAttachment *a1, const R2C2::TrackAttachment *a2) const
  24         { return a1->get_offset_from_endpoint(entry)<a2->get_offset_from_endpoint(entry); }
  25 };
  26
  27 }
  28
  29 namespace R2C2 {
  30
  31 Track::Track(Layout &l, const TrackType &t):
  32         Object(l),
  33         type(t),
  34         block(0),
  35         slope(0),
  36         flex(false),
  37         turnout_addr(0),
  38         sensor_addr(0),
  39         links(type.get_endpoints().size()),
  40         active_path(0),
  41         path_changing(false)
  42 {
  43         if(type.is_turnout())
  44         {
  45                 turnout_addr = layout.allocate_turnout_address();
  46
  47                 if(layout.has_driver())
  48                 {
  49                         Driver &driver = layout.get_driver();
  50                         turnout_id = driver.add_turnout(turnout_addr, type);
  51                         driver.signal_turnout.connect(sigc::mem_fun(this, &Track::turnout_event));
  52                         driver.signal_turnout_failed.connect(sigc::mem_fun(this, &Track::turnout_failed));
  53                 }
  54         }
  55
  56         layout.add(*this);
  57
  58
  59         for(unsigned paths = type.get_paths(); !(paths&1); ++active_path, paths>>=1) ;
  60 }
  61
  62 Track::~Track()
  63 {
  64         break_links();
  65         if(layout.has_driver() && turnout_id)
  66                 layout.get_driver().remove_turnout(turnout_id);
  67         layout.remove(*this);
  68 }
  69
  70 Track *Track::clone(Layout *to_layout) const
  71 {
  72         Track *track = new Track((to_layout ? *to_layout : layout), type);
  73         track->set_position(position);
  74         track->set_rotation(rotation);
  75         return track;
  76 }
  77
  78 void Track::set_block(Block *b)
  79 {
  80         if(b && !b->has_track(*this))
  81                 throw logic_error("track not in block");
  82         if(!b && block && block->has_track(*this))
  83                 throw logic_error("track still in block");
  84
  85         block = b;
  86 }
  87
  88 Block &Track::get_block() const
  89 {
  90         if(!block)
  91                 throw logic_error("!block");
  92
  93         return *block;
  94 }
  95
  96 void Track::set_position(const Vector &p)
  97 {
  98         position = p;
  99         signal_moved.emit();
 100         propagate_slope();
 101 }
 102
 103 void Track::set_rotation(const Angle &r)
 104 {
 105         rotation = wrap_positive(r);
 106         signal_moved.emit();
 107 }
 108
 109 void Track::set_tilt(const Angle &t)
 110 {
 111         if(links.size()!=2)
 112                 return;
 113
 114         tilt = t;
 115         slope = tan(tilt)*type.get_path_length(0);
 116         signal_moved.emit();
 117         propagate_slope();
 118 }
 119
 120 void Track::set_flex(bool f)
 121 {
 122         flex = f;
 123 }
 124
 125 void Track::propagate_slope()
 126 {
 127         for(vector<Track *>::const_iterator i=links.begin(); i!=links.end(); ++i)
 128                 if(*i)
 129                         (*i)->check_slope();
 130 }
 131
 132 void Track::check_slope()
 133 {
 134         if(links.size()!=2)
 135                 return;
 136
 137         if(links[0] && links[1])
 138         {
 139                 Vector epp0 = links[0]->get_snap_node(links[0]->get_link_slot(*this)).position;
 140                 Vector epp1 = links[1]->get_snap_node(links[1]->get_link_slot(*this)).position;
 141                 position.z = epp0.z;
 142                 slope = epp1.z-position.z;
 143                 tilt = Geometry::atan(slope/type.get_path_length(0));
 144         }
 145         else
 146         {
 147                 if(links[0])
 148                 {
 149                         Vector epp = links[0]->get_snap_node(links[0]->get_link_slot(*this)).position;
 150                         position.z = epp.z;
 151                 }
 152                 else if(links[1])
 153                 {
 154                         Vector epp = links[1]->get_snap_node(links[1]->get_link_slot(*this)).position;
 155                         position.z = epp.z-slope;
 156                 }
 157         }
 158
 159         signal_moved.emit();
 160 }
 161
 162 void Track::set_turnout_address(unsigned a)
 163 {
 164         if(!type.is_turnout())
 165                 throw logic_error("not a turnout");
 166         if(!a)
 167                 throw invalid_argument("Track::set_turnout_id");
 168
 169         Driver *driver = (layout.has_driver() ? &layout.get_driver() : 0);
 170
 171         if(driver && turnout_id)
 172                 driver->remove_turnout(turnout_id);
 173         turnout_addr = a;
 174         layout.create_blocks(*this);
 175         layout.update_routes();
 176         if(driver && turnout_addr)
 177                 turnout_id = driver->add_turnout(turnout_addr, type);
 178         else
 179                 turnout_id = 0;
 180 }
 181
 182 void Track::set_sensor_address(unsigned a)
 183 {
 184         if(type.is_turnout())
 185                 throw logic_error("is a turnout");
 186
 187         sensor_addr = a;
 188         layout.create_blocks(*this);
 189 }
 190
 191 void Track::set_active_path(unsigned p)
 192 {
 193         if(!type.is_turnout())
 194                 throw logic_error("not a turnout");
 195         if(!(type.get_paths()&(1<<p)))
 196                 throw invalid_argument("Track::set_active_path");
 197
 198         if(active_path==p)
 199                 return;
 200
 201         signal_path_changing(p);
 202         path_changing = true;
 203         layout.get_driver().set_turnout(turnout_id, p);
 204 }
 205
 206 float Track::get_path_length(int p) const
 207 {
 208         if(p<0)
 209                 p = active_path;
 210         return type.get_path_length(p);
 211 }
 212
 213 OrientedPoint Track::get_point(unsigned epi, unsigned path, float d) const
 214 {
 215         OrientedPoint p = type.get_point(epi, path, d);
 216
 217         p.position = position+rotated_vector(p.position, rotation);
 218         p.rotation += rotation;
 219         if(type.get_endpoints().size()==2)
 220         {
 221                 float dz = tan(tilt)*d;
 222                 if(epi==0)
 223                 {
 224                         p.position.z += dz;
 225                         p.tilt = tilt;
 226                 }
 227                 else
 228                 {
 229                         p.position.z += slope-dz;
 230                         p.tilt = -tilt;
 231                 }
 232         }
 233
 234         return p;
 235 }
 236
 237 OrientedPoint Track::get_point(unsigned epi, float d) const
 238 {
 239         return get_point(epi, active_path, d);
 240 }
 241
 242 unsigned Track::get_n_snap_nodes() const
 243 {
 244         return type.get_endpoints().size();
 245 }
 246
 247 Snap Track::get_snap_node(unsigned i) const
 248 {
 249         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
 250         if(i>=eps.size())
 251                 throw out_of_range("Track::get_snap_node");
 252
 253         Snap result;
 254         const TrackType::Endpoint &ep = eps[i];
 255
 256         result.position = position+rotated_vector(ep.pos, rotation);
 257         if(eps.size()==2 && i==1)
 258                 result.position.z += slope;
 259
 260         result.rotation = rotation+ep.dir;
 261
 262         return result;
 263 }
 264
 265 bool Track::snap(Snap &sn, float limit, SnapType what) const
 266 {
 267         if(Object::snap(sn, limit, what))
 268                 return true;
 269
 270         if(what&SNAP_SEGMENT)
 271         {
 272                 Vector local = rotated_vector(sn.position-position, -rotation);
 273
 274                 OrientedPoint np = type.get_nearest_point(local);
 275                 Vector span = local-np.position;
 276                 if(dot(span, span)<=limit*limit)
 277                 {
 278                         sn.position = position+rotated_vector(np.position, rotation);
 279                         sn.rotation = np.rotation+rotation;
 280                         return true;
 281                 }
 282         }
 283
 284         return false;
 285 }
 286
 287 SnapType Track::get_default_snap_type_to(const Object &other) const
 288 {
 289         if(dynamic_cast<const Track *>(&other))
 290                 return SNAP_NODE;
 291
 292         return NO_SNAP;
 293 }
 294
 295 unsigned Track::get_n_link_slots() const
 296 {
 297         return links.size();
 298 }
 299
 300 Track *Track::get_link(unsigned i) const
 301 {
 302         if(i>=links.size())
 303                 throw out_of_range("Track::get_link");
 304
 305         return links[i];
 306 }
 307
 308 int Track::get_link_slot(const Object &other) const
 309 {
 310         for(unsigned i=0; i<links.size(); ++i)
 311                 if(links[i]==&other)
 312                         return i;
 313
 314         return -1;
 315 }
 316
 317 bool Track::link_to(Object &other)
 318 {
 319         Track *otrack = dynamic_cast<Track *>(&other);
 320         if(!otrack)
 321                 return false;
 322
 323         float limit = layout.get_catalogue().get_gauge();
 324         if(!flex && !otrack->get_flex())
 325                 limit /= 10;
 326         limit *= limit;
 327
 328         unsigned nsn = get_n_snap_nodes();
 329         unsigned other_nsn = other.get_n_snap_nodes();
 330         for(unsigned i=0; i<nsn; ++i)
 331         {
 332                 Snap sn = get_snap_node(i);
 333                 for(unsigned j=0; j<other_nsn; ++j)
 334                 {
 335                         Snap osn = other.get_snap_node(j);
 336                         Vector span = osn.position-sn.position;
 337                         Angle da = wrap_balanced(osn.rotation-sn.rotation-Angle::half_turn());
 338
 339                         if(dot(span, span)<limit && abs(da).radians()<0.01)
 340                         {
 341                                 break_link(i);
 342                                 otrack->break_link(j);
 343                                 links[i] = otrack;
 344                                 otrack->links[j] = this;
 345                                 check_slope();
 346                                 layout.create_blocks(*this);
 347
 348                                 signal_link_changed.emit(i, otrack);
 349                                 otrack->signal_link_changed.emit(j, this);
 350                                 return true;
 351                         }
 352                 }
 353         }
 354
 355         return false;
 356 }
 357
 358 bool Track::break_link(unsigned i)
 359 {
 360         if(i>=links.size())
 361                 throw out_of_range("Track::break_link");
 362
 363         Track *other = links[i];
 364         if(!other)
 365                 return false;
 366
 367         links[i] = 0;
 368         if(!other->break_link(*this))
 369         {
 370                 /* If the call doesn't succeed, it means that the other track already
 371                 broke the link and is calling us right now.  Recreate blocks in the inner
 372                 call so it occurs before any signals are emitted. */
 373                 layout.create_blocks(*this);
 374         }
 375
 376         signal_link_changed.emit(i, 0);
 377
 378         return true;
 379 }
 380
 381 void Track::add_attachment(TrackAttachment &a)
 382 {
 383         if(find(attachments.begin(), attachments.end(), &a)!=attachments.end())
 384                 throw key_error(&a);
 385         attachments.push_back(&a);
 386 }
 387
 388 void Track::remove_attachment(TrackAttachment &a)
 389 {
 390         AttachmentList::iterator i = find(attachments.begin(), attachments.end(), &a);
 391         if(i==attachments.end())
 392                 throw key_error(&a);
 393         attachments.erase(i);
 394 }
 395
 396 Track::AttachmentList Track::get_attachments_ordered(unsigned epi) const
 397 {
 398         AttachmentList result = attachments;
 399         result.sort(AttachmentCompare(epi));
 400         return result;
 401 }
 402
 403 void Track::save(list<DataFile::Statement> &st) const
 404 {
 405         st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
 406         st.push_back((DataFile::Statement("rotation"), rotation.radians()));
 407         st.push_back((DataFile::Statement("tilt"), tilt.radians()));
 408         if(turnout_addr)
 409                 st.push_back((DataFile::Statement("turnout_address"), turnout_addr));
 410         if(sensor_addr)
 411                 st.push_back((DataFile::Statement("sensor_address"), sensor_addr));
 412         if(flex)
 413                 st.push_back((DataFile::Statement("flex"), true));
 414 }
 415
 416 void Track::turnout_event(unsigned id, unsigned state)
 417 {
 418         if(id==turnout_id)
 419         {
 420                 active_path = state;
 421                 path_changing = false;
 422                 signal_path_changed.emit(active_path);
 423         }
 424 }
 425
 426 void Track::turnout_failed(unsigned id)
 427 {
 428         if(id==turnout_id)
 429         {
 430                 path_changing = false;
 431                 layout.emergency(block, "Turnout failed");
 432         }
 433 }
 434
 435
 436 Track::Loader::Loader(Track &t):
 437         DataFile::ObjectLoader<Track>(t)
 438 {
 439         add("position",   &Loader::position);
 440         add("rotation",   &Loader::rotation);
 441         add("tilt",       &Loader::tilt);
 442         add("turnout_id", &Loader::turnout_address);
 443         add("turnout_address", &Loader::turnout_address);
 444         add("sensor_id",  &Loader::sensor_address);
 445         add("sensor_address",  &Loader::sensor_address);
 446         add("flex",       &Track::flex);
 447
 448         // deprecated
 449         add("slope",      &Loader::slope);
 450 }
 451
 452 void Track::Loader::position(float x, float y, float z)
 453 {
 454         obj.set_position(Vector(x, y, z));
 455 }
 456
 457 void Track::Loader::rotation(float r)
 458 {
 459         obj.set_rotation(Angle::from_radians(r));
 460 }
 461
 462 void Track::Loader::sensor_address(unsigned addr)
 463 {
 464         obj.set_sensor_address(addr);
 465 }
 466
 467 void Track::Loader::slope(float s)
 468 {
 469         obj.set_tilt(Geometry::atan(s/obj.type.get_path_length(0)));
 470 }
 471
 472 void Track::Loader::tilt(float t)
 473 {
 474         obj.set_tilt(Angle::from_radians(t));
 475 }
 476
 477 void Track::Loader::turnout_address(unsigned addr)
 478 {
 479         obj.set_turnout_address(addr);
 480 }
 481
 482 } // namespace R2C2