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                                 otrack->break_link(j);
 336                                 links[i] = otrack;
 337                                 otrack->links[j] = this;
 338                                 check_slope();
 339                                 layout.create_blocks(*this);
 340
 341                                 signal_link_changed.emit(i, otrack);
 342                                 otrack->signal_link_changed.emit(j, this);
 343                                 return true;
 344                         }
 345                 }
 346         }
 347
 348         return false;
 349 }
 350
 351 bool Track::break_link(unsigned i)
 352 {
 353         if(i>=links.size())
 354                 throw out_of_range("Track::break_link");
 355
 356         Track *other = links[i];
 357         if(!other)
 358                 return false;
 359
 360         links[i] = 0;
 361         if(!other->break_link(*this))
 362         {
 363                 /* If the call doesn't succeed, it means that the other track already
 364                 broke the link and is calling us right now.  Recreate blocks in the inner
 365                 call so it occurs before any signals are emitted. */
 366                 layout.create_blocks(*this);
 367         }
 368
 369         signal_link_changed.emit(i, 0);
 370
 371         return true;
 372 }
 373
 374 void Track::add_attachment(TrackAttachment &a)
 375 {
 376         if(find(attachments.begin(), attachments.end(), &a)!=attachments.end())
 377                 throw key_error(&a);
 378         attachments.push_back(&a);
 379 }
 380
 381 void Track::remove_attachment(TrackAttachment &a)
 382 {
 383         AttachmentList::iterator i = find(attachments.begin(), attachments.end(), &a);
 384         if(i==attachments.end())
 385                 throw key_error(&a);
 386         attachments.erase(i);
 387 }
 388
 389 Track::AttachmentList Track::get_attachments_ordered(unsigned epi) const
 390 {
 391         AttachmentList result = attachments;
 392         result.sort(AttachmentCompare(epi));
 393         return result;
 394 }
 395
 396 void Track::save(list<DataFile::Statement> &st) const
 397 {
 398         st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
 399         st.push_back((DataFile::Statement("rotation"), rotation.radians()));
 400         st.push_back((DataFile::Statement("tilt"), tilt.radians()));
 401         if(turnout_id)
 402                 st.push_back((DataFile::Statement("turnout_id"), turnout_id));
 403         if(sensor_id)
 404                 st.push_back((DataFile::Statement("sensor_id"), sensor_id));
 405         if(flex)
 406                 st.push_back((DataFile::Statement("flex"), true));
 407 }
 408
 409 void Track::turnout_event(unsigned addr, unsigned state)
 410 {
 411         if(addr==turnout_id)
 412         {
 413                 active_path = state;
 414                 path_changing = false;
 415                 signal_path_changed.emit(active_path);
 416         }
 417 }
 418
 419
 420 Track::Loader::Loader(Track &t):
 421         DataFile::ObjectLoader<Track>(t)
 422 {
 423         add("position",   &Loader::position);
 424         add("rotation",   &Loader::rotation);
 425         add("tilt",       &Loader::tilt);
 426         add("turnout_id", &Loader::turnout_id);
 427         add("sensor_id",  &Loader::sensor_id);
 428         add("flex",       &Track::flex);
 429
 430         // deprecated
 431         add("slope",      &Loader::slope);
 432 }
 433
 434 void Track::Loader::position(float x, float y, float z)
 435 {
 436         obj.set_position(Vector(x, y, z));
 437 }
 438
 439 void Track::Loader::rotation(float r)
 440 {
 441         obj.set_rotation(Angle::from_radians(r));
 442 }
 443
 444 void Track::Loader::sensor_id(unsigned id)
 445 {
 446         obj.set_sensor_id(id);
 447 }
 448
 449 void Track::Loader::slope(float s)
 450 {
 451         obj.set_tilt(Geometry::atan(s/obj.type.get_path_length(0)));
 452 }
 453
 454 void Track::Loader::tilt(float t)
 455 {
 456         obj.set_tilt(Angle::from_radians(t));
 457 }
 458
 459 void Track::Loader::turnout_id(unsigned id)
 460 {
 461         obj.set_turnout_id(id);
 462 }
 463
 464 } // namespace R2C2