--- /dev/null
+#include "vehicleplacement.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace R2C2 {
+
+VehiclePlacement::VehiclePlacement(const VehicleType &t):
+ type(t)
+{
+ const VehicleType::AxleArray &type_axles = type.get_axles();
+ const VehicleType::Axle *first_fixed = 0;
+ const VehicleType::Axle *last_fixed = 0;
+ for(VehicleType::AxleArray::const_iterator i=type_axles.begin(); i!=type_axles.end(); ++i)
+ if(!i->bogie)
+ {
+ if(!first_fixed)
+ first_fixed = &*i;
+ else
+ last_fixed = &*i;
+ }
+
+ float front_offset, back_offset;
+ if(last_fixed)
+ {
+ axles.push_back(*first_fixed);
+ axles.push_back(*last_fixed);
+ front_offset = axles.front().type->position;
+ back_offset = axles.back().type->position;
+ }
+ else
+ {
+ const VehicleType::BogieArray &type_bogies = type.get_bogies();
+ if(type_bogies.size()>=2)
+ {
+ axles.push_back(*type_bogies.front().axles.front());
+ axles.push_back(*type_bogies.front().axles.back());
+ axles.push_back(*type_bogies.back().axles.front());
+ axles.push_back(*type_bogies.back().axles.back());
+ front_offset = type_bogies.front().position;
+ back_offset = type_bogies.back().position;
+ }
+ else
+ // TODO Handle weird configurations; one fixed axle and one bogie?
+ throw invalid_argument("unsupported axle configuration");
+ }
+
+ front_back_span = front_offset-back_offset;
+ front_back_ratio = front_offset/front_back_span;
+}
+
+void VehiclePlacement::place(const TrackOffsetIter &p, Anchor a)
+{
+ float anchor = get_anchor_position(a);
+
+ for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
+ i->position = p.advance(i->type->position-anchor);
+
+ fix_positions(anchor);
+}
+
+float VehiclePlacement::get_anchor_position(Anchor a) const
+{
+ /* Use front and back axle from the type, as those are not necessarily the
+ same as the first and last axle used to determine position. */
+ if(a==FRONT_AXLE)
+ return type.get_axles().front().position;
+ else if(a==FRONT_BUFFER)
+ return type.get_length()/2;
+ else if(a==BACK_AXLE)
+ return type.get_axles().back().position;
+ else if(a==BACK_BUFFER)
+ return -type.get_length()/2;
+ else
+ return 0;
+}
+
+void VehiclePlacement::place_after(const VehiclePlacement &other)
+{
+ const Axle &other_axle = other.axles.back();
+ place(other_axle.position.advance(-other.type.get_length()/2-other_axle.type->position), FRONT_BUFFER);
+}
+
+void VehiclePlacement::place_before(const VehiclePlacement &other)
+{
+ const Axle &other_axle = other.axles.front();
+ place(other_axle.position.advance(other.type.get_length()/2-other_axle.type->position), BACK_BUFFER);
+}
+
+void VehiclePlacement::unplace()
+{
+ for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
+ i->position = TrackOffsetIter();
+}
+
+bool VehiclePlacement::is_placed() const
+{
+ return axles.front().position;
+}
+
+OrientedPoint VehiclePlacement::get_point() const
+{
+ Vector front = get_front_point().position;
+ Vector back = get_back_point().position;
+ return create_point(front, back, front_back_ratio);
+}
+
+const TrackOffsetIter &VehiclePlacement::get_axle_position(unsigned i) const
+{
+ for(vector<Axle>::const_iterator j=axles.begin(); j!=axles.end(); ++j)
+ if(j->type->index==i)
+ return j->position;
+
+ throw invalid_argument("VehiclePlacement::get_axle_position");
+}
+
+TrackOffsetIter VehiclePlacement::get_position(Anchor a) const
+{
+ float position = get_anchor_position(a);
+ const Axle *closest_axle = 0;
+ float closest_diff = -1;
+ for(vector<Axle>::const_iterator j=axles.begin(); j!=axles.end(); ++j)
+ {
+ float diff = abs(j->type->position-position);
+ if(!closest_axle || diff<closest_diff)
+ {
+ closest_axle = &*j;
+ closest_diff = diff;
+ }
+ }
+
+ return closest_axle->position.advance(position-closest_axle->type->position);
+}
+
+OrientedPoint VehiclePlacement::get_axle_point(unsigned i) const
+{
+ return get_axle_position(i).point();
+}
+
+OrientedPoint VehiclePlacement::get_bogie_point(unsigned i) const
+{
+ unsigned j;
+ for(j=0; j<axles.size(); ++j)
+ if(axles[j].type->bogie && axles[j].type->bogie->index==i)
+ return get_bogie_point_from_axle(j);
+
+ // TODO Come up with positions of other bogies
+ throw invalid_argument("VehiclePlacement::get_bogie_point");
+}
+
+OrientedPoint VehiclePlacement::get_front_point() const
+{
+ if(axles.front().type->bogie)
+ return get_bogie_point_from_axle(0);
+ else
+ return axles.front().position.point();
+}
+
+OrientedPoint VehiclePlacement::get_back_point() const
+{
+ if(axles.back().type->bogie)
+ return get_bogie_point_from_axle(axles.size()-2);
+ else
+ return axles.back().position.point();
+}
+
+OrientedPoint VehiclePlacement::get_bogie_point_from_axle(unsigned i) const
+{
+ Vector front = axles[i].position.point().position;
+ Vector back = axles[i+1].position.point().position;
+ float span = axles[i].type->local_position-axles[i+1].type->local_position;
+ float ratio = axles[i].type->local_position/span;
+ return create_point(front, back, ratio);
+}
+
+OrientedPoint VehiclePlacement::create_point(const Vector &front, const Vector &back, float ratio)
+{
+ OrientedPoint result;
+ result.position = front*(1-ratio)+back*ratio;
+ result.rotation = Geometry::atan2<float>(front.y-back.y, front.x-back.x);
+ result.tilt = Geometry::atan2<float>(front.z-back.z, LinAl::Vector<float, 2>(front-back).norm());
+ return result;
+}
+
+void VehiclePlacement::advance(float d, Anchor a)
+{
+ for(vector<Axle>::iterator i=axles.begin(); i!=axles.end(); ++i)
+ i->position = i->position.advance(d);
+
+ fix_positions(get_anchor_position(a));
+}
+
+void VehiclePlacement::fix_positions(float anchor)
+{
+ float ratio = front_back_ratio-anchor/front_back_span;
+ float last_adjust = -1;
+ while(1)
+ {
+ Vector front = get_front_point().position;
+ Vector back = get_back_point().position;
+
+ float adjust = compute_adjustment(front, back, front_back_span, last_adjust);
+ if(!adjust)
+ return;
+
+ axles.front().position = axles.front().position.advance(adjust*ratio);
+ if(axles.front().type->bogie)
+ {
+ axles[1].position = axles[1].position.advance(adjust*ratio);
+ fix_bogie_position(0);
+ }
+
+ axles.back().position = axles.back().position.advance(adjust*(ratio-1));
+ if(axles.back().type->bogie)
+ {
+ axles[axles.size()-2].position = axles[axles.size()-2].position.advance(adjust*(ratio-1));
+ fix_bogie_position(axles.size()-2);
+ }
+ }
+}
+
+void VehiclePlacement::fix_bogie_position(unsigned i)
+{
+ Axle &front_axle = axles[i];
+ Axle &back_axle = axles[i+1];
+ float target_span = front_axle.type->local_position-back_axle.type->local_position;
+ float ratio = front_axle.type->local_position/target_span;
+
+ float last_adjust = -1;
+ while(1)
+ {
+ Vector front_point = front_axle.position.point().position;
+ Vector back_point = back_axle.position.point().position;
+
+ float adjust = compute_adjustment(front_point, back_point, target_span, last_adjust);
+ if(!adjust)
+ return;
+
+ front_axle.position = front_axle.position.advance(adjust*ratio);
+ back_axle.position = back_axle.position.advance(adjust*(ratio-1));
+ }
+}
+
+float VehiclePlacement::compute_adjustment(const Vector &p1, const Vector &p2, float target, float &last)
+{
+ float span = distance(p1, p2);
+ float adjust = target-span;
+
+ /* If the adjustment is larger than the last one, we've hit a gap or
+ other oddity in the track. Terminate to avoid an infinite loop. */
+ if(last>0 && abs(adjust)>last)
+ return 0;
+
+ if(abs(adjust)*10000<target)
+ return 0;
+
+ last = abs(adjust);
+ return adjust;
+}
+
+
+VehiclePlacement::Axle::Axle(const VehicleType::Axle &t):
+ type(&t)
+{ }
+
+} // namespace R2C2
projects / r2c2.git / blobdiff