X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibmarklin%2Ftrain.cpp;h=23c4b52ad30851c9a5567c66900425f5afb6d8bc;hb=c18c79f72c065933b00c6092d9d5054f9e82b8df;hp=cdb8e67d169297925a627c4b7556338316c0d9b6;hpb=2bb6ce8ada216c0b2caa48f4639d03f2c581098e;p=r2c2.git diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp index cdb8e67..23c4b52 100644 --- a/source/libmarklin/train.cpp +++ b/source/libmarklin/train.cpp @@ -46,6 +46,7 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a): address(a), priority(0), yielding_to(0), + cur_blocks_end(blocks.end()), pending_block(0), reserving(false), advancing(false), @@ -204,51 +205,59 @@ void Train::set_timetable(Timetable *tt) timetable = tt; } -void Train::set_route(const Route *r) +bool Train::set_route(const Route *r) { free_noncritical_blocks(); - routes.clear(); - if(r) - routes.push_back(r); - end_of_route = false; - - if(r && !cur_blocks.empty()) + Route *lead = 0; + if(r && !blocks.empty()) { - TrackIter first = cur_blocks.front().track_iter(); - TrackIter next = (rsv_blocks.empty() ? cur_blocks : rsv_blocks).back().next().track_iter(); + TrackIter first = blocks.front().track_iter(); + TrackIter next = blocks.back().next().track_iter(); if(!r->has_track(*next)) { - Route *lead = Route::find(*next, next.entry(), *r); + lead = Route::find(next, *r); + if(!lead) + return false; create_lead_route(lead, lead); routes.push_front(lead); } else if(!r->has_track(*first)) - routes.push_front(create_lead_route(0, r)); + lead = create_lead_route(0, r); } + routes.clear(); + if(lead) + routes.push_back(lead); + if(r) + routes.push_back(r); + end_of_route = false; + reserve_more(); signal_route_changed.emit(get_route()); + + return true; } -void Train::go_to(Track &to) +bool Train::go_to(Track &to) { - for(BlockList::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i) + for(BlockList::const_iterator i=blocks.begin(); i!=cur_blocks_end; ++i) if((*i)->has_track(to)) { signal_arrived.emit(); - set_route(0); - return; + return set_route(0); } free_noncritical_blocks(); - TrackIter next = (rsv_blocks.empty() ? cur_blocks : rsv_blocks).back().next().track_iter(); + TrackIter next = blocks.back().next().track_iter(); - Route *route = Route::find(*next, next.entry(), to); + Route *route = Route::find(next, to); + if(!route) + return false; create_lead_route(route, route); - set_route(route); + return set_route(route); } bool Train::divert(Track &from) @@ -258,78 +267,64 @@ bool Train::divert(Track &from) if(routes.empty()) return false; - int path = -1; - unsigned from_ep = 0; + unsigned path = 0; + unsigned entry = 0; list::iterator route = routes.begin(); - BlockIter block = cur_blocks.back(); - set visited; // Follow our routes to find out where we're entering the turnout - while(1) + for(TrackLoopIter track = blocks.front().track_iter();;) { - block = block.next(route->route); - - const Block::Endpoint &entry_ep = block->get_endpoints()[block.entry()]; - - if(visited.count(entry_ep.track)) - return false; - visited.insert(entry_ep.track); - - if(!advance_route(route, *entry_ep.track)) + if(!advance_route(route, *track)) return false; - if(entry_ep.track==&from) + if(&*track==&from) { - if(block->get_train()==this && !free_block(*block)) + Block &block = track->get_block(); + if(block.get_train()==this && !free_block(block)) return false; - from_ep = entry_ep.track_ep; - path = route->route->get_turnout(from.get_turnout_id()); - break; - } - } + int route_path = route->route->get_turnout(from.get_turnout_id()); - // Check that more than one path is available - unsigned ep_paths = from.get_type().get_endpoints()[from_ep].paths; - if(!(ep_paths&(ep_paths-1))) - return false; + // Check that more than one path is available + unsigned ep_paths = track->get_type().get_endpoints()[track.entry()].paths; + if(!(ep_paths&(ep_paths-1))) + return false; - // Choose some other path - for(int i=0; ep_paths>>i; ++i) - if((ep_paths&(1<>i; ++i) + if((ep_paths&(1<route->get_path(*track)); + + if(!track || track.looped()) + return false; + } + + TrackIter track = TrackIter(&from, entry).next(path); if(!track) return false; - unsigned ep = track->get_endpoint_by_link(from); - set tracks; for(list::iterator i=routes.begin(); i!=routes.end(); ++i) tracks.insert(i->route->get_tracks().begin(), i->route->get_tracks().end()); - Route *diversion = 0; - try - { - diversion = Route::find(*track, ep, tracks); - } - catch(const Msp::Exception &) - { + RefPtr diversion = Route::find(track, tracks); + if(!diversion) return false; - } diversion->set_name("Diversion"); diversion->add_track(from); diversion->set_turnout(from.get_turnout_id(), path); - if(!is_valid_diversion(*diversion, from, from_ep)) - { - delete diversion; + if(!is_valid_diversion(*diversion, TrackIter(&from, entry))) return false; - } // Follow the diversion route until we get back to the original route list::iterator end = routes.end(); @@ -342,10 +337,9 @@ bool Train::divert(Track &from) if(end!=routes.end()) break; else if(!diversion->has_track(*track)) - throw Exception("Pathfinder returned a bad route"); + throw LogicError("Pathfinder returned a bad route"); - unsigned tid = track->get_turnout_id(); - track = track.next(tid ? diversion->get_turnout(tid) : 0); + track = track.next(diversion->get_path(*track)); } if(route==end) @@ -356,7 +350,7 @@ bool Train::divert(Track &from) ++route; routes.erase(route, end); } - routes.insert(end, RouteRef(diversion, from.get_turnout_id())); + routes.insert(end, RouteRef(diversion.release(), from.get_turnout_id())); return true; } @@ -373,8 +367,7 @@ void Train::place(Block &block, unsigned entry) if(controller->get_speed()) throw InvalidState("Must be stopped before placing"); - release_blocks(rsv_blocks); - release_blocks(cur_blocks); + release_blocks(); set_active(false); accurate_position = false; @@ -385,7 +378,7 @@ void Train::place(Block &block, unsigned entry) return; } - cur_blocks.push_back(BlockIter(&block, entry)); + blocks.push_back(BlockIter(&block, entry)); if(reverse) { TrackIter track = BlockIter(&block, entry).reverse().track_iter(); @@ -403,8 +396,7 @@ void Train::unplace() if(controller->get_speed()) throw InvalidState("Must be stopped before unplacing"); - release_blocks(rsv_blocks); - release_blocks(cur_blocks); + release_blocks(); set_active(false); accurate_position = false; @@ -422,13 +414,13 @@ bool Train::free_block(Block &block) return false; unsigned nsens = 0; - for(BlockList::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) + for(BlockList::iterator i=cur_blocks_end; i!=blocks.end(); ++i) { if(i->block()==&block) { if(nsens<1) return false; - release_blocks(rsv_blocks, i, rsv_blocks.end()); + release_blocks(i, blocks.end()); return true; } else if((*i)->get_sensor_id()) @@ -440,12 +432,12 @@ bool Train::free_block(Block &block) void Train::free_noncritical_blocks() { - if(cur_blocks.empty() || rsv_blocks.empty()) + if(blocks.empty()) return; if(controller->get_speed()==0) { - release_blocks(rsv_blocks); + release_blocks(cur_blocks_end, blocks.end()); return; } @@ -455,16 +447,13 @@ void Train::free_noncritical_blocks() Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front()); TrackIter track(veh.get_track(), veh.get_entry()); - BlockList::iterator block = cur_blocks.begin(); + BlockList::iterator block = blocks.begin(); bool in_rsv = false; - while(block!=rsv_blocks.end() && !(*block)->has_track(*track)) + while(block!=blocks.end() && !(*block)->has_track(*track)) { ++block; - if(block==cur_blocks.end()) - { - block = rsv_blocks.begin(); + if(block==cur_blocks_end) in_rsv = true; - } } float dist = veh.get_offset(); @@ -482,17 +471,14 @@ void Train::free_noncritical_blocks() if(!(*block)->has_track(*track)) { ++block; - if(block==cur_blocks.end()) - { - block = rsv_blocks.begin(); + if(block==cur_blocks_end) in_rsv = true; - } - if(block==rsv_blocks.end()) + if(block==blocks.end()) return; if(dist>min_dist && nsens>0) { - release_blocks(rsv_blocks, block, rsv_blocks.end()); + release_blocks(block, blocks.end()); return; } @@ -506,10 +492,7 @@ void Train::free_noncritical_blocks() int Train::get_entry_to_block(Block &block) const { - for(BlockList::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i) - if(i->block()==&block) - return i->entry(); - for(BlockList::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) + for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i) if(i->block()==&block) return i->entry(); return -1; @@ -524,7 +507,7 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) { if(!active && stop_timeout && t>=stop_timeout) { - release_blocks(rsv_blocks); + release_blocks(cur_blocks_end, blocks.end()); end_of_route = false; stop_timeout = Time::TimeStamp(); } @@ -542,8 +525,8 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) reverse = controller->get_reverse(); driver.set_loco_reverse(address, reverse); - release_blocks(rsv_blocks); - reverse_blocks(cur_blocks); + release_blocks(cur_blocks_end, blocks.end()); + reverse_blocks(blocks); reserve_more(); } @@ -569,7 +552,7 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) Track *track = vehicle.get_track(); bool ok = false; - for(BlockList::const_iterator i=cur_blocks.begin(); (!ok && i!=cur_blocks.end()); ++i) + for(BlockList::const_iterator i=blocks.begin(); (!ok && i!=cur_blocks_end); ++i) ok = (*i)->has_track(*track); float d = get_real_speed(current_speed)*(dt/Time::sec); @@ -588,21 +571,21 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) } } } - else if(end_of_route && rsv_blocks.empty()) + else if(end_of_route && cur_blocks_end==blocks.end()) { set_active(false); signal_arrived.emit(); set_route(0); } - if(!cur_blocks.empty() && !cur_blocks.front()->get_sensor_id()) + if(!blocks.empty() && !blocks.front()->get_sensor_id()) { - float dist = get_reserved_distance_until(&*cur_blocks.front(), true); + float dist = get_reserved_distance_until(&*blocks.front(), true); if(dist>10*layout.get_catalogue().get_scale()) { - cur_blocks.front()->reserve(0); - cur_blocks.pop_front(); + blocks.front()->reserve(0); + blocks.pop_front(); } } } @@ -621,16 +604,16 @@ void Train::save(list &st) const if(real_speed[i].weight) st.push_back((DataFile::Statement("real_speed"), i, real_speed[i].speed, real_speed[i].weight)); - if(!cur_blocks.empty()) + if(!blocks.empty() && cur_blocks_end!=blocks.begin()) { - BlockList blocks = cur_blocks; + BlockList blks(blocks.begin(), BlockList::const_iterator(cur_blocks_end)); if(reverse) - reverse_blocks(blocks); + reverse_blocks(blks); - BlockIter prev = blocks.front().flip(); + BlockIter prev = blks.front().flip(); st.push_back((DataFile::Statement("block_hint"), prev->get_id())); - for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i) + for(BlockList::const_iterator i=blks.begin(); i!=blks.end(); ++i) st.push_back((DataFile::Statement("block"), (*i)->get_id())); } @@ -685,12 +668,12 @@ void Train::sensor_event(unsigned addr, bool state) if(state) { // Find the first sensor block from our reserved blocks that isn't this sensor - BlockList::iterator i; + BlockList::iterator end; unsigned result = 0; - for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) - if((*i)->get_sensor_id()) + for(end=cur_blocks_end; end!=blocks.end(); ++end) + if((*end)->get_sensor_id()) { - if((*i)->get_sensor_id()!=addr) + if((*end)->get_sensor_id()!=addr) { if(result==0) result = 2; @@ -703,7 +686,7 @@ void Train::sensor_event(unsigned addr, bool state) result = 3; } - if(result==1 && i!=rsv_blocks.begin()) + if(result==1) { // Compute speed and update related state float travel_time_secs = (Time::now()-last_entry_time)/Time::sec; @@ -719,7 +702,7 @@ void Train::sensor_event(unsigned addr, bool state) } travel_dist = 0; - for(BlockList::iterator j=rsv_blocks.begin(); j!=i; ++j) + for(BlockList::iterator j=cur_blocks_end; j!=end; ++j) { travel_dist += (*j)->get_path_length(j->entry()); @@ -744,7 +727,7 @@ void Train::sensor_event(unsigned addr, bool state) if(routes.size()>1) { const set &rtracks = (++routes.begin())->route->get_tracks(); - for(BlockList::iterator j=rsv_blocks.begin(); j!=i; ++j) + for(BlockList::iterator j=cur_blocks_end; j!=end; ++j) if(rtracks.count((*j)->get_endpoints()[j->entry()].track)) { routes.pop_front(); @@ -755,7 +738,7 @@ void Train::sensor_event(unsigned addr, bool state) } // Move blocks up to the next sensor to our current blocks - cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i); + cur_blocks_end = end; // Try to get more blocks if we're moving if(active) @@ -769,8 +752,8 @@ void Train::sensor_event(unsigned addr, bool state) const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back()); // Find the first sensor in our current blocks that's still active - BlockList::iterator end = cur_blocks.begin(); - for(BlockList::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i) + BlockList::iterator end = blocks.begin(); + for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i) { if((*i)->has_track(*veh.get_track())) break; @@ -786,9 +769,9 @@ void Train::sensor_event(unsigned addr, bool state) } } - if(end!=cur_blocks.begin() && end!=cur_blocks.end()) + if(end!=blocks.begin() && end!=cur_blocks_end) // Free blocks up to the last inactive sensor - release_blocks(cur_blocks, cur_blocks.begin(), end); + release_blocks(blocks.begin(), end); } } @@ -822,23 +805,17 @@ void Train::block_reserved(const Block &block, const Train *train) unsigned Train::reserve_more() { - if(!active) + if(!active || blocks.empty()) return 0; - BlockIter start; - if(!rsv_blocks.empty()) - start = rsv_blocks.back(); - else if(!cur_blocks.empty()) - start = cur_blocks.back(); - else - return 0; + BlockIter start = blocks.back(); pending_block = 0; // See how many sensor blocks and how much track we already have unsigned nsens = 0; float dist = 0; - for(BlockList::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) + for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i) { if((*i)->get_sensor_id()) ++nsens; @@ -911,7 +888,7 @@ unsigned Train::reserve_more() if(blocking_train->free_block(*contested_blocks.back())) { // Roll back and start actually reserving the blocks - block = rsv_blocks.back(); + block = blocks.back(); cur_route = routes.begin(); advance_route(cur_route, *block.track_iter().track()); if(blocking_train->get_priority()==priority) @@ -990,7 +967,7 @@ unsigned Train::reserve_more() if(block->get_turnout_id()) { - const Endpoint &track_ep = track->get_type().get_endpoints()[track.entry()]; + const TrackType::Endpoint &track_ep = track->get_type().get_endpoints()[track.entry()]; bool multiple_paths = (track_ep.paths&(track_ep.paths-1)); if(multiple_paths || !last->get_turnout_id()) @@ -1038,7 +1015,9 @@ unsigned Train::reserve_more() if(!contested_blocks.empty() && contested_blocks.front()==block) contested_blocks.pop_front(); - rsv_blocks.push_back(block); + blocks.push_back(block); + if(cur_blocks_end==blocks.end()) + --cur_blocks_end; if(block->get_sensor_id()) ++nsens; if(nsens>0) @@ -1046,21 +1025,21 @@ unsigned Train::reserve_more() } // Unreserve blocks that were not good - while(!rsv_blocks.empty() && rsv_blocks.back()!=good) + while(blocks.end()!=cur_blocks_end && blocks.back()!=good) { - rsv_blocks.back()->reserve(0); - rsv_blocks.pop_back(); + if(--blocks.end()==cur_blocks_end) + cur_blocks_end = blocks.end(); + blocks.back()->reserve(0); + blocks.pop_back(); } - if(!rsv_blocks.empty() && rsv_blocks.back()!=start) + if(!blocks.back()!=start) // We got some new blocks, so no longer need to yield yielding_to = 0; // Make any sensorless blocks at the beginning immediately current - BlockList::iterator i; - for(i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !(*i)->get_sensor_id()); ++i) ; - if(i!=rsv_blocks.begin()) - cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i); + while(cur_blocks_end!=blocks.end() && !(*cur_blocks_end)->get_sensor_id()) + ++cur_blocks_end; if(try_divert && divert(*divert_track)) return reserve_more(); @@ -1070,28 +1049,20 @@ unsigned Train::reserve_more() float Train::get_reserved_distance_until(const Block *until_block, bool back) const { - if(cur_blocks.empty()) + if(blocks.empty()) return 0; Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front()); const VehicleType &vtype = veh.get_type(); TrackIter track(veh.get_track(), veh.get_entry()); - if(!track) + if(!track) // XXX Probably unnecessary return 0; - BlockList::const_iterator block = cur_blocks.begin(); - while(block!=rsv_blocks.end() && !(*block)->has_track(*track)) - { + BlockList::const_iterator block = blocks.begin(); + while(block!=blocks.end() && !(*block)->has_track(*track)) ++block; - if(block==cur_blocks.end()) - { - if(back) - return 0; - block = rsv_blocks.begin(); - } - } - if(block==rsv_blocks.end() || &**block==until_block) + if(block==blocks.end() || &**block==until_block) return 0; float result = veh.get_offset(); @@ -1111,16 +1082,14 @@ float Train::get_reserved_distance_until(const Block *until_block, bool back) co { if(back) { - if(block==cur_blocks.begin()) + if(block==blocks.begin()) break; --block; } else { ++block; - if(block==cur_blocks.end()) - block = rsv_blocks.begin(); - if(block==rsv_blocks.end()) + if(block==blocks.end()) break; } @@ -1210,25 +1179,28 @@ void Train::set_status(const string &s) signal_status_changed.emit(s); } -void Train::release_blocks(BlockList &blocks) +void Train::release_blocks() { - release_blocks(blocks, blocks.begin(), blocks.end()); + release_blocks(blocks.begin(), blocks.end()); } -void Train::release_blocks(BlockList &blocks, BlockList::iterator begin, BlockList::iterator end) +void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end) { while(begin!=end) { + if(begin==cur_blocks_end) + cur_blocks_end = end; + Block &block = **begin; blocks.erase(begin++); block.reserve(0); } } -void Train::reverse_blocks(BlockList &blocks) const +void Train::reverse_blocks(BlockList &blks) const { - blocks.reverse(); - for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i) + blks.reverse(); + for(BlockList::iterator i=blks.begin(); i!=blks.end(); ++i) *i = i->reverse(); } @@ -1257,15 +1229,12 @@ Route *Train::create_lead_route(Route *lead, const Route *target) } set tracks; - for(BlockList::iterator i=cur_blocks.begin(); i!=rsv_blocks.end(); ) + for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i) { const set &btracks = (*i)->get_tracks(); for(set::const_iterator j=btracks.begin(); j!=btracks.end(); ++j) if(!target || !target->has_track(**j)) tracks.insert(*j); - - if(++i==cur_blocks.end()) - i = rsv_blocks.begin(); } lead->add_tracks(tracks); @@ -1273,48 +1242,50 @@ Route *Train::create_lead_route(Route *lead, const Route *target) return lead; } -bool Train::is_valid_diversion(const Route &diversion, Track &from, unsigned from_ep) +bool Train::is_valid_diversion(const Route &diversion, const TrackIter &from) { float diversion_len = 0; - TrackIter track(&from, from_ep); - while(diversion.has_track(*track)) + TrackLoopIter track1 = from; + while(diversion.has_track(*track1)) { - unsigned tid = track->get_turnout_id(); - unsigned path = (tid ? diversion.get_turnout(tid) : 0); - diversion_len += track->get_type().get_path_length(path); + unsigned path = diversion.get_path(*track1); + diversion_len += track1->get_type().get_path_length(path); - track = track.next(path); + track1 = track1.next(path); - if(&*track==&from) + if(track1.looped()) return false; } list::iterator route = routes.begin(); - if(!advance_route(route, from)) + if(!advance_route(route, *from)) return false; - set visited; float route_len = 0; - track = TrackIter(&from, from_ep); + TrackLoopIter track2 = from; while(1) { - unsigned tid = track->get_turnout_id(); - unsigned path = (tid ? route->route->get_turnout(tid) : 0); - route_len += track->get_type().get_path_length(path); + unsigned path = route->route->get_path(*track2); + route_len += track2->get_type().get_path_length(path); + + bool ok = (track2!=from && diversion.has_track(*track2)); - if(&*track!=&from && diversion.has_track(*track)) + track2 = track2.next(path); + + if(ok) break; - if(visited.count(&*track)) + if(track2.looped()) return false; - visited.insert(&*track); - track = track.next(path); - - if(!advance_route(route, *track)) + if(!advance_route(route, *track2)) return false; } + // Must end up at the same place through both routes + if(track2!=track1) + return false; + return diversion_lenplace(*track, track.entry(), offset, Vehicle::BACK_BUFFER); @@ -1387,7 +1358,7 @@ void Train::Loader::block(unsigned id) entry = 0; blk->reserve(&obj); - obj.cur_blocks.push_back(BlockIter(blk, entry)); + obj.blocks.push_back(BlockIter(blk, entry)); if(blk->get_sensor_id()) obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true); @@ -1432,9 +1403,9 @@ void Train::Loader::timetable() load_sub(*obj.timetable); } -void Train::Loader::vehicle(unsigned n) +void Train::Loader::vehicle(ArticleNumber art_nr) { - const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(n); + const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(art_nr); Vehicle *veh = new Vehicle(obj.layout, vtype); obj.vehicles.back()->attach_back(*veh); obj.vehicles.push_back(veh);