source/libr2c2/tracktype.cpp

   1 #include <cmath>
   2 #include "tracktype.h"
   3
   4 using namespace std;
   5 using namespace Msp;
   6
   7 namespace R2C2 {
   8
   9 TrackType::TrackType(const ArticleNumber &an):
  10         ObjectType(an),
  11         state_bits(0),
  12         autofit_preference(1)
  13 { }
  14
  15 float TrackType::get_total_length() const
  16 {
  17         return get_path_length(-1);
  18 }
  19
  20 float TrackType::get_path_length(int p) const
  21 {
  22         float len = 0;
  23         for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
  24                 if(p<0 || i->get_path()==static_cast<unsigned>(p))
  25                         len += i->get_length();
  26         return len;
  27 }
  28
  29 unsigned TrackType::get_paths() const
  30 {
  31         unsigned mask = 0;
  32         for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
  33                 mask |= 1<<i->get_path();
  34         return mask;
  35 }
  36
  37 unsigned TrackType::get_n_paths() const
  38 {
  39         unsigned n = 0;
  40         for(unsigned mask = get_paths(); mask; ++n)
  41                 mask &= mask-1;
  42         return n;
  43 }
  44
  45 bool TrackType::is_turnout() const
  46 {
  47         return endpoints.size()>2;
  48 }
  49
  50 bool TrackType::is_dead_end() const
  51 {
  52         return endpoints.size()<2;
  53 }
  54
  55 const TrackType::Endpoint &TrackType::get_endpoint(unsigned i) const
  56 {
  57         if(i>=endpoints.size())
  58                 throw out_of_range("TrackType::get_endpoint");
  59
  60         return endpoints[i];
  61 }
  62
  63 TrackPoint TrackType::get_point(unsigned epi, unsigned path, float d) const
  64 {
  65         if(epi>=endpoints.size())
  66                 throw out_of_range("TrackType::get_point");
  67
  68         const TrackPart *part = 0;
  69         unsigned part_ep = 0;
  70         for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
  71         {
  72                 if(endpoints[epi].has_path(path) && i->get_path()!=path)
  73                         continue;
  74
  75                 unsigned n_part_eps = (i->is_dead_end() ? 1 : 2);
  76                 for(unsigned j=0; j<n_part_eps; ++j)
  77                 {
  78                         TrackPoint p = i->get_point(j ? i->get_length() : 0);
  79                         float dx = p.pos.x-endpoints[epi].pos.x;
  80                         float dy = p.pos.y-endpoints[epi].pos.y;
  81                         if(dx*dx+dy*dy<1e-6)
  82                         {
  83                                 part = &*i;
  84                                 part_ep = j;
  85                         }
  86                 }
  87         }
  88
  89         if(!part)
  90                 throw logic_error("internal error (endpoint does not match any part)");
  91
  92         while(1)
  93         {
  94                 float plen = part->get_length();
  95                 if(d<=plen)
  96                 {
  97                         if(part_ep==1)
  98                                 d = plen-d;
  99                         TrackPoint p = part->get_point(d);
 100                         if(part_ep==1)
 101                                 p.dir += M_PI;
 102                         return p;
 103                 }
 104                 else
 105                 {
 106                         d -= plen;
 107                         TrackPart *next = part->get_link(1-part_ep);
 108                         if(!next)
 109                                 throw invalid_argument("TrackType::get_point");
 110                         part_ep = (next->get_link(0)==part ? 0 : 1);
 111                         part = next;
 112                 }
 113         }
 114 }
 115
 116 TrackPoint TrackType::get_nearest_point(const Vector &p) const
 117 {
 118         TrackPoint result;
 119         float dist = -1;
 120
 121         for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
 122         {
 123                 TrackPoint n = i->get_nearest_point(p);
 124                 float d = distance(n.pos, p);
 125                 if(d<dist || dist<0)
 126                 {
 127                         result = n;
 128                         dist = d;
 129                 }
 130         }
 131
 132         return result;
 133 }
 134
 135 bool TrackType::collide_ray(const Vector &start, const Vector &dir, float width) const
 136 {
 137         for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
 138                 if(i->collide_ray(start, dir, width))
 139                         return true;
 140
 141         return false;
 142 }
 143
 144 void TrackType::collect_endpoints()
 145 {
 146         endpoints.clear();
 147
 148         for(vector<TrackPart>::iterator i=parts.begin(); i!=parts.end(); ++i)
 149         {
 150                 for(vector<TrackPart>::iterator j=i; ++j!=parts.end();)
 151                         i->check_link(*j);
 152
 153                 unsigned n_part_eps = (i->is_dead_end() ? 1 : 2);
 154                 for(unsigned j=0; j<n_part_eps; ++j)
 155                         if(!i->get_link(j))
 156                         {
 157                                 TrackPoint p = i->get_point(j ? i->get_length() : 0);
 158                                 if(j==0)
 159                                         p.dir += M_PI;
 160
 161                                 bool found = false;
 162                                 for(vector<Endpoint>::iterator k=endpoints.begin(); k!=endpoints.end(); ++k)
 163                                 {
 164                                         float dx = k->pos.x-p.pos.x;
 165                                         float dy = k->pos.y-p.pos.y;
 166
 167                                         float da = k->dir-p.dir;
 168                                         while(da>M_PI)
 169                                                 da -= M_PI*2;
 170                                         while(da<-M_PI)
 171                                                 da += M_PI*2;
 172
 173                                         if(dx*dx+dy*dy<1e-6 && da>-0.01 && da<0.01)
 174                                         {
 175                                                 k->paths |= 1<<i->get_path();
 176                                                 found = true;
 177                                                 break;
 178                                         }
 179                                 }
 180
 181                                 if(!found)
 182                                         endpoints.push_back(Endpoint(p.pos.x, p.pos.y, p.dir, 1<<i->get_path()));
 183                         }
 184         }
 185 }
 186
 187 TrackType::Endpoint::Endpoint(float x, float y, float d, unsigned p):
 188         pos(x, y),
 189         dir(d),
 190         paths(p)
 191 { }
 192
 193
 194 TrackType::Loader::Loader(TrackType &t):
 195         DataFile::DerivedObjectLoader<TrackType, ObjectType::Loader>(t),
 196         state_bits_set(false)
 197 {
 198         add("autofit_preference", &TrackType::autofit_preference);
 199         add("object",      &TrackType::object);
 200         add("state_bits",  &Loader::state_bits);
 201         add("part",        &Loader::part);
 202 }
 203
 204 void TrackType::Loader::finish()
 205 {
 206         obj.collect_endpoints();
 207 }
 208
 209 void TrackType::Loader::part()
 210 {
 211         TrackPart p;
 212         load_sub(p);
 213         obj.parts.push_back(p);
 214         if(!state_bits_set && p.get_path())
 215                 while(p.get_path()>=(1U<<obj.state_bits))
 216                         ++obj.state_bits;
 217 }
 218
 219 void TrackType::Loader::state_bits(unsigned b)
 220 {
 221         obj.state_bits = b;
 222         state_bits_set = true;
 223 }
 224
 225 } // namespace R2C2