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