]> git.tdb.fi Git - r2c2.git/commitdiff
Split vehicle placement code to a separate class
authorMikko Rasa <tdb@tdb.fi>
Fri, 2 Aug 2013 14:55:48 +0000 (17:55 +0300)
committerMikko Rasa <tdb@tdb.fi>
Fri, 2 Aug 2013 14:55:48 +0000 (17:55 +0300)
I'm going to need this elsewhere soon.

source/3d/vehicle.cpp
source/libr2c2/blockallocator.cpp
source/libr2c2/train.cpp
source/libr2c2/trainrouteplanner.cpp
source/libr2c2/trainstatus.cpp
source/libr2c2/vehicle.cpp
source/libr2c2/vehicle.h
source/libr2c2/vehicleplacement.cpp [new file with mode: 0644]
source/libr2c2/vehicleplacement.h [new file with mode: 0644]

index 9d0f3e88dc4203e8b497b02f01e27d6b1ef131a7..fa1d751c7e20cb297a7b665cc34e55123b4b199c 100644 (file)
@@ -57,7 +57,7 @@ Vector Vehicle3D::get_node() const
 
 bool Vehicle3D::is_visible() const
 {
-       return vehicle.get_track();
+       return vehicle.is_placed();
 }
 
 void Vehicle3D::moved()
@@ -70,7 +70,7 @@ void Vehicle3D::moved()
 
 void Vehicle3D::render(GL::Renderer &renderer, const GL::Tag &tag) const
 {
-       if(!vehicle.get_track())
+       if(!is_visible())
                return;
 
        ObjectInstance::render(renderer, tag);
index 347960ee2ec584029a7bc66dffaaabb56de0cdad..547415aa965173ad5eeb68fb0966eceb6aff000c 100644 (file)
@@ -316,8 +316,9 @@ void BlockAllocator::advance_front(const Sensor *sensor)
 
 void BlockAllocator::advance_back()
 {
-       const Vehicle &veh = train.get_vehicle(train.get_controller().get_reverse() ? 0 : train.get_n_vehicles()-1);
-       const Block &veh_block = veh.get_track()->get_block();
+       bool rev = train.get_controller().get_reverse();
+       const Vehicle &veh = train.get_vehicle(rev ? 0 : train.get_n_vehicles()-1);
+       const Block &veh_block = veh.get_placement().get_position(rev ? VehiclePlacement::FRONT_AXLE : VehiclePlacement::BACK_AXLE)->get_block();
 
        /* Sensors aren't guaranteed to be detriggered in order.  Go through the
        block list and locate the first sensor that's still active. */
index 285a15a27fb83794bcffa9406c144b416e1d0079..95b78b567c9498ba687c05bbd445082d252ad065 100644 (file)
@@ -197,9 +197,9 @@ void Train::place(const BlockIter &block)
        last_entry_block = BlockIter();
 
        if(reverse)
-               vehicles.front()->place(block.reverse().track_iter(), 0, Vehicle::FRONT_BUFFER);
+               vehicles.front()->place(block.reverse().track_iter(), VehiclePlacement::FRONT_BUFFER);
        else
-               vehicles.back()->place(block.track_iter(), 0, Vehicle::BACK_BUFFER);
+               vehicles.back()->place(block.track_iter(), VehiclePlacement::BACK_BUFFER);
 }
 
 void Train::unplace()
@@ -337,7 +337,7 @@ void Train::tick(const Time::TimeDelta &dt)
                Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
 
                float d = speed*(dt/Time::sec);
-               if(allocator.is_block_current(vehicle.get_track()->get_block()))
+               if(allocator.is_block_current(vehicle.get_placement().get_position(reverse ? VehiclePlacement::BACK_AXLE : VehiclePlacement::FRONT_AXLE)->get_block()))
                {
                        SetFlag setf(advancing);
                        vehicle.advance(reverse ? -d : d);
@@ -464,21 +464,21 @@ void Train::sensor_state_changed(Sensor &sensor, Sensor::State state)
                accurate_position = true;
                overshoot_dist = 0;
 
-               if(!advancing && vehicles.front()->get_track())
+               if(!advancing && vehicles.front()->is_placed())
                {
                        TrackIter track = last_entry_block.track_iter();
                        if(reverse)
                        {
                                track = track.flip();
-                               vehicles.back()->place(track, 0, Vehicle::BACK_AXLE);
+                               vehicles.back()->place(track, VehiclePlacement::BACK_AXLE);
                        }
                        else
-                               vehicles.front()->place(track, 0, Vehicle::FRONT_AXLE);
+                               vehicles.front()->place(track, VehiclePlacement::FRONT_AXLE);
                }
        }
        else if(BeamGate *gate = dynamic_cast<BeamGate *>(&sensor))
        {
-               if(!advancing && vehicles.front()->get_track())
+               if(!advancing && vehicles.front()->is_placed())
                {
                        TrackIter track = allocator.iter_for(*block).track_iter();
                        for(; (track && &track->get_block()==block); track=track.next())
@@ -488,9 +488,9 @@ void Train::sensor_state_changed(Sensor &sensor, Sensor::State state)
                                                track = track.reverse();
                                        float offset = gate->get_offset_from_endpoint(track.entry());
                                        if(reverse)
-                                               vehicles.back()->place(track, offset, Vehicle::BACK_BUFFER);
+                                               vehicles.back()->place(TrackOffsetIter(track, offset), VehiclePlacement::BACK_BUFFER);
                                        else
-                                               vehicles.front()->place(track, offset, Vehicle::FRONT_BUFFER);
+                                               vehicles.front()->place(TrackOffsetIter(track, offset), VehiclePlacement::FRONT_BUFFER);
                                        break;
                                }
                }
@@ -510,7 +510,7 @@ float Train::get_reserved_distance_until(const Block *until_block) const
 
        Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front());
 
