From 306cf0a8709a781b3031aa05a7b353a9b3a308e5 Mon Sep 17 00:00:00 2001 From: Mikko Rasa Date: Tue, 23 Mar 2010 21:14:25 +0000 Subject: [PATCH] If a train is not on the specified route, find a route to it --- source/libmarklin/route.cpp | 40 +++++++++--- source/libmarklin/route.h | 1 + source/libmarklin/train.cpp | 121 +++++++++++++++++++++++++++--------- source/libmarklin/train.h | 4 +- 4 files changed, 127 insertions(+), 39 deletions(-) diff --git a/source/libmarklin/route.cpp b/source/libmarklin/route.cpp index 13f4e3e..2ff6962 100644 --- a/source/libmarklin/route.cpp +++ b/source/libmarklin/route.cpp @@ -48,11 +48,20 @@ struct TrackMatch TrackMatch(const Track &t): track(t) { } - bool operator()(const Track &t) { return &t==&track; } + bool operator()(const Track &t) const { return &t==&track; } +}; + +struct TrackInRoute +{ + const Route &route; + + TrackInRoute(const Route &r): route(r) { } + + bool operator()(const Track &t) const { return route.get_tracks().count(&t); } }; template -list dijkstra(const Track &from, unsigned ep, Pred goal) +list dijkstra(const Track &from, unsigned ep, const Pred &goal) { // XXX Fails to find some routes - should use track+ep as key map track_nodes; @@ -104,6 +113,20 @@ list dijkstra(const Track &from, unsigned ep, Pred goal) return result; } +unsigned count = 0; + +template +Route *create_route(const Track &from, unsigned ep, const Pred &goal) +{ + list tracks = dijkstra(from, ep, goal); + + Route *route = new Route(from.get_layout(), format("-%d-", ++count)); + for(list::iterator i=tracks.begin(); i!=tracks.end(); ++i) + route->add_track(**i); + + return route; +} + } @@ -295,15 +318,12 @@ void Route::track_removed(Track &t) Route *Route::find(const Track &from, unsigned ep, const Track &to) { - TrackMatch goal(to); - list tracks = dijkstra(from, ep, goal); - - static unsigned n = 0; - Route *route = new Route(from.get_layout(), format("Route::find %d", ++n)); - for(list::iterator i=tracks.begin(); i!=tracks.end(); ++i) - route->add_track(**i); + return create_route(from, ep, TrackMatch(to)); +} - return route; +Route *Route::find(const Track &from, unsigned ep, const Route &to) +{ + return create_route(from, ep, TrackInRoute(to)); } diff --git a/source/libmarklin/route.h b/source/libmarklin/route.h index 14ac94c..89788e3 100644 --- a/source/libmarklin/route.h +++ b/source/libmarklin/route.h @@ -54,6 +54,7 @@ private: public: static Route *find(const Track &, unsigned, const Track &); + static Route *find(const Track &, unsigned, const Route &); }; } // namespace Marklin diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp index 69e7212..f745313 100644 --- a/source/libmarklin/train.cpp +++ b/source/libmarklin/train.cpp @@ -31,6 +31,7 @@ Train::Train(Layout &l, const LocoType &t, unsigned a): reverse(false), functions(0), route(0), + next_route(0), end_of_route(false), status("Unplaced"), travel_dist(0), @@ -137,8 +138,21 @@ void Train::set_route(const Route *r) } route = r; + next_route = 0; end_of_route = false; + if(route) + { + BlockRef &last = (rsv_blocks.empty() ? cur_blocks.back() : rsv_blocks.back()); + BlockRef next = last.next(); + const Block::Endpoint &ep = next.block->get_endpoints()[next.entry]; + if(!route->get_tracks().count(ep.track)) + { + next_route = route; + route = Route::find(*ep.track, ep.track_ep, *next_route); + } + } + if(target_speed && reserve_more()<2) update_speed(); @@ -147,6 +161,14 @@ void Train::set_route(const Route *r) void Train::go_to(const Track &to) { + for(list::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i) + if(i->block->get_tracks().count(const_cast(&to))) + { + set_speed(0); + set_route(0); + return; + } + BlockRef *last = 0; if(rsv_blocks.empty()) last = &cur_blocks.back(); @@ -157,15 +179,8 @@ void Train::go_to(const Track &to) last = &*i; } - Block *next = last->block->get_endpoints()[last->block->traverse(last->entry)].link; - if(!next) - throw InvalidState("At end of line"); - - int entry = next->get_endpoint_by_link(*last->block); - if(entry<0) - throw LogicError("Block links are inconsistent"); - - const Block::Endpoint &ep = next->get_endpoints()[entry]; + BlockRef next = last->next(); + const Block::Endpoint &ep = next.block->get_endpoints()[next.entry]; set_route(Route::find(*ep.track, ep.track_ep, to)); } @@ -267,8 +282,6 @@ void Train::save(list &st) const for(unsigned i=0; i<=14; ++i) if(real_speed[i].weight) st.push_back((DataFile::Statement("real_speed"), i, real_speed[i].speed, real_speed[i].weight)); - if(route) - st.push_back((DataFile::Statement("route"), route->get_name())); if(!cur_blocks.empty()) { @@ -282,6 +295,9 @@ void Train::save(list &st) const for(list::const_iterator i=blocks.begin(); i!=blocks.end(); ++i) st.push_back((DataFile::Statement("block"), i->block->get_id())); } + + if(route) + st.push_back((DataFile::Statement("route"), route->get_name())); } void Train::loco_speed_event(unsigned addr, unsigned speed, bool rev) @@ -346,6 +362,21 @@ void Train::sensor_event(unsigned addr, bool state) last_entry_time = Time::now(); pure_speed = true; + // Check if we've reached the next route + if(next_route) + { + const set &rtracks = next_route->get_tracks(); + for(list::iterator j=rsv_blocks.begin(); j!=i; ++j) + if(rtracks.count(j->block->get_endpoints()[j->entry].track)) + { + route = next_route; + next_route = 0; + // XXX Exceptions? + signal_route_changed.emit(route); + break; + } + } + // Move blocks up to the next sensor to our current blocks cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i); @@ -419,8 +450,18 @@ unsigned Train::reserve_more() for(list::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) if(i->block->get_sensor_id()) ++nsens; + + const Route *cur_route = 0; + if(route) + { + unsigned exit = last->block->traverse(last->entry); + Track *track = last->block->get_endpoints()[exit].track; + if(route->get_tracks().count(track)) + cur_route = route; + else if(next_route && next_route->get_tracks().count(track)) + cur_route = next_route; + } - bool on_route = (route && route->get_tracks().count(last->block->get_endpoints()[last->entry].track)); bool got_more = false; BlockRef *good = last; unsigned good_sens = nsens; @@ -436,14 +477,23 @@ unsigned Train::reserve_more() if(entry<0) throw LogicError("Block links are inconsistent!"); - if(on_route && !route->get_tracks().count(link->get_endpoints()[entry].track)) + const Block::Endpoint &entry_ep = link->get_endpoints()[entry]; + + if(cur_route) { - // Keep the blocks if we arrived at the end of the route - good = last; - good_sens = nsens; - end_of_route = true; - break; + if(cur_route!=next_route && next_route && next_route->get_tracks().count(entry_ep.track)) + cur_route = next_route; + else if(!cur_route->get_tracks().count(entry_ep.track)) + { + // Keep the blocks if we arrived at the end of the route + good = last; + good_sens = nsens; + end_of_route = true; + break; + } } + else if(route && route->get_tracks().count(entry_ep.track)) + cur_route = route; if(!link->reserve(this)) { @@ -462,8 +512,7 @@ unsigned Train::reserve_more() if(link->get_turnout_id()) { - const Block::Endpoint &ep = link->get_endpoints()[entry]; - const Endpoint &track_ep = ep.track->get_type().get_endpoints()[ep.track_ep]; + const Endpoint &track_ep = entry_ep.track->get_type().get_endpoints()[entry_ep.track_ep]; // Keep the blocks reserved so far, as either us or the other train can diverge good = last; @@ -471,10 +520,10 @@ unsigned Train::reserve_more() // Figure out what path we'd like to take on the turnout int path = -1; - if(route) - path = route->get_turnout(link->get_turnout_id()); + if(cur_route) + path = cur_route->get_turnout(link->get_turnout_id()); if(path<0) - path = ep.track->get_active_path(); + path = entry_ep.track->get_active_path(); if(!((track_ep.paths>>path)&1)) { for(unsigned i=0; track_ep.paths>>i; ++i) @@ -482,12 +531,12 @@ unsigned Train::reserve_more() path = i; } - if(path!=static_cast(ep.track->get_active_path())) + if(path!=static_cast(entry_ep.track->get_active_path())) { // The turnout is set to wrong path - switch and wait for it link->reserve(0); pending_block = link; - ep.track->set_active_path(path); + entry_ep.track->set_active_path(path); break; } } @@ -499,9 +548,6 @@ unsigned Train::reserve_more() ++nsens; got_more = true; } - - if(route && !on_route) - on_route = route->get_tracks().count(link->get_endpoints()[entry].track); } // Unreserve blocks that were not good @@ -652,6 +698,25 @@ void Train::reverse_blocks(list &blocks) const } +Train::BlockRef::BlockRef(Block *b, unsigned e): + block(b), + entry(e) +{ } + +Train::BlockRef Train::BlockRef::next() const +{ + Block *blk = block->get_endpoints()[block->traverse(entry)].link; + if(!blk) + throw InvalidState("At end of line"); + + int ep = blk->get_endpoint_by_link(*block); + if(ep<0) + throw LogicError("Block links are inconsistent"); + + return BlockRef(blk, ep); +} + + Train::RealSpeed::RealSpeed(): speed(0), weight(0) diff --git a/source/libmarklin/train.h b/source/libmarklin/train.h index a89dc93..b579d7f 100644 --- a/source/libmarklin/train.h +++ b/source/libmarklin/train.h @@ -50,7 +50,8 @@ private: Block *block; unsigned entry; - BlockRef(Block *s, unsigned e): block(s), entry(e) { } + BlockRef(Block *, unsigned); + BlockRef next() const; }; struct RealSpeed @@ -75,6 +76,7 @@ private: Msp::Time::TimeStamp stop_timeout; unsigned functions; const Route *route; + const Route *next_route; bool end_of_route; std::string status; -- 2.45.2