X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Ftrain.cpp;h=91387d85297e7f2a6b5bed13a1b620cf01f687b9;hb=5c1ddd2f213af3fea15237e02f7da112c0abba36;hp=a45675c90c146e078427bb754848b6b907d806d5;hpb=5b1b2196908c159d4a1339bce2682afa22700c35;p=r2c2.git diff --git a/source/libr2c2/train.cpp b/source/libr2c2/train.cpp index a45675c..91387d8 100644 --- a/source/libr2c2/train.cpp +++ b/source/libr2c2/train.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -23,19 +24,6 @@ using namespace std; using namespace Msp; -namespace { - -struct SetFlag -{ - bool &flag; - - SetFlag(bool &f): flag(f) { flag = true; } - ~SetFlag() { flag = false; } -}; - -} - - namespace R2C2 { Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p): @@ -44,9 +32,7 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p): address(a), protocol(p), preceding_train(0), - cur_blocks_end(blocks.end()), - pending_block(0), - reserving(false), + allocator(*this), advancing(false), controller(new SimpleController), active(false), @@ -54,7 +40,6 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p): speed_changing(false), reverse(false), functions(0), - travel_dist(0), pure_speed(false), speed_quantizer(0), accurate_position(false), @@ -75,16 +60,10 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p): layout.get_driver().signal_loco_speed.connect(sigc::mem_fun(this, &Train::loco_speed_event)); layout.get_driver().signal_loco_function.connect(sigc::mem_fun(this, &Train::loco_func_event)); - layout.signal_block_reserved.connect(sigc::mem_fun(this, &Train::block_reserved)); layout.signal_block_state_changed.connect(sigc::mem_fun(this, &Train::block_state_changed)); layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &Train::halt_event)); - const set &tracks = layout.get_tracks(); - for(set::const_iterator i=tracks.begin(); i!=tracks.end(); ++i) - if((*i)->get_turnout_id()) - (*i)->signal_path_changed.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &Train::turnout_path_changed), sigc::ref(**i)))); - controller->signal_control_changed.connect(sigc::mem_fun(this, &Train::control_changed)); } @@ -157,7 +136,7 @@ void Train::set_active(bool a) if(active) { stop_timeout = Time::TimeStamp(); - reserve_more(); + allocator.reserve_more(); } else stop_timeout = Time::now()+2*Time::sec; @@ -206,48 +185,28 @@ void Train::remove_ai(TrainAI &ai) ais.erase(i); } -TrainAI *Train::get_tagged_ai(const string &tag) const -{ - for(list::const_iterator i=ais.begin(); i!=ais.end(); ++i) - if((*i)->get_tag()==tag) - return *i; - - return 0; -} - void Train::ai_message(const TrainAI::Message &msg) { for(list::iterator i=ais.begin(); i!=ais.end(); ++i) (*i)->message(msg); } -void Train::place(Block &block, unsigned entry) +void Train::place(const BlockIter &block) { + if(!block) + throw invalid_argument("Train::place"); if(controller->get_speed()) throw logic_error("moving"); - release_blocks(); - set_active(false); accurate_position = false; - blocks.push_back(BlockIter(&block, entry)); - if(!block.reserve(this)) - { - blocks.pop_back(); - return; - } + allocator.start_from(block); if(reverse) - { - TrackIter track = BlockIter(&block, entry).reverse().track_iter(); - vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_BUFFER); - } + vehicles.front()->place(block.reverse().track_iter(), 0, Vehicle::FRONT_BUFFER); else - { - const Block::Endpoint &bep = block.get_endpoint(entry); - vehicles.back()->place(*bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER); - } + vehicles.back()->place(block.track_iter(), 0, Vehicle::BACK_BUFFER); } void Train::unplace() @@ -255,7 +214,7 @@ void Train::unplace() if(controller->get_speed()) throw logic_error("moving"); - release_blocks(); + allocator.clear(); set_active(false); accurate_position = false; @@ -266,131 +225,74 @@ void Train::unplace() void Train::stop_at(Block *block) { - stop_at_block = block; + allocator.stop_at(block); + if(active && !block) + allocator.reserve_more(); } bool Train::free_block(Block &block) { - if(get_reserved_distance_until(&block, false)get_braking_distance()*1.3) + if(get_reserved_distance_until(&block)get_braking_distance()*1.3) return false; - unsigned nsens = 0; - for(BlockList::iterator i=cur_blocks_end; i!=blocks.end(); ++i) - { - if(i->block()==&block) - { - if(nsens<1) - return false; - release_blocks(i, blocks.end()); - return true; - } - else if((*i)->get_sensor_id()) - ++nsens; - } - - return false; + return allocator.release_from(block); } void Train::free_noncritical_blocks() { - if(blocks.empty()) + if(allocator.empty()) return; if(controller->get_speed()==0) { - release_blocks(cur_blocks_end, blocks.end()); + allocator.release_noncurrent(); return; } float margin = 10*layout.get_catalogue().get_scale(); float min_dist = controller->get_braking_distance()*1.3+margin; - Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front()); - - TrackIter track(veh.get_track(), veh.get_entry()); - BlockList::iterator block = blocks.begin(); - bool in_rsv = false; - while(block!=blocks.end() && !(*block)->has_track(*track)) - { - ++block; - if(block==cur_blocks_end) - in_rsv = true; - } - - float dist = veh.get_offset(); - if(reverse) - track.reverse(); - else - dist = track->get_type().get_path_length(track->get_active_path())-dist; - dist -= veh.get_type().get_length()/2; - - bool nsens = 0; - while(1) + BlockIter i = allocator.last_current().next(); + float dist = 0; + bool sensor_seen = false; + for(; i->get_train()==this; i=i.next()) { - track = track.next(); - - if(!(*block)->has_track(*track)) + if(dist>min_dist && sensor_seen) { - ++block; - if(block==cur_blocks_end) - in_rsv = true; - if(block==blocks.end()) - return; - - if(dist>min_dist && nsens>0) - { - release_blocks(block, blocks.end()); - return; - } - - if(in_rsv && (*block)->get_sensor_id()) - ++nsens; + allocator.release_from(*i); + return; } - dist += track->get_type().get_path_length(track->get_active_path()); - } -} - -const BlockIter &Train::get_head_block() const -{ - if(blocks.empty()) - throw logic_error("no blocks"); - return blocks.back(); -} + dist += i->get_path_length(i.entry()); -const BlockIter &Train::get_tail_block() const -{ - if(blocks.empty()) - throw logic_error("no blocks"); - return blocks.front(); -} - -int Train::get_entry_to_block(const Block &block) const -{ - for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i) - if(i->block()==&block) - return i->entry(); - return -1; + if(i->get_sensor_id()) + sensor_seen = true; + } } float Train::get_reserved_distance() const { - if(blocks.empty()) + if(allocator.empty()) return 0; float margin = 0; - TrackIter next = blocks.back().next().track_iter(); + TrackIter next = allocator.last().next().track_iter(); if(next && next->get_type().is_turnout()) margin = 15*layout.get_catalogue().get_scale(); - return max(get_reserved_distance_until(pending_block, false)-margin, 0.0f); + return max(get_reserved_distance_until(0)-margin, 0.0f); +} + +void Train::reserve_more() +{ + allocator.reserve_more(); } void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) { if(!active && stop_timeout && t>=stop_timeout) { - release_blocks(cur_blocks_end, blocks.end()); + allocator.release_noncurrent(); stop_timeout = Time::TimeStamp(); } @@ -410,10 +312,9 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) r = !r; driver.set_loco_reverse(address, r); - release_blocks(cur_blocks_end, blocks.end()); - reverse_blocks(blocks); - - reserve_more(); + allocator.reverse(); + if(active) + allocator.reserve_more(); } if(speed_quantizer) @@ -436,14 +337,9 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) set_active(true); Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front()); - Track *track = vehicle.get_track(); - - bool ok = false; - for(BlockList::const_iterator i=blocks.begin(); (!ok && i!=cur_blocks_end); ++i) - ok = (*i)->has_track(*track); float d = speed*(dt/Time::sec); - if(ok) + if(allocator.is_block_current(vehicle.get_track()->get_block())) { SetFlag setf(advancing); vehicle.advance(reverse ? -d : d); @@ -458,18 +354,6 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) } } } - - if(!blocks.empty() && !blocks.front()->get_sensor_id()) - { - float dist = get_reserved_distance_until(&*blocks.front(), true); - - if(dist>10*layout.get_catalogue().get_scale()) - { - Block &block = *blocks.front(); - blocks.pop_front(); - block.reserve(0); - } - } } void Train::save(list &st) const @@ -487,17 +371,10 @@ void Train::save(list &st) const st.push_back(ss); } - if(!blocks.empty() && cur_blocks_end!=blocks.begin()) { - BlockList blks(blocks.begin(), BlockList::const_iterator(cur_blocks_end)); - if(reverse) - reverse_blocks(blks); - - BlockIter prev = blks.front().flip(); - st.push_back((DataFile::Statement("block_hint"), prev->get_id())); - - for(BlockList::const_iterator i=blks.begin(); i!=blks.end(); ++i) - st.push_back((DataFile::Statement("block"), (*i)->get_id())); + DataFile::Statement ss("blocks"); + allocator.save(ss.sub); + st.push_back(ss); } // XXX Need more generic way of saving AI state @@ -553,302 +430,98 @@ void Train::loco_func_event(unsigned addr, unsigned func, bool state) void Train::block_state_changed(Block &block, Block::State state) { - if(state==Block::MAYBE_ACTIVE) + if(block.get_train()==this && state==Block::MAYBE_ACTIVE) { - // Find the first sensor block from our reserved blocks that isn't this sensor - BlockList::iterator end; - unsigned result = 0; - for(end=cur_blocks_end; end!=blocks.end(); ++end) - if((*end)->get_sensor_id()) - { - if(&**end!=&block) - { - if(result==0) - result = 2; - else if(result==1) - break; - } - else if(result==0) - result = 1; - else if(result==2) - result = 3; - } - - if(result==1) + if(last_entry_block) { - // Compute speed and update related state - float travel_time_secs = (Time::now()-last_entry_time)/Time::sec; + float travel_distance = -1; + if(pure_speed && speed_quantizer && current_speed_step>0) + travel_distance = 0; - if(pure_speed && speed_quantizer && current_speed_step>0 && travel_time_secs>=2) - speed_quantizer->learn(current_speed_step, travel_dist/travel_time_secs, travel_time_secs); + for(BlockIter i=last_entry_block; &*i!=█ i=i.next()) + { + if(i->get_sensor_id()) + return; + if(travel_distance>=0) + travel_distance += i->get_path_length(i.entry()); + } - travel_dist = 0; - for(BlockList::iterator j=cur_blocks_end; j!=end; ++j) + if(travel_distance>0) { - travel_dist += (*j)->get_path_length(j->entry()); - - if(&**j==&block && !advancing) - { - TrackIter track = j->track_iter(); - if(reverse) - { - track = track.flip(); - vehicles.back()->place(*track, track.entry(), 0, Vehicle::BACK_AXLE); - } - else - vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_AXLE); - } + float travel_time_secs = (Time::now()-last_entry_time)/Time::sec; + + if(travel_time_secs>=2) + speed_quantizer->learn(current_speed_step, travel_distance/travel_time_secs, travel_time_secs); } - last_entry_time = Time::now(); - pure_speed = true; - accurate_position = true; - overshoot_dist = 0; - - // Move blocks up to the next sensor to our current blocks - for(BlockList::iterator j=cur_blocks_end; j!=end; ++j) - signal_advanced.emit(**j); - cur_blocks_end = end; - - // Try to get more blocks if we're moving - if(active) - reserve_more(); } - else if(result==3) - layout.emergency("Sensor for "+name+" triggered out of order"); - } - else if(state==Block::INACTIVE) - { - const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back()); - // Find the first sensor in our current blocks that's still active - BlockList::iterator end = blocks.begin(); - for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i) + last_entry_block = allocator.iter_for(block); + last_entry_time = Time::now(); + pure_speed = true; + accurate_position = true; + overshoot_dist = 0; + + if(!advancing && vehicles.front()->get_track()) { - if((*i)->has_track(*veh.get_track())) - break; - if((*i)->get_sensor_id()) + TrackIter track = last_entry_block.track_iter(); + if(reverse) { - if(layout.get_driver().get_sensor((*i)->get_sensor_id())) - break; - else - { - end = i; - ++end; - } + track = track.flip(); + vehicles.back()->place(track, 0, Vehicle::BACK_AXLE); } + else + vehicles.front()->place(track, 0, Vehicle::FRONT_AXLE); } - - if(end!=blocks.begin() && end!=cur_blocks_end) - // Free blocks up to the last inactive sensor - release_blocks(blocks.begin(), end); } } -void Train::turnout_path_changed(Track &track) -{ - for(list::iterator i=blocks.begin(); i!=blocks.end(); ++i) - if((*i)->get_turnout_id()==track.get_turnout_id() && !reserving && &**i==pending_block) - reserve_more(); -} - void Train::halt_event(bool h) { if(h) accurate_position = false; } -void Train::block_reserved(const Block &block, const Train *train) -{ - if(&block==pending_block && !train && !reserving) - reserve_more(); -} - -void Train::reserve_more() -{ - if(!active || blocks.empty()) - return; - - BlockIter start = blocks.back(); - if(&*start==stop_at_block) - return; - else if(&*start==pending_block) - { - TrackIter track = start.track_iter(); - if(!(track.endpoint().paths&(1<get_active_path()))) - return; - } - - pending_block = 0; - preceding_train = 0; - - // See how many sensor blocks and how much track we already have - unsigned nsens = 0; - float dist = 0; - for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i) - { - if((*i)->get_sensor_id()) - ++nsens; - if(nsens>0) - dist += (*i)->get_path_length(i->entry()); - } - - float approach_margin = 50*layout.get_catalogue().get_scale(); - float min_dist = controller->get_braking_distance()*1.3+approach_margin*2; - - BlockIter block = start; - - SetFlag setf(reserving); - - while(1) - { - BlockIter last = block; - block = block.next(); - if(!block || block->get_endpoints().size()<2) - // The track ends here - break; - - if(block->get_turnout_id() && !last->get_turnout_id()) - { - /* We are arriving at a turnout. See if we have enough blocks and - distance reserved. */ - if(nsens>=3 && dist>=min_dist) - break; - } - - blocks.push_back(block); - if(!block->reserve(this)) - { - blocks.pop_back(); - pending_block = &*block; - break; - } - - if(cur_blocks_end==blocks.end()) - --cur_blocks_end; - - TrackIter track = block.track_iter(); - if(track->is_path_changing()) - { - pending_block = &*block; - break; - } - else - { - const TrackType::Endpoint &entry_ep = track.endpoint(); - unsigned path = track->get_active_path(); - if(!(entry_ep.paths&(1<1; ++path, mask>>=1) ; - - track->set_active_path(path); - if(track->is_path_changing()) - { - pending_block = &*block; - break; - } - } - else - // XXX Do something here - break; - } - } - - if(&*block==stop_at_block) - break; - - if(block->get_sensor_id()) - ++nsens; - if(nsens>0) - dist += block->get_path_length(block.entry()); - } - - // Make any sensorless blocks at the beginning immediately current - while(cur_blocks_end!=blocks.end() && !(*cur_blocks_end)->get_sensor_id()) - ++cur_blocks_end; -} - -float Train::get_reserved_distance_until(const Block *until_block, bool back) const +float Train::get_reserved_distance_until(const Block *until_block) const { - if(blocks.empty()) + if(allocator.empty()) return 0; - Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front()); - const VehicleType &vtype = veh.get_type(); + Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front()); - TrackIter track(veh.get_track(), veh.get_entry()); + TrackIter track = veh.get_track_iter(); if(!track) // XXX Probably unnecessary return 0; - BlockList::const_iterator block = blocks.begin(); - while(block!=blocks.end() && !(*block)->has_track(*track)) - ++block; - if(block==blocks.end() || &**block==until_block) + BlockIter block = track.block_iter(); + if(&*block==until_block) return 0; + // Account for the vehicle's offset on its current track float result = veh.get_offset(); - if(reverse!=back) + if(reverse) track = track.reverse(); else result = track->get_type().get_path_length(track->get_active_path())-result; - result -= vtype.get_length()/2; - - while(1) - { - track = track.next(); - if(!track) - break; - - if(!(*block)->has_track(*track)) - { - if(back) - { - if(block==blocks.begin()) - break; - --block; - } - else - { - ++block; - if(block==blocks.end()) - break; - } - - if(&**block==until_block) - break; - } + result -= veh.get_type().get_length()/2; + // Count remaining distance in the vehicle's current block + for(track=track.next(); &track->get_block()==&*block; track=track.next()) result += track->get_type().get_path_length(track->get_active_path()); - } - - return result; -} -void Train::release_blocks() -{ - release_blocks(blocks.begin(), blocks.end()); -} + const BlockIter &last = allocator.last(); + if(&*block==&*last) + return result; -void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end) -{ - while(begin!=end) + // Count any remaining blocks + for(block=block.next(); (&*block!=until_block && block->get_train()==this); block=block.next()) { - if(begin==cur_blocks_end) - cur_blocks_end = end; + result += block->get_path_length(block.entry()); - Block &block = **begin; - blocks.erase(begin++); - block.reserve(0); + if(&*block==&*last) + break; } -} -void Train::reverse_blocks(BlockList &blks) const -{ - blks.reverse(); - for(BlockList::iterator i=blks.begin(); i!=blks.end(); ++i) - *i = i->reverse(); + return result; } @@ -857,8 +530,7 @@ Train::Loader::Loader(Train &t): prev_block(0), blocks_valid(true) { - add("block", &Loader::block); - add("block_hint", &Loader::block_hint); + add("blocks", &Loader::blocks); add("name", &Loader::name); add("quantized_speed", &Loader::quantized_speed); add("router", &Loader::router); @@ -868,55 +540,17 @@ Train::Loader::Loader(Train &t): void Train::Loader::finish() { - if(!obj.blocks.empty()) + if(!obj.allocator.empty()) { - TrackIter track = obj.blocks.front().track_iter(); + TrackIter track = obj.allocator.first().track_iter(); float offset = 2*obj.layout.get_catalogue().get_scale(); - obj.vehicles.back()->place(*track, track.entry(), offset, Vehicle::BACK_BUFFER); + obj.vehicles.back()->place(track, offset, Vehicle::BACK_BUFFER); } } -void Train::Loader::block(unsigned id) +void Train::Loader::blocks() { - if(!blocks_valid) - return; - - Block *blk; - try - { - blk = &obj.layout.get_block(id); - } - catch(const key_error &) - { - blocks_valid = false; - return; - } - - int entry = -1; - if(prev_block) - entry = blk->get_endpoint_by_link(*prev_block); - if(entry<0) - entry = 0; - - obj.blocks.push_back(BlockIter(blk, entry)); - blk->reserve(&obj); - - if(blk->get_sensor_id()) - obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true); - - prev_block = blk; -} - -void Train::Loader::block_hint(unsigned id) -{ - try - { - prev_block = &obj.layout.get_block(id); - } - catch(const key_error &) - { - blocks_valid = false; - } + load_sub(obj.allocator); } void Train::Loader::name(const string &n)