-       TrackIter track = veh.get_track_iter().track_iter();
+       TrackOffsetIter track = veh.get_placement().get_position(reverse ? VehiclePlacement::BACK_AXLE : VehiclePlacement::FRONT_AXLE);
        if(!track)  // XXX Probably unnecessary
                return 0;
 
@@ -518,7 +518,7 @@ float Train::get_reserved_distance_until(const Block *until_block) const
                return 0;
 
        // Account for the vehicle's offset on its current track
-       float result = veh.get_offset();
+       float result = track.offset();
        if(reverse)
                track = track.reverse();
        else
@@ -566,7 +566,7 @@ void Train::Loader::finish()
        {
                TrackIter track = obj.allocator.first().track_iter();
                float offset = 2*obj.layout.get_catalogue().get_scale();
-               obj.vehicles.back()->place(track, offset, Vehicle::BACK_BUFFER);
+               obj.vehicles.back()->place(TrackOffsetIter(track, offset), VehiclePlacement::BACK_BUFFER);
        }
 }
 
index e75cec9cc6f9921f248dd76f89ecc8af80dc6067..770c0ea537897a01ee0f3a70ae5418f6aa78927d 100644 (file)
@@ -227,15 +227,14 @@ TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf):
 {
        const Vehicle *veh = &info->train->get_vehicle(0);
        // TODO margins
-       float half_length = veh->get_type().get_length()/2;
-       TrackOffsetIter track_and_offs = veh->get_track_iter().advance(half_length);
+       TrackOffsetIter track_and_offs = veh->get_placement().get_position(VehiclePlacement::FRONT_BUFFER);
        track = track_and_offs.track_iter();
        offset = track_and_offs.offset();
        path = track->get_active_path();
 
        while(Vehicle *next = veh->get_link(1))
                veh = next;
-       track_and_offs = veh->get_track_iter().advance(-half_length);
+       track_and_offs = veh->get_placement().get_position(VehiclePlacement::BACK_BUFFER);
        back_offset = track_and_offs.offset();
 
        TrackIter iter = track_and_offs.track_iter();
index 10651d01c210ccba7d8f805b2bcfde2f9524cf25..e786f8cbfdfdce79a37982fdd17993844d8340b5 100644 (file)
@@ -38,7 +38,7 @@ void TrainStatus::check()
                }
                else if(s==-1)
                        status = "Waiting";
-               else if(!train.get_vehicle(0).get_track())
+               else if(!train.get_vehicle(0).is_placed())
                        status = "Unplaced";
                else
                        status = "Stopped";
index 96ad879fbc0c2d300ad1ae19cb6f10e2f2bb050e..0316887fded747e90e2d62fa07afa3fd287de499 100644 (file)
@@ -19,6 +19,7 @@ Vehicle::Vehicle(Layout &l, const VehicleType &t):
        train(0),
        next(0),
        prev(0),
+       placement(type),
        front_sensor(0),
        back_sensor(0)
 {
@@ -65,7 +66,7 @@ void Vehicle::attach_back(Vehicle &veh)
        next = &veh;
        veh.prev = this;
 
-       if(track)
+       if(is_placed())
                propagate_backward();
 }
 
