track.cpp

   1 /* $Id$
   2
   3 This file is part of R²C²
   4 Copyright © 2006-2010  Mikkosoft Productions, Mikko Rasa
   5 Distributed under the GPL
   6 */
   7
   8 #include <cmath>
   9 #include "block.h"
  10 #include "catalogue.h"
  11 #include "driver.h"
  12 #include "layout.h"
  13 #include "track.h"
  14 #include "tracktype.h"
  15
  16 using namespace std;
  17 using namespace Msp;
  18
  19 namespace R2C2 {
  20
  21 Track::Track(Layout &l, const TrackType &t):
  22         layout(l),
  23         type(t),
  24         block(0),
  25         rot(0),
  26         slope(0),
  27         flex(false),
  28         turnout_id(type.is_turnout() ? layout.allocate_turnout_id(type.is_double_address()) : 0),
  29         sensor_id(0),
  30         links(type.get_endpoints().size()),
  31         active_path(0)
  32 {
  33         layout.add_track(*this);
  34
  35         if(layout.has_driver())
  36                 layout.get_driver().signal_turnout.connect(sigc::mem_fun(this, &Track::turnout_event));
  37
  38         for(unsigned paths = type.get_paths(); !(paths&1); ++active_path, paths>>=1) ;
  39 }
  40
  41 Track::~Track()
  42 {
  43         break_links();
  44         layout.remove_track(*this);
  45 }
  46
  47 void Track::set_block(Block *b)
  48 {
  49         if(b && !b->has_track(*this))
  50                 throw InvalidParameterValue("Track is not in the Block");
  51         if(!b && block && block->has_track(*this))
  52                 throw InvalidState("Track is still in a Block");
  53
  54         block = b;
  55 }
  56
  57 Block &Track::get_block() const
  58 {
  59         if(!block)
  60                 throw InvalidState("No Block");
  61
  62         return *block;
  63 }
  64
  65 void Track::set_position(const Point &p)
  66 {
  67         pos = p;
  68 }
  69
  70 void Track::set_rotation(float r)
  71 {
  72         rot = r;
  73         while(rot<0)
  74                 rot += M_PI*2;
  75         while(rot>M_PI*2)
  76                 rot -= M_PI*2;
  77 }
  78
  79 void Track::set_slope(float s)
  80 {
  81         if(links.size()!=2)
  82                 return;
  83
  84         slope = s;
  85 }
  86
  87 void Track::set_flex(bool f)
  88 {
  89         flex = f;
  90 }
  91
  92 void Track::check_slope()
  93 {
  94         if(links.size()!=2)
  95                 return;
  96
  97         if(links[0] && links[1])
  98         {
  99                 Point epp0 = links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
 100                 Point epp1 = links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
 101                 pos.z = epp0.z;
 102                 slope = epp1.z-pos.z;
 103         }
 104         else
 105         {
 106                 slope = 0;
 107                 if(links[0])
 108                 {
 109                         Point epp = links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
 110                         pos.z = epp.z;
 111                 }
 112                 else if(links[1])
 113                 {
 114                         Point epp = links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
 115                         pos.z = epp.z;
 116                 }
 117         }
 118 }
 119
 120 void Track::set_turnout_id(unsigned i)
 121 {
 122         if(!type.is_turnout())
 123                 throw InvalidState("Not a turnout");
 124
 125         turnout_id = i;
 126         layout.create_blocks(*this);
 127         layout.update_routes();
 128         if(layout.has_driver() && turnout_id)
 129         {
 130                 layout.get_driver().add_turnout(turnout_id);
 131                 if(type.is_double_address())
 132                         layout.get_driver().add_turnout(turnout_id+1);
 133         }
 134 }
 135
 136 void Track::set_sensor_id(unsigned i)
 137 {
 138         if(type.is_turnout())
 139                 throw InvalidState("Can't set sensor on a turnout");
 140
 141         sensor_id = i;
 142         layout.create_blocks(*this);
 143         if(layout.has_driver() && sensor_id)
 144                 layout.get_driver().add_sensor(sensor_id);
 145 }
 146
 147 void Track::set_active_path(unsigned p)
 148 {
 149         if(!turnout_id)
 150                 throw InvalidState("Not a turnout");
 151         if(!(type.get_paths()&(1<<p)))
 152                 throw InvalidParameterValue("Invalid path");
 153
 154         layout.get_driver().set_turnout(turnout_id, p&1);
 155         if(type.is_double_address())
 156                 layout.get_driver().set_turnout(turnout_id+1, p&2);
 157         else if(type.get_n_paths()>2)
 158                 active_path = (active_path&1) | (p&2);
 159 }
 160
 161 int Track::get_endpoint_by_link(Track &other) const
 162 {
 163         for(unsigned i=0; i<links.size(); ++i)
 164                 if(links[i]==&other)
 165                         return i;
 166
 167         return -1;
 168 }
 169
 170 Point Track::get_endpoint_position(unsigned epi) const
 171 {
 172         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
 173         if(epi>=eps.size())
 174                 throw InvalidParameterValue("TrackType::Endpoint index out of range");
 175
 176         const TrackType::Endpoint &ep = eps[epi];
 177
 178         float c = cos(rot);
 179         float s = sin(rot);
 180
 181         Point p(pos.x+c*ep.pos.x-s*ep.pos.y, pos.y+s*ep.pos.x+c*ep.pos.y, pos.z);
 182         if(eps.size()==2 && epi==1)
 183                 p.z += slope;
 184         return p;
 185 }
 186
 187 float Track::get_endpoint_direction(unsigned epi) const
 188 {
 189         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
 190         if(epi>=eps.size())
 191                 throw InvalidParameterValue("TrackType::Endpoint index out of range");
 192
 193         const TrackType::Endpoint &ep = eps[epi];
 194
 195         return rot+ep.dir;
 196 }
 197
 198 bool Track::snap_to(Track &other, bool link, float limit)
 199 {
 200         if(!limit || link)
 201         {
 202                 limit = layout.get_catalogue().get_gauge();
 203                 if(link && !flex && !other.get_flex())
 204                         limit /= 10;
 205         }
 206         limit *= limit;
 207
 208         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
 209         const vector<TrackType::Endpoint> &other_eps = other.get_type().get_endpoints();
 210
 211         for(unsigned i=0; i<eps.size(); ++i)
 212         {
 213                 Point epp = get_endpoint_position(i);
 214
 215                 for(unsigned j=0; j<other_eps.size(); ++j)
 216                 {
 217                         if(other.get_link(j))
 218                                 continue;
 219
 220                         Point epp2 = other.get_endpoint_position(j);
 221                         float dx = epp2.x-epp.x;
 222                         float dy = epp2.y-epp.y;
 223                         float dz = epp2.z-epp.z;
 224                         if(dx*dx+dy*dy<limit && dz*dz<limit)
 225                         {
 226                                 if(!link || (!flex && !other.get_flex()))
 227                                 {
 228                                         set_rotation(other.rot+other_eps[j].dir-eps[i].dir+M_PI);
 229                                         Point p(epp2.x-(eps[i].pos.x*cos(rot)-eps[i].pos.y*sin(rot)),
 230                                                 epp2.y-(eps[i].pos.y*cos(rot)+eps[i].pos.x*sin(rot)),
 231                                                 epp2.z);
 232                                         if(eps.size()==2 && i==1)
 233                                                 p.z -= slope;
 234                                         set_position(p);
 235                                 }
 236
 237                                 if(link)
 238                                 {
 239                                         if(links[i])
 240                                                 break_link(*links[i]);
 241                                         links[i] = &other;
 242                                         other.links[j] = this;
 243                                         layout.create_blocks(*this);
 244                                 }
 245
 246                                 return true;
 247                         }
 248                 }
 249         }
 250
 251         return false;
 252 }
 253
 254 bool Track::snap(Point &pt, float &d) const
 255 {
 256         const vector<TrackType::Endpoint> &eps = type.get_endpoints();
 257
 258         for(unsigned i=0; i<eps.size(); ++i)
 259         {
 260                 Point epp = get_endpoint_position(i);
 261                 float dx = pt.x-epp.x;
 262                 float dy = pt.y-epp.y;
 263                 if(dx*dx+dy*dy<1e-4)
 264                 {
 265                         pt = epp;
 266                         d = rot+eps[i].dir;
 267                         return true;
 268                 }
 269         }
 270
 271         return false;
 272 }
 273
 274 void Track::break_link(Track &trk)
 275 {
 276         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 277                 if(*i==&trk)
 278                 {
 279                         *i = 0;
 280                         trk.break_link(*this);
 281                         // XXX Creates the blocks twice
 282                         layout.create_blocks(*this);
 283                         return;
 284                 }
 285 }
 286
 287 void Track::break_links()
 288 {
 289         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 290                 if(Track *trk=*i)
 291                 {
 292                         *i = 0;
 293                         trk->break_link(*this);
 294                 }
 295 }
 296
 297 Track *Track::get_link(unsigned i) const
 298 {
 299         if(i>links.size())
 300                 throw InvalidParameterValue("Link index out of range");
 301
 302         return links[i];
 303 }
 304
 305 TrackPoint Track::get_point(unsigned epi, unsigned path, float d) const
 306 {
 307         TrackPoint p = type.get_point(epi, path, d);
 308         float c = cos(rot);
 309         float s = sin(rot);
 310
 311         p.pos = Point(pos.x+c*p.pos.x-s*p.pos.y, pos.y+s*p.pos.x+c*p.pos.y, pos.z);
 312         p.dir += rot;
 313         if(type.get_endpoints().size()==2)
 314         {
 315                 float len = type.get_path_length(path);
 316                 float grade = slope/len;
 317                 if(epi==0)
 318                 {
 319                         p.pos.z += grade*d;
 320                         p.grade = grade;
 321                 }
 322                 else
 323                 {
 324                         p.pos.z += slope-grade*d;
 325                         p.grade = -grade;
 326                 }
 327         }
 328
 329         return p;
 330 }
 331
 332 TrackPoint Track::get_point(unsigned epi, float d) const
 333 {
 334         return get_point(epi, active_path, d);
 335 }
 336
 337 void Track::save(list<DataFile::Statement> &st) const
 338 {
 339         st.push_back((DataFile::Statement("position"), pos.x, pos.y, pos.z));
 340         st.push_back((DataFile::Statement("rotation"), rot));
 341         st.push_back((DataFile::Statement("slope"), slope));
 342         if(turnout_id)
 343                 st.push_back((DataFile::Statement("turnout_id"), turnout_id));
 344         if(sensor_id)
 345                 st.push_back((DataFile::Statement("sensor_id"), sensor_id));
 346         if(flex)
 347                 st.push_back((DataFile::Statement("flex"), true));
 348 }
 349
 350 void Track::turnout_event(unsigned addr, bool state)
 351 {
 352         if(!turnout_id)
 353                 return;
 354
 355         if(addr==turnout_id)
 356                 active_path = (active_path&2) | (state ? 1 : 0);
 357         else if(type.is_double_address() && addr==turnout_id+1)
 358                 active_path = (active_path&1) | (state ? 2 : 0);
 359         else
 360                 return;
 361
 362         signal_path_changed.emit(active_path);
 363 }
 364
 365
 366 Track::Loader::Loader(Track &t):
 367         DataFile::BasicLoader<Track>(t)
 368 {
 369         add("position",   &Loader::position);
 370         add("rotation",   &Track::rot);
 371         add("slope",      &Track::slope);
 372         add("turnout_id", &Loader::turnout_id);
 373         add("sensor_id",  &Loader::sensor_id);
 374         add("flex",       &Track::flex);
 375 }
 376
 377 void Track::Loader::position(float x, float y, float z)
 378 {
 379         obj.pos = Point(x, y, z);
 380 }
 381
 382 void Track::Loader::sensor_id(unsigned id)
 383 {
 384         obj.set_sensor_id(id);
 385 }
 386
 387 void Track::Loader::turnout_id(unsigned id)
 388 {
 389         obj.set_turnout_id(id);
 390 }
 391
 392 } // namespace R2C2