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