@@ -77,7 +78,7 @@ void Vehicle::attach_front(Vehicle &veh)
        prev = &veh;
        veh.next = this;
 
-       if(prev->get_track())
+       if(prev->is_placed())
                prev->propagate_backward();
 }
 
@@ -99,32 +100,23 @@ void Vehicle::detach_front()
        prev = 0;
 }
 
-void Vehicle::place(const TrackIter &t, float o, PlaceMode m)
+void Vehicle::place(const TrackOffsetIter &t, VehiclePlacement::Anchor a)
 {
        if(!t)
                throw invalid_argument("Vehicle::place");
 
-       track = TrackOffsetIter(t, o);
+       placement.place(t, a);
 
-       if(m==FRONT_AXLE)
-               track = track.advance(-type.get_front_axle_offset());
-       else if(m==FRONT_BUFFER)
-               track = track.advance(-type.get_length()/2);
-       else if(m==BACK_AXLE)
-               track = track.advance(-type.get_back_axle_offset());
-       else if(m==BACK_BUFFER)
-               track = track.advance(type.get_length()/2);
-
-       update_position();
+       update_position(0);
        propagate_position();
 }
 
 void Vehicle::unplace()
 {
-       if(!track)
+       if(!placement.is_placed())
                return;
 
-       track = TrackOffsetIter();
+       placement.unplace();
 
        if(prev)
                prev->unplace();
@@ -134,9 +126,9 @@ void Vehicle::unplace()
 
 void Vehicle::advance(float d)
 {
-       track = track.advance(d);
-       update_position();
+       placement.advance(d);
        turn_axles(d);
+       update_position(d<0 ? -1 : 1);
        propagate_position();
 }
 
@@ -177,47 +169,28 @@ const Vehicle::Rod &Vehicle::get_rod(unsigned i) const
        return rods[i];
 }
 
-void Vehicle::update_position()
+void Vehicle::update_position(int sign)
 {
-       OrientedPoint p;
+       OrientedPoint p = placement.get_point();
+       position = p.position;
+       position.z += layout.get_catalogue().get_rail_elevation();
+       rotation = p.rotation;
+       tilt = p.tilt;
 
-       if(fixed_axles.size()>=2)
-       {
-               float wheelbase = fixed_axles.front()->type->position-fixed_axles.back()->type->position;
-               p = get_point(track, wheelbase, -fixed_axles.back()->type->position/wheelbase);
-       }
-       else if(bogies.size()>=2)
+       if(bogies.size()>=2)
        {
-               TrackOffsetIter front = track.advance(bogies.front().type->position);
-               TrackOffsetIter back = track.advance(bogies.back().type->position);
-               float bogie_spacing = bogies.front().type->position-bogies.back().type->position;
-               adjust_for_distance(front, back, bogie_spacing);
-
-               const vector<Axle *> &front_axles = bogies.front().axles;
-               float wheelbase = front_axles.front()->type->position-front_axles.back()->type->position;
-               OrientedPoint front_point = get_point(front, wheelbase, -front_axles.back()->type->position/wheelbase);
-
-               const vector<Axle *> &back_axles = bogies.back().axles;
-               wheelbase = back_axles.front()->type->position-back_axles.back()->type->position;
-               OrientedPoint back_point = get_point(back, wheelbase, -back_axles.back()->type->position/wheelbase);
-
-               p = get_point(front_point.position, back_point.position, -bogies.back().type->position/bogie_spacing);
-
+               OrientedPoint front_point = placement.get_bogie_point(bogies.front().type->index);
                bogies.front().direction = front_point.rotation-p.rotation;
+
+               OrientedPoint back_point = placement.get_bogie_point(bogies.back().type->index);
                bogies.back().direction = back_point.rotation-p.rotation;
        }
-       else
-               p = track.point();
 
        if(!prev)
-               check_sensor(type.get_front_axle_offset(), front_sensor);
+               check_sensor(placement.get_position(VehiclePlacement::FRONT_AXLE), front_sensor, sign<0);
        if(!next)
-               check_sensor(type.get_back_axle_offset(), back_sensor);
+               check_sensor(placement.get_position(VehiclePlacement::BACK_AXLE), back_sensor, sign>0);
 
-       position = p.position;
-       position.z += layout.get_catalogue().get_rail_elevation();
-       rotation = p.rotation;
-       tilt = p.tilt;
        signal_moved.emit();
 }
 
