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         other->break_link(*this);
 348         // XXX Creates the blocks twice, because the other track calls this too
 349         layout.create_blocks(*this);
 350         signal_link_changed.emit(i, 0);
 351
 352         return true;
 353 }
 354
 355 void Track::add_attachment(TrackAttachment &a)
 356 {
 357         if(find(attachments.begin(), attachments.end(), &a)!=attachments.end())
 358                 throw key_error(&a);
 359         attachments.push_back(&a);
 360 }
 361
 362 void Track::remove_attachment(TrackAttachment &a)
 363 {
 364         AttachmentList::iterator i = find(attachments.begin(), attachments.end(), &a);
 365         if(i==attachments.end())
 366                 throw key_error(&a);
 367         attachments.erase(i);
 368 }
 369
 370 Track::AttachmentList Track::get_attachments_ordered(unsigned epi) const
 371 {
 372         AttachmentList result = attachments;
 373         result.sort(AttachmentCompare(epi));
 374         return result;
 375 }
 376
 377 void Track::save(list<DataFile::Statement> &st) const
 378 {
 379         st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
 380         st.push_back((DataFile::Statement("rotation"), rotation.radians()));
 381         st.push_back((DataFile::Statement("tilt"), tilt.radians()));
 382         if(turnout_id)
 383                 st.push_back((DataFile::Statement("turnout_id"), turnout_id));
 384         if(sensor_id)
 385                 st.push_back((DataFile::Statement("sensor_id"), sensor_id));
 386         if(flex)
 387                 st.push_back((DataFile::Statement("flex"), true));
 388 }
 389
 390 void Track::turnout_event(unsigned addr, unsigned state)
 391 {
 392         if(!turnout_id)
 393                 return;
 394
 395         if(addr==turnout_id)
 396         {
 397                 active_path = state;
 398                 path_changing = false;
 399                 signal_path_changed.emit(active_path);
 400         }
 401 }
 402
 403
 404 Track::Loader::Loader(Track &t):
 405         DataFile::ObjectLoader<Track>(t)
 406 {
 407         add("position",   &Loader::position);
 408         add("rotation",   &Loader::rotation);
 409         add("tilt",       &Loader::tilt);
 410         add("turnout_id", &Loader::turnout_id);
 411         add("sensor_id",  &Loader::sensor_id);
 412         add("flex",       &Track::flex);
 413
 414         // deprecated
 415         add("slope",      &Loader::slope);
 416 }
 417
 418 void Track::Loader::position(float x, float y, float z)
 419 {
 420         obj.set_position(Vector(x, y, z));
 421 }
 422
 423 void Track::Loader::rotation(float r)
 424 {
 425         obj.set_rotation(Angle::from_radians(r));
 426 }
 427
 428 void Track::Loader::sensor_id(unsigned id)
 429 {
 430         obj.set_sensor_id(id);
 431 }
 432
 433 void Track::Loader::slope(float s)
 434 {
 435         obj.set_tilt(Geometry::atan(s/obj.type.get_path_length(0)));
 436 }
 437
 438 void Track::Loader::tilt(float t)
 439 {
 440         obj.set_tilt(Angle::from_radians(t));
 441 }
 442
 443 void Track::Loader::turnout_id(unsigned id)
 444 {
 445         obj.set_turnout_id(id);
 446 }
 447
 448 } // namespace R2C2