Refactor and fix vehicle positioning logic

diff --git a/source/libr2c2/train.cpp b/source/libr2c2/train.cpp
index aa2ba3f528627beb093dc936446b1f2b3bbd4d81..b5f600e09e3a58a9744cbef5e7844cf57252aa78 100644 (file)
--- a/source/libr2c2/train.cpp
+++ b/source/libr2c2/train.cpp
@@ -193,6 +193,7 @@ void Train::place(const BlockIter &block)
 
        allocator.start_from(block);
        accurate_position = false;
+       last_entry_block = BlockIter();
 
        if(reverse)
                vehicles.front()->place(block.reverse().track_iter(), 0, Vehicle::FRONT_BUFFER);
@@ -207,6 +208,7 @@ void Train::unplace()
 
        allocator.clear();
        accurate_position = false;
+       last_entry_block = BlockIter();
 
        for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
                (*i)->unplace();
@@ -309,6 +311,7 @@ void Train::tick(const Time::TimeDelta &dt)
                driver.set_loco_reverse(address, r);
 
                allocator.reverse();
+               last_entry_block = BlockIter();
        }
 
        if(speed_quantizer)
@@ -422,28 +425,28 @@ void Train::loco_func_event(unsigned addr, unsigned func, bool state)
 
 void Train::sensor_state_changed(Sensor &sensor, Sensor::State state)
 {
-       if(state!=Sensor::MAYBE_ACTIVE)
+       if(!current_speed_step || state!=Sensor::MAYBE_ACTIVE)
                return;
 
        Block *block = sensor.get_block();
        if(!block || block->get_train()!=this)
                return;
 
+       if(last_entry_block && &*last_entry_block!=block)
+       {
+               for(BlockIter i=last_entry_block.next(); (i && &*i!=block); i=i.next())
+                       if(i->get_train()!=this || i->get_sensor_id())
+                               return;
+       }
+
        if(dynamic_cast<TrackCircuit *>(&sensor))
        {
-               if(last_entry_block)
+               if(last_entry_block && pure_speed && speed_quantizer)
                {
-                       float travel_distance = -1;
-                       if(pure_speed && speed_quantizer && current_speed_step>0)
-                               travel_distance = 0;
+                       float travel_distance = 0;
 
                        for(BlockIter i=last_entry_block; &*i!=block; i=i.next())
-                       {
-                               if(i->get_sensor_id())
-                                       return;
-                               if(travel_distance>=0)
-                                       travel_distance += i->get_path_length(i.entry());
-                       }
+                               travel_distance += i->get_path_length(i.entry());
 
                        if(travel_distance>0)
                        {