Set correct Z coordinate to vehicle position

[r2c2.git] / source / libmarklin / vehicle.cpp

diff --git a/source/libmarklin/vehicle.cpp b/source/libmarklin/vehicle.cpp
index 1f3d3ec178e56a771987264a495b5c20982d80b9..75afa4bba78f9c10c03c4fb3254875d49770d9d1 100644 (file)
--- a/source/libmarklin/vehicle.cpp
+++ b/source/libmarklin/vehicle.cpp
@@ -7,6 +7,7 @@ Distributed under the GPL
 
 #include <cmath>
 #include "catalogue.h"
+#include "driver.h"
 #include "layout.h"
 #include "track.h"
 #include "tracktype.h"
@@ -24,13 +25,19 @@ Vehicle::Vehicle(Layout &l, const VehicleType &t):
        next(0),
        prev(0),
        direction(0),
-       bogie_dirs(type.get_bogies().size())
+       bogie_dirs(type.get_bogies().size()),
+       front_sensor(0),
+       back_sensor(0)
 {
        layout.add_vehicle(*this);
 }
 
 Vehicle::~Vehicle()
 {
+       if(next)
+               detach_back();
+       if(prev)
+               detach_front();
        layout.remove_vehicle(*this);
 }
 
@@ -41,6 +48,9 @@ void Vehicle::attach_back(Vehicle &veh)
 
        next = &veh;
        veh.prev = this;
+
+       if(track_pos.track)
+               propagate_backward();
 }
 
 void Vehicle::attach_front(Vehicle &veh)
@@ -48,8 +58,11 @@ void Vehicle::attach_front(Vehicle &veh)
        if(prev || veh.next)
                throw InvalidState("Already attached");
 
-       next = &veh;
-       veh.prev = this;
+       prev = &veh;
+       veh.next = this;
+
+       if(prev->get_track())
+               prev->propagate_backward();
 }
 
 void Vehicle::detach_back()
@@ -75,31 +88,11 @@ void Vehicle::place(Track *t, unsigned e, float o, PlaceMode m)
        track_pos = TrackPosition(t, e, o);
 
        if(m==FRONT_AXLE)
-       {
-               float front = type.get_length()/2;
-               if(!type.get_axles().empty())
-                       front = type.get_axles().front().position;
-               if(!type.get_bogies().empty())
-               {
-                       const VehicleType::Bogie &bogie = type.get_bogies().front();
-                       front = max(front, bogie.position+bogie.axles.front().position);
-               }
-               track_pos.advance(-front);
-       }
+               track_pos.advance(-type.get_front_axle_offset());
        else if(m==FRONT_BUFFER)
                track_pos.advance(-type.get_length()/2);
        else if(m==BACK_AXLE)
-       {
-               float back = type.get_length()/2;
-               if(!type.get_axles().empty())
-                       back = type.get_axles().back().position;
-               if(!type.get_bogies().empty())
-               {
-                       const VehicleType::Bogie &bogie = type.get_bogies().back();
-                       back = min(back, bogie.position+bogie.axles.back().position);
-               }
-               track_pos.advance(-back);
-       }
+               track_pos.advance(-type.get_back_axle_offset());
        else if(m==BACK_BUFFER)
                track_pos.advance(type.get_length()/2);
 
@@ -126,41 +119,44 @@ void Vehicle::update_position()
        TrackPoint tp;
 
        const vector<VehicleType::Axle> &axles = type.get_axles();
+       const vector<VehicleType::Bogie> &bogies = type.get_bogies();
        if(axles.size()>=2)
        {
                float wheelbase = axles.front().position-axles.back().position;
                tp = get_point(track_pos, wheelbase, -axles.back().position/wheelbase);
        }
-       else
+       else if(bogies.size()>=2)
        {
-               const vector<VehicleType::Bogie> &bogies = type.get_bogies();
-               if(bogies.size()>=2)
-               {
-                       TrackPosition front = track_pos;
-                       front.advance(bogies.front().position);
-                       TrackPosition back = track_pos;
-                       back.advance(bogies.back().position);
-                       float bogie_spacing = bogies.front().position-bogies.back().position;
-                       adjust_for_distance(front, back, bogie_spacing);
+               TrackPosition front = track_pos;
+               front.advance(bogies.front().position);
+               TrackPosition back = track_pos;
+               back.advance(bogies.back().position);
+               float bogie_spacing = bogies.front().position-bogies.back().position;
+               adjust_for_distance(front, back, bogie_spacing);
 
-                       const vector<VehicleType::Axle> &front_axles = bogies.front().axles;
-                       float wheelbase = front_axles.front().position-front_axles.back().position;
-                       TrackPoint front_point = get_point(front, wheelbase, -front_axles.back().position/wheelbase);
+               const vector<VehicleType::Axle> &front_axles = bogies.front().axles;
+               float wheelbase = front_axles.front().position-front_axles.back().position;
+               TrackPoint front_point = get_point(front, wheelbase, -front_axles.back().position/wheelbase);
 
-                       const vector<VehicleType::Axle> &back_axles = bogies.back().axles;
-                       wheelbase = back_axles.front().position-back_axles.back().position;
-                       TrackPoint back_point = get_point(back, wheelbase, -back_axles.back().position/wheelbase);
+               const vector<VehicleType::Axle> &back_axles = bogies.back().axles;
+               wheelbase = back_axles.front().position-back_axles.back().position;
+               TrackPoint back_point = get_point(back, wheelbase, -back_axles.back().position/wheelbase);
 
-                       tp = get_point(front_point.pos, back_point.pos, -bogies.back().position/bogie_spacing);
+               tp = get_point(front_point.pos, back_point.pos, -bogies.back().position/bogie_spacing);
 
-                       bogie_dirs.front() = front_point.dir-tp.dir;
-                       bogie_dirs.back() = back_point.dir-tp.dir;
-               }
-               else
-                       tp = track_pos.get_point();
+               bogie_dirs.front() = front_point.dir-tp.dir;
+               bogie_dirs.back() = back_point.dir-tp.dir;
        }
+       else
+               tp = track_pos.get_point();
+
+       if(!prev)
+               check_sensor(type.get_front_axle_offset(), front_sensor);
+       if(!next)
+               check_sensor(type.get_back_axle_offset(), back_sensor);
 
        position = tp.pos;
+       position.z += layout.get_catalogue().get_rail_elevation();
        direction = tp.dir;
 }
 
@@ -209,6 +205,32 @@ void Vehicle::propagate_backward()
                next->propagate_backward();
 }
 
+void Vehicle::check_sensor(float offset, unsigned &sensor)
+{
+       TrackPosition pos = track_pos;
+       pos.advance(offset);
+       unsigned s = pos.track->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_pos.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. */
+                       layout.get_driver().set_sensor(old, false);
+       }
+}
+
 void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, float tdist, float ratio) const
 {
        float margin = 0.01*layout.get_catalogue().get_scale();