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