source/libr2c2/trackpart.cpp

   1 #include <cmath>
   2 #include "trackpart.h"
   3
   4 using namespace std;
   5 using namespace Msp;
   6
   7 namespace R2C2 {
   8
   9 TrackPart::TrackPart():
  10         dir(0),
  11         length(0),
  12         radius(0),
  13         path(0),
  14         dead_end(false)
  15 {
  16         links[0] = 0;
  17         links[1] = 0;
  18 }
  19
  20 float TrackPart::get_length() const
  21 {
  22         if(radius)
  23                 return abs(radius)*length;
  24         else
  25                 return length;
  26 }
  27
  28 TrackPoint TrackPart::get_point(float d) const
  29 {
  30         TrackPoint result;
  31
  32         if(radius)
  33         {
  34                 float a = d/radius;
  35                 float c = cos(a);
  36                 float s = sin(a);
  37                 float rx = radius*sin(dir);
  38                 float ry = -radius*cos(dir);
  39                 result.pos = Vector(pos.x+c*rx-s*ry-rx, pos.y+c*ry+s*rx-ry);
  40                 result.dir = dir+a;
  41         }
  42         else
  43         {
  44                 result.pos = Vector(pos.x+cos(dir)*d, pos.y+sin(dir)*d);
  45                 result.dir = dir;
  46         }
  47
  48         return result;
  49 }
  50
  51 TrackPoint TrackPart::get_nearest_point(const Vector &p) const
  52 {
  53         TrackPoint tp;
  54         float c = cos(dir);
  55         float s = sin(dir);
  56         if(radius)
  57         {
  58                 Vector v(p.x-pos.x+radius*s, p.y-pos.y-radius*c);
  59                 float a = atan2(v.y, v.x)+M_PI/2-dir;
  60                 if(radius<0)
  61                         a = M_PI-a;
  62                 while(a<length/2-M_PI)
  63                         a += M_PI*2;
  64                 while(a>length/2+M_PI)
  65                         a -= M_PI*2;
  66                 a = min(max(a, 0.0f), length);
  67                 if(radius<0)
  68                         a = -a;
  69                 tp.pos = Vector(pos.x+radius*(sin(dir+a)-s), pos.y+radius*(c-cos(dir+a)));
  70                 tp.dir = dir+a;
  71         }
  72         else
  73         {
  74                 Vector v(p.x-pos.x, p.y-pos.y);
  75                 float d = min(max(c*v.x+s*v.y, 0.0f), length);
  76                 tp.pos = Vector(pos.x+c*d, pos.y+s*d);
  77                 tp.dir = dir;
  78         }
  79         return tp;
  80 }
  81
  82 void TrackPart::check_link(TrackPart &other)
  83 {
  84         unsigned n_eps = (dead_end ? 1 : 2);
  85         unsigned n_other_eps = (other.is_dead_end() ? 1 : 2);
  86         for(unsigned i=0; i<n_eps; ++i)
  87         {
  88                 TrackPoint p1 = get_point(i ? get_length() : 0);
  89                 for(unsigned j=0; j<n_other_eps; ++j)
  90                 {
  91                         TrackPoint p2 = other.get_point(j ? other.get_length() : 0);
  92
  93                         float dx = p2.pos.x-p1.pos.x;
  94                         float dy = p2.pos.y-p1.pos.y;
  95
  96                         float da = p2.dir-p1.dir+M_PI*((i+j+1)%2);
  97                         while(da>M_PI)
  98                                 da -= M_PI*2;
  99                         while(da<-M_PI)
 100                                 da += M_PI*2;
 101
 102                         if(dx*dx+dy*dy<1e-6 && da>=-0.01 && da<=0.01)
 103                         {
 104                                 links[i] = &other;
 105                                 other.links[j] = this;
 106                                 return;
 107                         }
 108                 }
 109         }
 110 }
 111
 112 TrackPart *TrackPart::get_link(unsigned i) const
 113 {
 114         if(i>=2)
 115                 throw out_of_range("TrackPart::get_link");
 116         return links[i];
 117 }
 118
 119 bool TrackPart::collide_ray(const Vector &start, const Vector &ray, float width) const
 120 {
 121         Vector local_start(start.x-pos.x, start.y-pos.y, start.z);
 122         float c = cos(dir);
 123         float s = sin(dir);
 124         local_start = Vector(c*local_start.x+s*local_start.y, c*local_start.y-s*local_start.x, local_start.z);
 125         Vector local_ray(c*ray.x+s*ray.y, c*ray.y-s*ray.x, ray.z);
 126
 127         float d = -local_start.z/local_ray.z;
 128         if(d<0)
 129                 return false;
 130
 131         Vector base(local_start.x+d*local_ray.x, local_start.y+d*local_ray.y);
 132
 133         if(radius)
 134         {
 135                 base.y -= radius;
 136                 if(radius<0)
 137                         base.y = -base.y;
 138                 float r = sqrt(base.x*base.x+base.y*base.y)-abs(radius);
 139                 float a = atan2(base.x, -base.y);
 140                 return (a>=0 && a<=length && r>=-width/2 && r<=width/2);
 141         }
 142         else
 143                 return (base.x>=0 && base.x<=length && base.y>=-width/2 && base.y<=width/2);
 144 }
 145
 146
 147 TrackPart::Loader::Loader(TrackPart &p):
 148         Msp::DataFile::ObjectLoader<TrackPart>(p)
 149 {
 150         add("start",    &Loader::start);
 151         add("length",   &TrackPart::length);
 152         add("radius",   &TrackPart::radius);
 153         add("path",     &TrackPart::path);
 154         add("dead_end", &TrackPart::dead_end);
 155 }
 156
 157 void TrackPart::Loader::finish()
 158 {
 159         if(obj.radius)
 160         {
 161                 obj.length *= M_PI/180;
 162                 obj.radius /= 1000;
 163         }
 164         else
 165                 obj.length /= 1000;
 166
 167         obj.pos.x /= 1000;
 168         obj.pos.y /= 1000;
 169         obj.dir *= M_PI/180;
 170 }
 171
 172 void TrackPart::Loader::start(float x, float y, float d)
 173 {
 174         obj.pos = Vector(x, y);
 175         obj.dir = d;
 176 }
 177
 178 } // namespace R2C2