source/libr2c2/trackpart.cpp

   1 #include <cmath>
   2 #include <msp/geometry/box.h>
   3 #include <msp/geometry/circle.h>
   4 #include <msp/geometry/extrudedshape.h>
   5 #include <msp/geometry/halfspace.h>
   6 #include <msp/geometry/intersection.h>
   7 #include <msp/geometry/negation.h>
   8 #include <msp/geometry/transformedshape.h>
   9 #include "trackpart.h"
  10
  11 using namespace std;
  12 using namespace Msp;
  13
  14 namespace R2C2 {
  15
  16 TrackPart::TrackPart():
  17         length(0),
  18         radius(0),
  19         path(0),
  20         dead_end(false),
  21         shape(0)
  22 {
  23         links[0] = 0;
  24         links[1] = 0;
  25 }
  26
  27 TrackPart::~TrackPart()
  28 {
  29         delete shape;
  30 }
  31
  32 float TrackPart::get_length() const
  33 {
  34         if(radius)
  35                 return abs(radius)*length;
  36         else
  37                 return length;
  38 }
  39
  40 OrientedPoint TrackPart::get_point(float d) const
  41 {
  42         OrientedPoint result;
  43
  44         Transform dir_trans = Transform::rotation(dir, Vector(0, 0, 1));
  45         if(radius)
  46         {
  47                 Angle a = Angle::from_radians(d/radius);
  48                 Vector r = dir_trans.transform(Vector(0, -radius, 0));
  49                 result.position = pos-r+Transform::rotation(a, Vector(0, 0, 1)).transform(r);
  50                 result.rotation = dir+a;
  51         }
  52         else
  53         {
  54                 result.position = pos+dir_trans.transform(Vector(d, 0, 0));
  55                 result.rotation = dir;
  56         }
  57
  58         return result;
  59 }
  60
  61 OrientedPoint TrackPart::get_nearest_point(const Vector &p) const
  62 {
  63         OrientedPoint result;
  64         Transform dir_trans = Transform::rotation(dir, Vector(0, 0, 1));
  65         if(radius)
  66         {
  67                 Vector r = dir_trans.transform(Vector(0, -radius, 0));
  68                 Vector v = p-pos+r;
  69                 Angle a = Geometry::atan2(v.y, v.x)+Angle::quarter_turn()-dir;
  70                 if(radius<0)
  71                         a = Angle::half_turn()-a;
  72                 a.wrap_with_base(Angle::from_radians(length/2)-Angle::half_turn());
  73                 a = min(max(a, Angle::zero()), Angle::from_radians(length));
  74                 if(radius<0)
  75                         a = -a;
  76                 result.position = pos-r+Transform::rotation(a, Vector(0, 0, 1)).transform(r);
  77                 result.rotation = dir+a;
  78         }
  79         else
  80         {
  81                 Vector v = p-pos;
  82                 Vector dir_vec = dir_trans.transform(Vector(1, 0, 0));
  83                 float d = min(max(dot(dir_vec, v), 0.0f), length);
  84                 result.position = pos+dir_vec*d;
  85                 result.rotation = dir;
  86         }
  87         return result;
  88 }
  89
  90 void TrackPart::check_link(TrackPart &other)
  91 {
  92         unsigned n_eps = (dead_end ? 1 : 2);
  93         unsigned n_other_eps = (other.is_dead_end() ? 1 : 2);
  94         for(unsigned i=0; i<n_eps; ++i)
  95         {
  96                 OrientedPoint p1 = get_point(i ? get_length() : 0);
  97                 for(unsigned j=0; j<n_other_eps; ++j)
  98                 {
  99                         OrientedPoint p2 = other.get_point(j ? other.get_length() : 0);
 100
 101                         Vector span = p2.position-p1.position;
 102
 103                         Angle da = wrap_balanced(p2.rotation-p1.rotation+Angle::half_turn()*float((i+j+1)%2));
 104
 105                         if(dot(span, span)<1e-6 && abs(da).radians()<=0.01)
 106                         {
 107                                 links[i] = &other;
 108                                 other.links[j] = this;
 109                                 return;
 110                         }
 111                 }
 112         }
 113 }
 114
 115 TrackPart *TrackPart::get_link(unsigned i) const
 116 {
 117         if(i>=2)
 118                 throw out_of_range("TrackPart::get_link");
 119         return links[i];
 120 }
 121
 122 void TrackPart::create_shape()
 123 {
 124         Transform trans = Transform::translation(pos)*
 125                 Transform::rotation(dir, LinAl::Vector<float, 3>(0, 0, 1));
 126         if(radius)
 127         {
 128                 Geometry::ExtrudedShape<float, 3> ring(
 129                         Geometry::Intersection<float, 2>(
 130                                 Geometry::Circle<float>(abs(radius)+0.02),
 131                                 Geometry::Negation<float, 2>(Geometry::Circle<float>(abs(radius)-0.02))),
 132                         0.01);
 133                 Geometry::HalfSpace<float, 3> start_wall(Vector(-1, 0, 0));
 134                 Geometry::HalfSpace<float, 3> end_wall(rotated_vector(Vector(1, 0, 0),
 135                         Angle::from_radians(length*(radius<0 ? -1 : 1))));
 136                 /* This box only exists so the bounding boxes of curves make sense.  It
 137                 can be removed when libmspmath produces better bounding boxes. */
 138                 float w = sin(length)*(abs(radius)+0.02);
 139                 float h = (1-cos(length))*abs(radius-0.02)+0.04;
 140                 Geometry::TransformedShape<float, 3> bounds(
 141                         Geometry::Box<float>(w, h, 0.01),
 142                         Transform::translation(Vector(w/2, (h/2-0.02)*(radius<0 ? -1 : 1)-radius, 0.005)));
 143                 vector<const Shape *> shapes;
 144                 shapes.push_back(&ring);
 145                 shapes.push_back(&start_wall);
 146                 shapes.push_back(&end_wall);
 147                 shapes.push_back(&bounds);
 148                 shape = new Geometry::TransformedShape<float, 3>(
 149                         Geometry::Intersection<float, 3>::from_iterator_range(shapes.begin(), shapes.end()),
 150                         trans*Transform::translation(Vector(0, radius, 0.005)));
 151         }
 152         else
 153         {
 154                 // TODO Get the track profile dimensions from somewhere
 155                 shape = new Geometry::TransformedShape<float, 3>(
 156                         Geometry::Box<float>(length, 0.04, 0.01),
 157                         trans*Transform::translation(Vector(length/2, 0, 0.005)));
 158         }
 159 }
 160
 161
 162 TrackPart::Loader::Loader(TrackPart &p):
 163         Msp::DataFile::ObjectLoader<TrackPart>(p)
 164 {
 165         add("start",    &Loader::start);
 166         add("length",   &TrackPart::length);
 167         add("radius",   &TrackPart::radius);
 168         add("path",     &TrackPart::path);
 169         add("dead_end", &TrackPart::dead_end);
 170 }
 171
 172 void TrackPart::Loader::finish()
 173 {
 174         if(obj.radius)
 175         {
 176                 obj.length *= M_PI/180;
 177                 obj.radius /= 1000;
 178         }
 179         else
 180                 obj.length /= 1000;
 181
 182         obj.pos /= 1000;
 183 }
 184
 185 void TrackPart::Loader::start(float x, float y, float d)
 186 {
 187         obj.pos = Vector(x, y, 0);
 188         obj.dir = Angle::from_degrees(d);
 189 }
 190
 191 } // namespace R2C2