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