source/libr2c2/trackpart.cpp

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