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::turnout_event(unsigned id, unsigned state)
 421 {
 422         if(id==turnout_id)
 423         {
 424                 active_path = state;
 425                 path_changing = false;
 426                 signal_path_changed.emit(active_path);
 427         }
 428 }
 429
 430 void Track::turnout_failed(unsigned id)
 431 {
 432         if(id==turnout_id)
 433         {
 434                 path_changing = false;
 435                 layout.emergency(block, "Turnout failed");
 436         }
 437 }
 438
 439
 440 Track::Loader::Loader(Track &t):
 441         DataFile::ObjectLoader<Track>(t)
 442 {
 443         add("position",   &Loader::position);
 444         add("rotation",   &Loader::rotation);
 445         add("tilt",       &Loader::tilt);
 446         add("turnout_id", &Loader::turnout_address);
 447         add("turnout_address", &Loader::turnout_address);
 448         add("sensor_id",  &Loader::sensor_address);
 449         add("sensor_address",  &Loader::sensor_address);
 450         add("flex",       &Track::flex);
 451
 452         // deprecated
 453         add("slope",      &Loader::slope);
 454 }
 455
 456 void Track::Loader::position(float x, float y, float z)
 457 {
 458         obj.set_position(Vector(x, y, z));
 459 }
 460
 461 void Track::Loader::rotation(float r)
 462 {
 463         obj.set_rotation(Angle::from_radians(r));
 464 }
 465
 466 void Track::Loader::sensor_address(unsigned addr)
 467 {
 468         obj.set_sensor_address(addr);
 469 }
 470
 471 void Track::Loader::slope(float s)
 472 {
 473         obj.set_tilt(Geometry::atan(s/obj.type.get_path_length(0)));
 474 }
 475
 476 void Track::Loader::tilt(float t)
 477 {
 478         obj.set_tilt(Angle::from_radians(t));
 479 }
 480
 481 void Track::Loader::turnout_address(unsigned addr)
 482 {
 483         obj.set_turnout_address(addr);
 484 }
 485
 486 } // namespace R2C2