source/libr2c2/track.cpp

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