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