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         preferred_exit(-1)
  43 {
  44         if(type.is_turnout())
  45         {
  46                 turnout_addr = layout.allocate_turnout_address();
  47
  48                 if(layout.has_driver())
  49                 {
  50                         Driver &driver = layout.get_driver();
  51                         turnout_id = driver.add_turnout(turnout_addr, type);
  52                         driver.signal_turnout.connect(sigc::mem_fun(this, &Track::turnout_event));
  53                         driver.signal_turnout_failed.connect(sigc::mem_fun(this, &Track::turnout_failed));
  54                 }
  55         }
  56
  57         layout.add(*this);
  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_preferred_exit(int e)
 192 {
 193         preferred_exit = e;
 194 }
 195
 196 void Track::set_active_path(unsigned p)
 197 {
 198         if(!type.is_turnout())
 199                 throw logic_error("not a turnout");
 200         if(!(type.get_paths()&(1<<p)))
 201                 throw invalid_argument("Track::set_active_path");
 202
 203         if(active_path==p)
 204                 return;
 205
 206         signal_path_changing(p);
 207         path_changing = true;
 208         layout.get_driver().set_turnout(turnout_id, p);
 209 }
 210
 211 float Track::get_path_length(int p) const
 212 {
 213         if(p<0)
 214                 p = active_path;
 215         return type.get_path_length(p);
 216 }
 217
 218 OrientedPoint Track::get_point(unsigned epi, unsigned path, float d) const
 219 {
 220         OrientedPoint p = type.get_point(epi, path, d);
 221
 222         p.position = position+rotated_vector(p.position, rotation);
 223         p.rotation += rotation;
 224         if(type.get_endpoints().size()==2)
 225         {
 226                 float dz = tan(tilt)*d;
 227                 if(epi==0)
 228                 {
 229                         p.position.z += dz;
 230                         p.tilt = tilt;
 231                 }
 232                 else
 233                 {
 234                         p.position.z += slope-dz;
 235                         p.tilt = -tilt;
 236                 }
 237         }
 238
 239         return p;
 240 }
 241
 242 OrientedPoint Track::get_point(unsigned epi, float d) const
 243 {
 244         return get_point(epi, active_path, d);
 245 }
 246
 247 unsigned Track::get_n_snap_nodes() const
 248 {
 249         return type.get_endpoints().size();
 250 }
 251
 252 Snap Track::get_snap_node(unsigned i) const
 253 {
 254         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
 255         if(i>=eps.size())
 256                 throw out_of_range("Track::get_snap_node");
 257
 258         Snap result;
 259         const TrackType::Endpoint &ep = eps[i];
 260
 261         result.position = position+rotated_vector(ep.pos, rotation);
 262         if(eps.size()==2 && i==1)
 263                 result.position.z += slope;
 264
 265         result.rotation = rotation+ep.dir;
 266
 267         return result;
 268 }
 269
 270 bool Track::snap(Snap &sn, float limit, SnapType what) const
 271 {
 272         if(Object::snap(sn, limit, what))
 273                 return true;
 274
 275         if(what&SNAP_SEGMENT)
 276         {
 277                 Vector local = rotated_vector(sn.position-position, -rotation);
 278
 279                 OrientedPoint np = type.get_nearest_point(local);
 280                 Vector span = local-np.position;
 281                 if(dot(span, span)<=limit*limit)
 282                 {
 283                         sn.position = position+rotated_vector(np.position, rotation);
 284                         sn.rotation = np.rotation+rotation;
 285                         return true;
 286                 }
 287         }
 288
 289         return false;
 290 }
 291
 292 SnapType Track::get_default_snap_type_to(const Object &other) const
 293 {
 294         if(dynamic_cast<const Track *>(&other))
 295                 return SNAP_NODE;
 296
 297         return NO_SNAP;
 298 }
 299
 300 unsigned Track::get_n_link_slots() const
 301 {
 302         return links.size();
 303 }
 304
 305 Track *Track::get_link(unsigned i) const
 306 {
 307         if(i>=links.size())
 308                 throw out_of_range("Track::get_link");
 309
 310         return links[i];
 311 }
 312
 313 int Track::get_link_slot(const Object &other) const
 314 {
 315         for(unsigned i=0; i<links.size(); ++i)
 316                 if(links[i]==&other)
 317                         return i;
 318
 319         return -1;
 320 }
 321
 322 bool Track::link_to(Object &other)
 323 {
 324         Track *otrack = dynamic_cast<Track *>(&other);
 325         if(!otrack)
 326                 return false;
 327
 328         float gauge_ratio = otrack->get_type().get_gauge()/type.get_gauge();
 329         if(gauge_ratio<0.99 || gauge_ratio>1.01)
 330                 return false;
 331
 332         float limit = type.get_gauge();
 333         if(!flex && !otrack->get_flex())
 334                 limit /= 10;
 335         limit *= limit;
 336
 337         unsigned nsn = get_n_snap_nodes();
 338         unsigned other_nsn = other.get_n_snap_nodes();
 339         for(unsigned i=0; i<nsn; ++i)
 340         {
 341                 Snap sn = get_snap_node(i);
 342                 for(unsigned j=0; j<other_nsn; ++j)
 343                 {
 344                         Snap osn = other.get_snap_node(j);
 345                         Vector span = osn.position-sn.position;
 346                         Angle da = wrap_balanced(osn.rotation-sn.rotation-Angle::half_turn());
 347
 348                         if(dot(span, span)<limit && abs(da).radians()<0.01)
 349                         {
 350                                 break_link(i);
 351                                 otrack->break_link(j);
 352                                 links[i] = otrack;
 353                                 otrack->links[j] = this;
 354                                 check_slope();
 355                                 layout.create_blocks(*this);
 356
 357                                 signal_link_changed.emit(i, otrack);
 358                                 otrack->signal_link_changed.emit(j, this);
 359                                 return true;
 360                         }
 361                 }
 362         }
 363
 364         return false;
 365 }
 366
 367 bool Track::break_link(unsigned i)
 368 {
 369         if(i>=links.size())
 370                 throw out_of_range("Track::break_link");
 371
 372         Track *other = links[i];
 373         if(!other)
 374                 return false;
 375
 376         links[i] = 0;
 377         if(!other->break_link(*this))
 378         {
 379                 /* If the call doesn't succeed, it means that the other track already
 380                 broke the link and is calling us right now.  Recreate blocks in the inner
 381                 call so it occurs before any signals are emitted. */
 382                 layout.create_blocks(*this);
 383         }
 384
 385         signal_link_changed.emit(i, 0);
 386
 387         return true;
 388 }
 389
 390 void Track::add_attachment(TrackAttachment &a)
 391 {
 392         if(find(attachments.begin(), attachments.end(), &a)!=attachments.end())
 393                 throw key_error(&a);
 394         attachments.push_back(&a);
 395 }
 396
 397 void Track::remove_attachment(TrackAttachment &a)
 398 {
 399         AttachmentList::iterator i = find(attachments.begin(), attachments.end(), &a);
 400         if(i==attachments.end())
 401                 throw key_error(&a);
 402         attachments.erase(i);
 403 }
 404
 405 Track::AttachmentList Track::get_attachments_ordered(unsigned epi) const
 406 {
 407         AttachmentList result = attachments;
 408         result.sort(AttachmentCompare(epi));
 409         return result;
 410 }
 411
 412 void Track::save(list<DataFile::Statement> &st) const
 413 {
 414         st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
 415         st.push_back((DataFile::Statement("rotation"), rotation.radians()));
 416         st.push_back((DataFile::Statement("tilt"), tilt.radians()));
 417         if(turnout_addr)
 418                 st.push_back((DataFile::Statement("turnout_address"), turnout_addr));
 419         if(sensor_addr)
 420                 st.push_back((DataFile::Statement("sensor_address"), sensor_addr));
 421         if(flex)
 422                 st.push_back((DataFile::Statement("flex"), true));
 423 }
 424
 425 void Track::save_dynamic(list<DataFile::Statement> &st) const
 426 {
 427         if(turnout_addr)
 428                 st.push_back((DataFile::Statement("path"), active_path));
 429 }
 430
 431 void Track::turnout_event(unsigned id, unsigned state)
 432 {
 433         if(id==turnout_id)
 434         {
 435                 active_path = state;
 436                 path_changing = false;
 437                 signal_path_changed.emit(active_path);
 438         }
 439 }
 440
 441 void Track::turnout_failed(unsigned id)
 442 {
 443         if(id==turnout_id)
 444         {
 445                 path_changing = false;
 446                 layout.emergency(block, "Turnout failed");
 447         }
 448 }
 449
 450
 451 Track::Loader::Loader(Track &t):
 452         DataFile::ObjectLoader<Track>(t)
 453 {
 454         add("path",       &Loader::path);
 455         add("position",   &Loader::position);
 456         add("rotation",   &Loader::rotation);
 457         add("tilt",       &Loader::tilt);
 458         add("turnout_id", &Loader::turnout_address);
 459         add("turnout_address", &Loader::turnout_address);
 460         add("sensor_id",  &Loader::sensor_address);
 461         add("sensor_address",  &Loader::sensor_address);
 462         add("flex",       &Track::flex);
 463
 464         // deprecated
 465         add("slope",      &Loader::slope);
 466 }
 467
 468 void Track::Loader::path(unsigned p)
 469 {
 470         obj.set_active_path(p);
 471         if(obj.path_changing)
 472         {
 473                 obj.active_path = p;
 474                 obj.signal_path_changed.emit(p);
 475         }
 476 }
 477
 478 void Track::Loader::position(float x, float y, float z)
 479 {
 480         obj.set_position(Vector(x, y, z));
 481 }
 482
 483 void Track::Loader::rotation(float r)
 484 {
 485         obj.set_rotation(Angle::from_radians(r));
 486 }
 487
 488 void Track::Loader::sensor_address(unsigned addr)
 489 {
 490         obj.set_sensor_address(addr);
 491 }
 492
 493 void Track::Loader::slope(float s)
 494 {
 495         obj.set_tilt(Geometry::atan(s/obj.type.get_path_length(0)));
 496 }
 497
 498 void Track::Loader::tilt(float t)
 499 {
 500         obj.set_tilt(Angle::from_radians(t));
 501 }
 502
 503 void Track::Loader::turnout_address(unsigned addr)
 504 {
 505         obj.set_turnout_address(addr);
 506 }
 507
 508 } // namespace R2C2