@@ -229,17 +202,21 @@ void Vehicle::update_position_from(const Vehicle &veh)
        float margin = layout.get_catalogue().get_scale();
 
        float dist = distance(veh.position, position);
-       if(!track || dist<tdist-margin || dist>tdist+margin)
+       if(!is_placed() || dist<tdist-margin || dist>tdist+margin)
        {
-               track = veh.track.advance(sign*tdist);
-               update_position();
+               if(sign<0)
+                       placement.place_after(veh.placement);
+               else
+                       placement.place_before(veh.placement);
+               update_position(0);
 
                dist = distance(veh.position, position);
        }
 
-       track = track.advance(sign*(tdist-dist));
-       update_position();
-       turn_axles(sign*(tdist-dist));
+       float d = sign*(tdist-dist);
+       placement.advance(d);
+       update_position(d<0 ? -1 : 1);
+       turn_axles(d);
 }
 
 void Vehicle::propagate_position()
@@ -266,28 +243,17 @@ void Vehicle::propagate_backward()
                next->propagate_backward();
 }
 
-void Vehicle::check_sensor(float offset, unsigned &sensor)
+void Vehicle::check_sensor(const TrackOffsetIter &t, unsigned &sensor, bool release)
 {
-       TrackOffsetIter iter = track.advance(offset);
-       unsigned s = iter->get_sensor_id();
+       unsigned s = t->get_sensor_id();
        if(s!=sensor)
        {
-               /* Sensor ID under axle has changed.  Deduce movement direction by using
-               the sensor ID under the midpoint of the vehicle. */
-               /* XXX This depends on the simulation running fast enough.  Something
-               more robust would be preferable. */
                unsigned old = sensor;
                sensor = s;
-               unsigned mid = track->get_sensor_id();
-
-               if(s && s!=mid)
-                       /* There's a sensor and it's different from mid.  We've just entered
-                       that sensor. */
-                       layout.get_driver().set_sensor(sensor, true);
-               if(old && old!=mid)
-                       /* A sensor was under the axle and it was different from mid.  We've
-                       just left that sensor. */
+               if(release)
                        layout.get_driver().set_sensor(old, false);
+               else
+                       layout.get_driver().set_sensor(sensor, true);
        }
 }
 
@@ -352,57 +318,6 @@ void Vehicle::update_rods()
        }
 }
 
