1 #include "vehicleplacement.h"
8 VehiclePlacement::VehiclePlacement(const VehicleType &t):
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)
23 float front_offset, back_offset;
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;
33 const VehicleType::BogieArray &type_bogies = type.get_bogies();
34 if(type_bogies.size()>=2)
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;
44 // TODO Handle weird configurations; one fixed axle and one bogie?
45 throw invalid_argument("unsupported axle configuration");
48 front_back_span = front_offset-back_offset;
49 front_back_ratio = front_offset/front_back_span;
52 void VehiclePlacement::place(const TrackOffsetIter &p, Anchor a)
54 float anchor = get_anchor_position(a);
56 for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
57 i->position = p.advance(i->type->position-anchor);
59 fix_positions(anchor);
62 float VehiclePlacement::get_anchor_position(Anchor a) const
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. */
67 return type.get_axles().front().position;
68 else if(a==FRONT_BUFFER)
69 return type.get_length()/2;
71 return type.get_axles().back().position;
72 else if(a==BACK_BUFFER)
73 return -type.get_length()/2;
78 void VehiclePlacement::place_after(const VehiclePlacement &other)
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);
84 void VehiclePlacement::place_before(const VehiclePlacement &other)
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);
90 void VehiclePlacement::unplace()
92 for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
93 i->position = TrackOffsetIter();
96 bool VehiclePlacement::is_placed() const
98 return axles.front().position;
101 OrientedPoint VehiclePlacement::get_point() const
103 Vector front = get_front_point().position;
104 Vector back = get_back_point().position;
105 return create_point(front, back, front_back_ratio);
108 const TrackOffsetIter &VehiclePlacement::get_axle_position(unsigned i) const
110 for(vector<Axle>::const_iterator j=axles.begin(); j!=axles.end(); ++j)
111 if(j->type->index==i)
114 throw invalid_argument("VehiclePlacement::get_axle_position");
117 TrackOffsetIter VehiclePlacement::get_position(Anchor a) const
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)
124 float diff = abs(j->type->position-position);
125 if(!closest_axle || diff<closest_diff)
132 return closest_axle->position.advance(position-closest_axle->type->position);
135 OrientedPoint VehiclePlacement::get_axle_point(unsigned i) const
137 return get_axle_position(i).point();
140 OrientedPoint VehiclePlacement::get_bogie_point(unsigned i) const
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);
147 // TODO Come up with positions of other bogies
148 throw invalid_argument("VehiclePlacement::get_bogie_point");
151 OrientedPoint VehiclePlacement::get_front_point() const
153 if(axles.front().type->bogie)
154 return get_bogie_point_from_axle(0);
156 return axles.front().position.point();
159 OrientedPoint VehiclePlacement::get_back_point() const
161 if(axles.back().type->bogie)
162 return get_bogie_point_from_axle(axles.size()-2);
164 return axles.back().position.point();
167 OrientedPoint VehiclePlacement::get_bogie_point_from_axle(unsigned i) const
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);
176 OrientedPoint VehiclePlacement::create_point(const Vector &front, const Vector &back, float ratio)
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());
185 void VehiclePlacement::advance(float d, Anchor a)
187 for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
188 i->position = i->position.advance(d);
190 fix_positions(get_anchor_position(a));
193 void VehiclePlacement::fix_positions(float anchor)
195 float ratio = front_back_ratio-anchor/front_back_span;
196 float last_adjust = -1;
199 Vector front = get_front_point().position;
200 Vector back = get_back_point().position;
202 float adjust = compute_adjustment(front, back, front_back_span, last_adjust);
206 axles.front().position = axles.front().position.advance(adjust*ratio);
207 if(axles.front().type->bogie)
209 axles[1].position = axles[1].position.advance(adjust*ratio);
210 fix_bogie_position(0);
213 axles.back().position = axles.back().position.advance(adjust*(ratio-1));
214 if(axles.back().type->bogie)
216 axles[axles.size()-2].position = axles[axles.size()-2].position.advance(adjust*(ratio-1));
217 fix_bogie_position(axles.size()-2);
222 void VehiclePlacement::fix_bogie_position(unsigned i)
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;
229 float last_adjust = -1;
232 Vector front_point = front_axle.position.point().position;
233 Vector back_point = back_axle.position.point().position;
235 float adjust = compute_adjustment(front_point, back_point, target_span, last_adjust);
239 front_axle.position = front_axle.position.advance(adjust*ratio);
240 back_axle.position = back_axle.position.advance(adjust*(ratio-1));
244 float VehiclePlacement::compute_adjustment(const Vector &p1, const Vector &p2, float target, float &last)
246 float span = distance(p1, p2);
247 float adjust = target-span;
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)
254 if(abs(adjust)*10000<target)
262 VehiclePlacement::Axle::Axle(const VehicleType::Axle &t):