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