-void Vehicle::adjust_for_distance(TrackOffsetIter &front, TrackOffsetIter &back, float tdist, float ratio) const
-{
-       float margin = 0.01*layout.get_catalogue().get_scale();
-       int adjust_dir = 0;
-       while(1)
-       {
-               Vector front_point = front.point().position;
-               Vector back_point = back.point().position;
-
-               float dist = distance(front_point, back_point);
-
-               float diff = tdist-dist;
-               if(diff<-margin && adjust_dir<=0)
-               {
-                       diff -= margin;
-                       adjust_dir = -1;
-               }
-               else if(diff>margin && adjust_dir>=0)
-               {
-                       diff += margin;
-                       adjust_dir = 1;
-               }
-               else
-                       return;
-
-               front = front.advance(diff*(1-ratio));
-               back = back.advance(-diff*ratio);
-       }
-}
-
-OrientedPoint Vehicle::get_point(const Vector &front, const Vector &back, float ratio) const
-{
-       Vector span = front-back;
-
-       OrientedPoint p;
-       p.position = back+span*ratio;
-       p.rotation = Geometry::atan2(span.y, span.x);
-       p.tilt = Geometry::atan2(span.z, LinAl::Vector<float, 2>(span).norm());
-
-       return p;
-}
-
-OrientedPoint Vehicle::get_point(const TrackOffsetIter &iter, float tdist, float ratio) const
-{
-       TrackOffsetIter front = iter.advance(tdist*(1-ratio));
-       TrackOffsetIter back = iter.advance(-tdist*ratio);
-
-       adjust_for_distance(front, back, tdist, ratio);
-       return get_point(front.point().position, back.point().position, ratio);
-}
-
 unsigned Vehicle::get_n_link_slots() const
 {
        return 2;
index 1e9454fa5afb0fd2607099a72cbfc5c1e06de205..b1e9163f1019336045ed142c5c5ff91c01510a57 100644 (file)
@@ -4,6 +4,7 @@
 #include "geometry.h"
 #include "object.h"
 #include "trackoffsetiter.h"
+#include "vehicleplacement.h"
 #include "vehicletype.h"
 
 namespace R2C2 {
@@ -62,7 +63,7 @@ private:
        Train *train;
        Vehicle *next;
        Vehicle *prev;
-       TrackOffsetIter track;
+       VehiclePlacement placement;
        std::vector<Axle> axles;
        std::vector<Axle *> fixed_axles;
        std::vector<Bogie> bogies;
@@ -90,32 +91,26 @@ public:
        virtual void set_position(const Vector &) { }
        virtual void set_rotation(const Angle &) { }
        virtual void set_tilt(const Angle &) { }
-       void place(const TrackIter &, float, PlaceMode = CENTER);
+       void place(const TrackOffsetIter &, VehiclePlacement::Anchor = VehiclePlacement::CENTER);
        void unplace();
        void advance(float);
-       const TrackOffsetIter &get_track_iter() const { return track; }
-       Track *get_track() const { return track.track(); }
-       unsigned get_entry() const { return track.entry(); }
-       float get_offset() const { return track.offset(); }
+       const VehiclePlacement &get_placement() const { return placement; }
+       bool is_placed() const { return placement.is_placed(); }
        const Axle &get_axle(unsigned) const;
        const Axle &get_fixed_axle(unsigned) const;
        const Bogie &get_bogie(unsigned) const;
        const Axle &get_bogie_axle(unsigned, unsigned) const;
        const Rod &get_rod(unsigned) const;
 private:
-       void update_position();
+       void update_position(int);
        void update_position_from(const Vehicle &);
        void propagate_position();
        void propagate_forward();
        void propagate_backward();
-       void check_sensor(float, unsigned &);
+       void check_sensor(const TrackOffsetIter &, unsigned &, bool);
        void turn_axles(float);
        void update_rods();
 
-       void adjust_for_distance(TrackOffsetIter &, TrackOffsetIter &, float, float = 0.5) const;
-       OrientedPoint get_point(const Vector &, const Vector &, float = 0.5) const;
-       OrientedPoint get_point(const TrackOffsetIter &, float, float = 0.5) const;
-
 public:
        virtual unsigned get_n_link_slots() const;
        virtual Vehicle *get_link(unsigned) const;
diff --git a/source/libr2c2/vehicleplacement.cpp b/source/libr2c2/vehicleplacement.cpp
new file mode 100644 (file)
index 0000000..9f9001f
--- /dev/null
@@ -0,0 +1,266 @@
+#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
diff --git a/source/libr2c2/vehicleplacement.h b/source/libr2c2/vehicleplacement.h
new file mode 100644 (file)
index 0000000..e898f56
--- /dev/null
@@ -0,0 +1,69 @@
+#ifndef LIBR2C2_VEHICLEPLACEMENT_H_
+#define LIBR2C2_VEHICLEPLACEMENT_H_
+
+#include <vector>
+#include "trackoffsetiter.h"
+#include "vehicletype.h"
+
+namespace R2C2 {
+
+class VehiclePlacement
+{
+public:
+       enum Anchor
+       {
+               CENTER,
+               FRONT_AXLE,
+               FRONT_BUFFER,
+               BACK_AXLE,
+               BACK_BUFFER
+       };
+
+private:
+       struct Axle
+       {
+               const VehicleType::Axle *type;
+               TrackOffsetIter position;
+
+               Axle(const VehicleType::Axle &);
+       };
+
+       const VehicleType &type;
+       std::vector<Axle> axles;
+       float front_back_span;
+       float front_back_ratio;
+
+public:
+       VehiclePlacement(const VehicleType &);
+
+       void place(const TrackOffsetIter &, Anchor = CENTER);
+private:
+       float get_anchor_position(Anchor) const;
+public:
+       void place_after(const VehiclePlacement &);
+       void place_before(const VehiclePlacement &);
+       void unplace();
+       bool is_placed() const;
+
+       OrientedPoint get_point() const;
+       const TrackOffsetIter &get_axle_position(unsigned) const;
+       TrackOffsetIter get_position(Anchor) const;
+       OrientedPoint get_axle_point(unsigned) const;
+       OrientedPoint get_bogie_point(unsigned) const;
+private:
+       OrientedPoint get_front_point() const;
+       OrientedPoint get_back_point() const;
+       OrientedPoint get_bogie_point_from_axle(unsigned) const;
+       static OrientedPoint create_point(const Vector &, const Vector &, float);
+
+public:
+       void advance(float, Anchor = CENTER);
+private:
+       void fix_positions(float);
+       void fix_bogie_position(unsigned);
+       static float compute_adjustment(const Vector &, const Vector &, float, float &);
+};
+
+} // namespace R2C2
+
+#endif