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