Don't advance the estimated train position outside current blocks

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

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 8f8ee490fe2fa67cf5f33113d2efd0efea03c7c5..5524a5d77c6a2740e24d4894a811dcac75441b44 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -67,7 +67,7 @@ void Train::set_speed(unsigned speed)
        if(!target_speed)
        {
                trfc_mgr.get_control().set_timer(3*Time::sec).signal_timeout.connect(
-                       sigc::bind_return(sigc::mem_fun(this, &Train::release_reserved_blocks), false));
+                       sigc::bind_return(sigc::bind(sigc::mem_fun(this, &Train::release_blocks), sigc::ref(rsv_blocks)), false));
        }
        else
                reserve_more();
@@ -89,46 +89,38 @@ void Train::set_route(const Route *r)
        signal_route_changed.emit(route);
 }
 
-void Train::place(Block *block, unsigned entry)
+void Train::place(Block &block, unsigned entry)
 {
-       for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end();)
-       {
-               i->block->reserve(0);
-               i = rsv_blocks.erase(i);
-       }
+       if(target_speed)
+               set_speed(0);
 
-       for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end();)
-       {
-               i->block->reserve(0);
-               i = cur_blocks.erase(i);
-       }
+       release_blocks(rsv_blocks);
+       release_blocks(cur_blocks);
 
-       if(!block->reserve(this))
+       if(!block.reserve(this))
        {
                set_status("Unplaced");
                return;
        }
 
-       cur_blocks.push_back(BlockRef(block, entry));
-       set_position(block->get_endpoints()[entry]);
+       cur_blocks.push_back(BlockRef(&block, entry));
+       set_position(block.get_endpoints()[entry]);
 
        set_status("Stopped");
 }
 
-bool Train::free_block(Block *block)
+bool Train::free_block(Block &block)
 {
        unsigned nsens = 0;
        for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
        {
-               if(i->block==block)
+               if(i->block==&block)
                {
                        if(nsens<1)
                                return false;
-                       while(i!=rsv_blocks.end())
-                       {
-                               i->block->reserve(0);
-                               i = rsv_blocks.erase(i);
-                       }
+                       for(list<BlockRef>::iterator j=i; j!=rsv_blocks.end(); ++j)
+                               j->block->reserve(0);
+                       rsv_blocks.erase(i, rsv_blocks.end());
                        update_speed();
                        return true;
                }
@@ -139,13 +131,13 @@ bool Train::free_block(Block *block)
        return false;
 }
 
-int Train::get_entry_to_block(Block *block) const
+int Train::get_entry_to_block(Block &block) const
 {
        for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
-               if(i->block==block)
+               if(i->block==&block)
                        return i->entry;
        for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
-               if(i->block==block)
+               if(i->block==&block)
                        return i->entry;
        return -1;
 }
@@ -162,19 +154,25 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
                        path = trfc_mgr.get_control().get_turnout(cur_track->get_turnout_id()).get_path();
 
                offset += get_real_speed(loco.get_speed())*(dt/Time::sec);
-               if(offset>cur_track->get_type().get_path_length(path))
+               float path_len = cur_track->get_type().get_path_length(path);
+               if(offset>path_len)
                {
-                       int out = cur_track->traverse(cur_track_ep, path);
-                       if(out>=0)
+                       unsigned out = cur_track->traverse(cur_track_ep, path);
+                       Track *next = cur_track->get_link(out);
+
+                       bool ok = false;
+                       for(list<BlockRef>::const_iterator i=cur_blocks.begin(); (!ok && i!=cur_blocks.end()); ++i)
+                               ok = i->block->get_tracks().count(next);
+
+                       if(ok)
                        {
-                               Track *next = cur_track->get_link(out);
                                if(next)
                                        cur_track_ep = next->get_endpoint_by_link(*cur_track);
                                cur_track = next;
                                offset = 0;
                        }
                        else
-                               cur_track = 0;
+                               offset = path_len-0.001;
                }
 
                if(cur_track)
@@ -345,7 +343,7 @@ unsigned Train::reserve_more()
                if(!link->reserve(this))
                {
                        // If we found another train going in the same direction as us, we can keep the blocks we got
-                       int other_entry = link->get_train()->get_entry_to_block(link);
+                       int other_entry = link->get_train()->get_entry_to_block(*link);
                        if(other_entry==entry || link->traverse(entry)==link->traverse(other_entry))
                        {
                                good = last;
@@ -525,11 +523,11 @@ void Train::set_position(const Block::Endpoint &bep)
        pos = cur_track->get_endpoint_position(cur_track_ep);
 }
 
-void Train::release_reserved_blocks()
+void Train::release_blocks(list<BlockRef> &blocks)
 {
-       for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+       for(list<BlockRef>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
                i->block->reserve(0);
-       rsv_blocks.clear();
+       blocks.clear();
 }