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 gauge_ratio = otrack->get_type().get_gauge()/type.get_gauge();
 324         if(gauge_ratio<0.99 || gauge_ratio>1.01)
 325                 return false;
 326
 327         float limit = type.get_gauge();
 328         if(!flex && !otrack->get_flex())
 329                 limit /= 10;
 330         limit *= limit;
 331
 332         unsigned nsn = get_n_snap_nodes();
 333         unsigned other_nsn = other.get_n_snap_nodes();
 334         for(unsigned i=0; i<nsn; ++i)
 335         {
 336                 Snap sn = get_snap_node(i);
 337                 for(unsigned j=0; j<other_nsn; ++j)
 338                 {
 339                         Snap osn = other.get_snap_node(j);
 340                         Vector span = osn.position-sn.position;
 341                         Angle da = wrap_balanced(osn.rotation-sn.rotation-Angle::half_turn());
 342
 343                         if(dot(span, span)<limit && abs(da).radians()<0.01)
 344                         {
 345                                 break_link(i);
 346                                 otrack->break_link(j);
 347                                 links[i] = otrack;
 348                                 otrack->links[j] = this;
 349                                 check_slope();
 350                                 layout.create_blocks(*this);
 351
 352                                 signal_link_changed.emit(i, otrack);
 353                                 otrack->signal_link_changed.emit(j, this);
 354                                 return true;
 355                         }
 356                 }
 357         }
 358
 359         return false;
 360 }
 361
 362 bool Track::break_link(unsigned i)
 363 {
 364         if(i>=links.size())
 365                 throw out_of_range("Track::break_link");
 366
 367         Track *other = links[i];
 368         if(!other)
 369                 return false;
 370
 371         links[i] = 0;
 372         if(!other->break_link(*this))
 373         {
 374                 /* If the call doesn't succeed, it means that the other track already
 375                 broke the link and is calling us right now.  Recreate blocks in the inner
 376                 call so it occurs before any signals are emitted. */
 377                 layout.create_blocks(*this);
 378         }
 379
 380         signal_link_changed.emit(i, 0);
 381
 382         return true;
 383 }
 384
 385 void Track::add_attachment(TrackAttachment &a)
 386 {
 387         if(find(attachments.begin(), attachments.end(), &a)!=attachments.end())
 388                 throw key_error(&a);
 389         attachments.push_back(&a);
 390 }
 391
 392 void Track::remove_attachment(TrackAttachment &a)
 393 {
 394         AttachmentList::iterator i = find(attachments.begin(), attachments.end(), &a);
 395         if(i==attachments.end())
 396                 throw key_error(&a);
 397         attachments.erase(i);
 398 }
 399
 400 Track::AttachmentList Track::get_attachments_ordered(unsigned epi) const
 401 {
 402         AttachmentList result = attachments;
 403         result.sort(AttachmentCompare(epi));
 404         return result;
 405 }
 406
 407 void Track::save(list<DataFile::Statement> &st) const
 408 {
 409         st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
 410         st.push_back((DataFile::Statement("rotation"), rotation.radians()));
 411         st.push_back((DataFile::Statement("tilt"), tilt.radians()));
 412         if(turnout_addr)
 413                 st.push_back((DataFile::Statement("turnout_address"), turnout_addr));
 414         if(sensor_addr)
 415                 st.push_back((DataFile::Statement("sensor_address"), sensor_addr));
 416         if(flex)
 417                 st.push_back((DataFile::Statement("flex"), true));
 418 }
 419
 420 void Track::save_dynamic(list<DataFile::Statement> &st) const
 421 {
 422         if(turnout_addr)
 423                 st.push_back((DataFile::Statement("path"), active_path));
 424 }
 425
 426 void Track::turnout_event(unsigned id, unsigned state)
 427 {
 428         if(id==turnout_id)
 429         {
 430                 active_path = state;
 431                 path_changing = false;
 432                 signal_path_changed.emit(active_path);
 433         }
 434 }
 435
 436 void Track::turnout_failed(unsigned id)
 437 {
 438         if(id==turnout_id)
 439         {
 440                 path_changing = false;
 441                 layout.emergency(block, "Turnout failed");
 442         }
 443 }
 444
 445
 446 Track::Loader::Loader(Track &t):
 447         DataFile::ObjectLoader<Track>(t)
 448 {
 449         add("path",       &Loader::path);
 450         add("position",   &Loader::position);
 451         add("rotation",   &Loader::rotation);
 452         add("tilt",       &Loader::tilt);
 453         add("turnout_id", &Loader::turnout_address);
 454         add("turnout_address", &Loader::turnout_address);
 455         add("sensor_id",  &Loader::sensor_address);
 456         add("sensor_address",  &Loader::sensor_address);
 457         add("flex",       &Track::flex);
 458
 459         // deprecated
 460         add("slope",      &Loader::slope);
 461 }
 462
 463 void Track::Loader::path(unsigned p)
 464 {
 465         obj.set_active_path(p);
 466         if(obj.path_changing)
 467         {
 468                 obj.active_path = p;
 469                 obj.signal_path_changed.emit(p);
 470         }
 471 }
 472
 473 void Track::Loader::position(float x, float y, float z)
 474 {
 475         obj.set_position(Vector(x, y, z));
 476 }
 477
 478 void Track::Loader::rotation(float r)
 479 {
 480         obj.set_rotation(Angle::from_radians(r));
 481 }
 482
 483 void Track::Loader::sensor_address(unsigned addr)
 484 {
 485         obj.set_sensor_address(addr);
 486 }
 487
 488 void Track::Loader::slope(float s)
 489 {
 490         obj.set_tilt(Geometry::atan(s/obj.type.get_path_length(0)));
 491 }
 492
 493 void Track::Loader::tilt(float t)
 494 {
 495         obj.set_tilt(Angle::from_radians(t));
 496 }
 497
 498 void Track::Loader::turnout_address(unsigned addr)
 499 {
 500         obj.set_turnout_address(addr);
 501 }
 502
 503 } // namespace R2C2