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