source/libmarklin/track.cpp

   1 /* $Id$
   2
   3 This file is part of the MSP Märklin suite
   4 Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
   5 Distributed under the GPL
   6 */
   7
   8 #include <cmath>
   9 #include "track.h"
  10 #include "tracktype.h"
  11
  12 using namespace std;
  13 using namespace Msp;
  14
  15 #include <iostream>
  16
  17 namespace Marklin {
  18
  19 Track::Track(const TrackType &t):
  20         type(t),
  21         rot(0),
  22         slope(0),
  23         flex(false),
  24         turnout_id(0),
  25         sensor_id(0),
  26         links(t.get_endpoints().size())
  27 { }
  28
  29 Track::~Track()
  30 {
  31         break_links();
  32 }
  33
  34 void Track::set_position(const Point &p)
  35 {
  36         pos=p;
  37 }
  38
  39 void Track::set_rotation(float r)
  40 {
  41         rot=r;
  42         while(rot<0)
  43                 rot+=M_PI*2;
  44         while(rot>M_PI*2)
  45                 rot-=M_PI*2;
  46 }
  47
  48 void Track::set_slope(float s)
  49 {
  50         if(links.size()!=2) return;
  51
  52         slope=s;
  53 }
  54
  55 void Track::set_flex(bool f)
  56 {
  57         flex=f;
  58 }
  59
  60 void Track::set_turnout_id(unsigned i)
  61 {
  62         turnout_id=i;
  63 }
  64
  65 void Track::set_sensor_id(unsigned i)
  66 {
  67         sensor_id=i;
  68 }
  69
  70 int Track::get_endpoint_by_link(const Track &other) const
  71 {
  72         for(unsigned i=0; i<links.size(); ++i)
  73                 if(links[i]==&other)
  74                         return i;
  75
  76         return -1;
  77 }
  78
  79 Point Track::get_endpoint_position(unsigned epi) const
  80 {
  81         const vector<Endpoint> &eps=type.get_endpoints();
  82         if(epi>=eps.size())
  83                 throw InvalidParameterValue("Endpoint index out of range");
  84
  85         const Endpoint &ep=eps[epi];
  86
  87         float c=cos(rot);
  88         float s=sin(rot);
  89
  90         Point p(pos.x+c*ep.x-s*ep.y, pos.y+s*ep.x+c*ep.y, pos.z);
  91         if(eps.size()==2 && epi==1)
  92                 p.z+=slope;
  93         return p;
  94 }
  95
  96 float Track::get_endpoint_direction(unsigned epi) const
  97 {
  98         const vector<Endpoint> &eps=type.get_endpoints();
  99         if(epi>=eps.size())
 100                 throw InvalidParameterValue("Endpoint index out of range");
 101
 102         const Endpoint &ep=eps[epi];
 103
 104         return rot+ep.dir;
 105 }
 106
 107 bool Track::snap_to(Track &other, bool link)
 108 {
 109         float limit=(link && !flex) ? 1e-6 : 1e-4;
 110         const vector<Endpoint> &eps=type.get_endpoints();
 111         const vector<Endpoint> &other_eps=other.get_type().get_endpoints();
 112
 113         for(unsigned i=0; i<eps.size(); ++i)
 114         {
 115                 Point epp=get_endpoint_position(i);
 116
 117                 for(unsigned j=0; j<other_eps.size(); ++j)
 118                 {
 119                         if(other.get_link(j))
 120                                 continue;
 121
 122                         Point epp2=other.get_endpoint_position(j);
 123                         float dx=epp2.x-epp.x;
 124                         float dy=epp2.y-epp.y;
 125                         if(dx*dx+dy*dy<limit)
 126                         {
 127                                 set_rotation(other.rot+other_eps[j].dir-eps[i].dir+M_PI);
 128                                 set_position(Point(epp2.x-(eps[i].x*cos(rot)-eps[i].y*sin(rot)), epp2.y-(eps[i].y*cos(rot)+eps[i].x*sin(rot)), epp2.z));
 129
 130                                 if(link)
 131                                 {
 132                                         if(links[i])
 133                                                 break_link(*links[i]);
 134                                         links[i]=&other;
 135                                         other.links[j]=this;
 136                                 }
 137
 138                                 return true;
 139                         }
 140                 }
 141         }
 142
 143         return false;
 144 }
 145
 146 bool Track::snap(Point &pt, float &d) const
 147 {
 148         const vector<Endpoint> &eps=type.get_endpoints();
 149
 150         for(unsigned i=0; i<eps.size(); ++i)
 151         {
 152                 Point epp=get_endpoint_position(i);
 153                 float dx=pt.x-epp.x;
 154                 float dy=pt.y-epp.y;
 155                 if(dx*dx+dy*dy<1e-4)
 156                 {
 157                         pt=epp;
 158                         d=rot+eps[i].dir;
 159                         return true;
 160                 }
 161         }
 162
 163         return false;
 164 }
 165
 166 void Track::break_link(Track &trk)
 167 {
 168         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 169                 if(*i==&trk)
 170                 {
 171                         *i=0;
 172                         trk.break_link(*this);
 173                         return;
 174                 }
 175 }
 176
 177 void Track::break_links()
 178 {
 179         for(vector<Track *>::iterator i=links.begin(); i!=links.end(); ++i)
 180                 if(Track *trk=*i)
 181                 {
 182                         *i=0;
 183                         trk->break_link(*this);
 184                 }
 185 }
 186
 187 Track *Track::get_link(unsigned i) const
 188 {
 189         if(i>links.size())
 190                 throw InvalidParameterValue("Link index out of range");
 191
 192         return links[i];
 193 }
 194
 195 void Track::check_slope()
 196 {
 197         if(links.size()!=2)
 198                 return;
 199
 200         if(links[0] && links[1])
 201         {
 202                 Point epp0=links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
 203                 Point epp1=links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
 204                 pos.z=epp0.z;
 205                 slope=epp1.z-pos.z;
 206         }
 207         else
 208         {
 209                 slope=0;
 210                 if(links[0])
 211                 {
 212                         Point epp=links[0]->get_endpoint_position(links[0]->get_endpoint_by_link(*this));
 213                         pos.z=epp.z;
 214                 }
 215                 else if(links[1])
 216                 {
 217                         Point epp=links[1]->get_endpoint_position(links[1]->get_endpoint_by_link(*this));
 218                         pos.z=epp.z;
 219                 }
 220         }
 221 }
 222
 223 int Track::traverse(unsigned i, unsigned route) const
 224 {
 225         const vector<Endpoint> &eps=type.get_endpoints();
 226         if(i>=eps.size())
 227                 throw InvalidParameterValue("Endpoint index out of range");
 228
 229         const Endpoint &ep=eps[i];
 230
 231         if(ep.routes&(1<<route))
 232         {
 233                 // Find the other endpoint for this route
 234                 for(unsigned j=0; j<eps.size(); ++j)
 235                         if((eps[j].routes&(1<<route)) && j!=i)
 236                                 return j;
 237         }
 238         else
 239         {
 240                 // Find an endpoint that's connected to this one and has the requested route
 241                 for(unsigned j=0; j<eps.size(); ++j)
 242                         if((eps[j].routes&(1<<route)) && (eps[j].routes&ep.routes))
 243                                 return j;
 244         }
 245
 246         return -1;
 247 }
 248
 249 Track *Track::copy() const
 250 {
 251         Track *trk=new Track(type);
 252         trk->set_position(pos);
 253         trk->set_rotation(rot);
 254         trk->set_slope(slope);
 255         trk->set_flex(flex);
 256
 257         return trk;
 258 }
 259
 260 /*******************
 261 ** Track::Loader
 262 */
 263
 264 Track::Loader::Loader(Track &t):
 265         track(t)
 266 {
 267         add("position",    &Loader::position);
 268         add("rotation",    &Track::rot);
 269         add("slope",       &Track::slope);
 270         add("turnout_id",  &Track::turnout_id);
 271         add("sensor_id",   &Track::sensor_id);
 272         add("flex",        &Track::flex);
 273 }
 274
 275 void Track::Loader::position(float x, float y, float z)
 276 {
 277         track.pos=Point(x, y, z);
 278 }
 279
 280 } // namespace Marklin