source/libr2c2/vehicleplacement.cpp

   1 #include "vehicleplacement.h"
   2
   3 using namespace std;
   4 using namespace Msp;
   5
   6 namespace R2C2 {
   7
   8 VehiclePlacement::VehiclePlacement(const VehicleType &t):
   9         type(t)
  10 {
  11         const VehicleType::AxleArray &type_axles = type.get_axles();
  12         const VehicleType::Axle *first_fixed = 0;
  13         const VehicleType::Axle *last_fixed = 0;
  14         for(VehicleType::AxleArray::const_iterator i=type_axles.begin(); i!=type_axles.end(); ++i)
  15                 if(!i->bogie)
  16                 {
  17                         if(!first_fixed)
  18                                 first_fixed = &*i;
  19                         else
  20                                 last_fixed = &*i;
  21                 }
  22
  23         float front_offset, back_offset;
  24         if(last_fixed)
  25         {
  26                 axles.push_back(*first_fixed);
  27                 axles.push_back(*last_fixed);
  28                 front_offset = axles.front().type->position;
  29                 back_offset = axles.back().type->position;
  30         }
  31         else
  32         {
  33                 const VehicleType::BogieArray &type_bogies = type.get_bogies();
  34                 if(type_bogies.size()>=2)
  35                 {
  36                         axles.push_back(*type_bogies.front().axles.front());
  37                         axles.push_back(*type_bogies.front().axles.back());
  38                         axles.push_back(*type_bogies.back().axles.front());
  39                         axles.push_back(*type_bogies.back().axles.back());
  40                         front_offset = type_bogies.front().position;
  41                         back_offset = type_bogies.back().position;
  42                 }
  43                 else
  44                         // TODO Handle weird configurations; one fixed axle and one bogie?
  45                         throw invalid_argument("unsupported axle configuration");
  46         }
  47
  48         front_back_span = front_offset-back_offset;
  49         front_back_ratio = front_offset/front_back_span;
  50 }
  51
  52 void VehiclePlacement::place(const TrackOffsetIter &p, Anchor a)
  53 {
  54         if(!p)
  55                 return unplace();
  56
  57         float anchor = get_anchor_position(a);
  58
  59         for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
  60                 i->position = p.advance(i->type->position-anchor);
  61
  62         fix_positions(anchor);
  63 }
  64
  65 float VehiclePlacement::get_anchor_position(Anchor a) const
  66 {
  67         /* Use front and back axle from the type, as those are not necessarily the
  68         same as the first and last axle used to determine position. */
  69         if(a==FRONT_AXLE)
  70                 return type.get_axles().front().position;
  71         else if(a==FRONT_BUFFER)
  72                 return type.get_length()/2;
  73         else if(a==BACK_AXLE)
  74                 return type.get_axles().back().position;
  75         else if(a==BACK_BUFFER)
  76                 return -type.get_length()/2;
  77         else
  78                 return 0;
  79 }
  80
  81 void VehiclePlacement::place_after(const VehiclePlacement &other)
  82 {
  83         const Axle &other_axle = other.axles.back();
  84         place(other_axle.position.advance(-other.type.get_length()/2-other_axle.type->position), FRONT_BUFFER);
  85 }
  86
  87 void VehiclePlacement::place_before(const VehiclePlacement &other)
  88 {
  89         const Axle &other_axle = other.axles.front();
  90         place(other_axle.position.advance(other.type.get_length()/2-other_axle.type->position), BACK_BUFFER);
  91 }
  92
  93 void VehiclePlacement::unplace()
  94 {
  95         for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
  96                 i->position = TrackOffsetIter();
  97 }
  98
  99 bool VehiclePlacement::is_placed() const
 100 {
 101         return axles.front().position;
 102 }
 103
 104 OrientedPoint VehiclePlacement::get_point() const
 105 {
 106         Vector front = get_front_point().position;
 107         Vector back = get_back_point().position;
 108         return create_point(front, back, front_back_ratio);
 109 }
 110
 111 const TrackOffsetIter &VehiclePlacement::get_axle_position(unsigned i) const
 112 {
 113         for(vector<Axle>::const_iterator j=axles.begin(); j!=axles.end(); ++j)
 114                 if(j->type->index==i)
 115                         return j->position;
 116
 117         throw invalid_argument("VehiclePlacement::get_axle_position");
 118 }
 119
 120 TrackOffsetIter VehiclePlacement::get_position(Anchor a) const
 121 {
 122         float position = get_anchor_position(a);
 123         const Axle *closest_axle = 0;
 124         float closest_diff = -1;
 125         for(vector<Axle>::const_iterator j=axles.begin(); j!=axles.end(); ++j)
 126         {
 127                 float diff = abs(j->type->position-position);
 128                 if(!closest_axle || diff<closest_diff)
 129                 {
 130                         closest_axle = &*j;
 131                         closest_diff = diff;
 132                 }
 133         }
 134
 135         return closest_axle->position.advance(position-closest_axle->type->position);
 136 }
 137
 138 OrientedPoint VehiclePlacement::get_axle_point(unsigned i) const
 139 {
 140         return get_axle_position(i).point();
 141 }
 142
 143 OrientedPoint VehiclePlacement::get_bogie_point(unsigned i) const
 144 {
 145         unsigned j;
 146         for(j=0; j<axles.size(); ++j)
 147                 if(axles[j].type->bogie && axles[j].type->bogie->index==i)
 148                         return get_bogie_point_from_axle(j);
 149
 150         // TODO Come up with positions of other bogies
 151         throw invalid_argument("VehiclePlacement::get_bogie_point");
 152 }
 153
 154 OrientedPoint VehiclePlacement::get_front_point() const
 155 {
 156         if(axles.front().type->bogie)
 157                 return get_bogie_point_from_axle(0);
 158         else
 159                 return axles.front().position.point();
 160 }
 161
 162 OrientedPoint VehiclePlacement::get_back_point() const
 163 {
 164         if(axles.back().type->bogie)
 165                 return get_bogie_point_from_axle(axles.size()-2);
 166         else
 167                 return axles.back().position.point();
 168 }
 169
 170 OrientedPoint VehiclePlacement::get_bogie_point_from_axle(unsigned i) const
 171 {
 172         Vector front = axles[i].position.point().position;
 173         Vector back = axles[i+1].position.point().position;
 174         float span = axles[i].type->local_position-axles[i+1].type->local_position;
 175         float ratio = axles[i].type->local_position/span;
 176         return create_point(front, back, ratio);
 177 }
 178
 179 OrientedPoint VehiclePlacement::create_point(const Vector &front, const Vector &back, float ratio)
 180 {
 181         OrientedPoint result;
 182         result.position = front*(1-ratio)+back*ratio;
 183         result.rotation = Geometry::atan2<float>(front.y-back.y, front.x-back.x);
 184         result.tilt = Geometry::atan2<float>(front.z-back.z, LinAl::Vector<float, 2>(front-back).norm());
 185         return result;
 186 }
 187
 188 void VehiclePlacement::advance(float d, Anchor a)
 189 {
 190         for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
 191                 i->position = i->position.advance(d);
 192
 193         fix_positions(get_anchor_position(a));
 194 }
 195
 196 void VehiclePlacement::fix_positions(float anchor)
 197 {
 198         float ratio = front_back_ratio-anchor/front_back_span;
 199         float last_adjust = -1;
 200         while(1)
 201         {
 202                 Vector front = get_front_point().position;
 203                 Vector back = get_back_point().position;
 204
 205                 float adjust = compute_adjustment(front, back, front_back_span, last_adjust);
 206                 if(!adjust)
 207                         return;
 208
 209                 axles.front().position = axles.front().position.advance(adjust*ratio);
 210                 if(axles.front().type->bogie)
 211                 {
 212                         axles[1].position = axles[1].position.advance(adjust*ratio);
 213                         fix_bogie_position(0);
 214                 }
 215
 216                 axles.back().position = axles.back().position.advance(adjust*(ratio-1));
 217                 if(axles.back().type->bogie)
 218                 {
 219                         axles[axles.size()-2].position = axles[axles.size()-2].position.advance(adjust*(ratio-1));
 220                         fix_bogie_position(axles.size()-2);
 221                 }
 222         }
 223 }
 224
 225 void VehiclePlacement::fix_bogie_position(unsigned i)
 226 {
 227         Axle &front_axle = axles[i];
 228         Axle &back_axle = axles[i+1];
 229         float target_span = front_axle.type->local_position-back_axle.type->local_position;
 230         float ratio = front_axle.type->local_position/target_span;
 231
 232         float last_adjust = -1;
 233         while(1)
 234         {
 235                 Vector front_point = front_axle.position.point().position;
 236                 Vector back_point = back_axle.position.point().position;
 237
 238                 float adjust = compute_adjustment(front_point, back_point, target_span, last_adjust);
 239                 if(!adjust)
 240                         return;
 241
 242                 front_axle.position = front_axle.position.advance(adjust*ratio);
 243                 back_axle.position = back_axle.position.advance(adjust*(ratio-1));
 244         }
 245 }
 246
 247 float VehiclePlacement::compute_adjustment(const Vector &p1, const Vector &p2, float target, float &last)
 248 {
 249         float span = distance(p1, p2);
 250         float adjust = target-span;
 251
 252         /* If the adjustment is not smaller than the last one, we've hit a gap or
 253         other oddity in the track.  Terminate to avoid an infinite loop. */
 254         if(last>0 && abs(adjust)>=last)
 255                 return 0;
 256
 257         if(abs(adjust)*10000<target)
 258                 return 0;
 259
 260         last = abs(adjust);
 261         return adjust;
 262 }
 263
 264
 265 VehiclePlacement::Axle::Axle(const VehicleType::Axle &t):
 266         type(&t)
 267 { }
 268
 269 } // namespace R2C2