From d27edb207810f0b4e44be8c3632d421faa5fed80 Mon Sep 17 00:00:00 2001 From: Mikko Rasa Date: Sat, 20 Mar 2010 11:42:43 +0000 Subject: [PATCH] Add a pathfinder function to Route Implement train go-to functionality Stop train when arriving at the end of its route Fix a bug with multi-address turnouts Add a Z offset to Path3D Rewrite track picking in Engineer --- source/3d/path.cpp | 9 ++- source/3d/path.h | 2 + source/engineer/engineer.cpp | 88 ++++++++++++++--------- source/engineer/engineer.h | 14 ++-- source/engineer/trainpanel.cpp | 54 +++++++++++++- source/engineer/trainpanel.h | 4 ++ source/engineer/trainproperties.cpp | 1 - source/libmarklin/route.cpp | 107 ++++++++++++++++++++++++++++ source/libmarklin/route.h | 3 + source/libmarklin/track.h | 1 + source/libmarklin/train.cpp | 83 +++++++++++++++++++-- source/libmarklin/train.h | 2 + 12 files changed, 320 insertions(+), 48 deletions(-) diff --git a/source/3d/path.cpp b/source/3d/path.cpp index 1128b67..e413ace 100644 --- a/source/3d/path.cpp +++ b/source/3d/path.cpp @@ -19,7 +19,8 @@ namespace Marklin { Path3D::Path3D(const Track3D &t): track(t), paths(0), - automatic(true) + automatic(true), + z_offs(0) { track.get_layout().get_path_scene().add(*this); } @@ -55,6 +56,11 @@ void Path3D::set_color(const GL::Color &c) color = c; } +void Path3D::set_layer(float l) +{ + z_offs = l*track.get_track().get_layout().get_catalogue().get_gauge()*0.01; +} + void Path3D::render(const GL::Tag &tag) const { if(tag==0) @@ -66,6 +72,7 @@ void Path3D::render(const GL::Tag &tag) const GL::PushMatrix push_mat; track.apply_matrix(); + GL::translate(0, 0, z_offs); glColor4f(color.r, color.g, color.b, color.a); for(unsigned i=0; mask; ++i, mask>>=1) diff --git a/source/3d/path.h b/source/3d/path.h index 56e7a2f..dd071f7 100644 --- a/source/3d/path.h +++ b/source/3d/path.h @@ -23,6 +23,7 @@ private: unsigned paths; bool automatic; Msp::GL::Color color; + float z_offs; public: Path3D(const Track3D &); @@ -32,6 +33,7 @@ public: void set_path(unsigned); void set_mask(unsigned); void set_color(const Msp::GL::Color &); + void set_layer(float); virtual void render(const Msp::GL::Tag &) const; }; diff --git a/source/engineer/engineer.cpp b/source/engineer/engineer.cpp index 371e82e..ea5139d 100644 --- a/source/engineer/engineer.cpp +++ b/source/engineer/engineer.cpp @@ -23,6 +23,7 @@ Distributed under the GPL #include #include #include +#include #include "libmarklin/driver.h" #include "libmarklin/tracktype.h" #include "3d/path.h" @@ -44,9 +45,10 @@ Engineer::Engineer(int argc, char **argv): layout_3d(layout), server(0), pipeline(window.get_width(), window.get_height(), false), - placing_train(0), - placing_block(0), - placing_entry(0) + picking(false), + picking_track(0), + picking_entry(0), + picking_path(0) { // Setup GUI window.set_title("Railroad Engineer"); @@ -123,11 +125,17 @@ Engineer::~Engineer() delete server; } -void Engineer::place_train(Train &train) +void Engineer::set_status(const string &text) { - placing_train = &train; - placing_block = 0; - main_panel->set_status_text("Select location"); + main_panel->set_status_text(text); + status_timeout = Time::now()+10*Time::sec; +} + +void Engineer::pick(bool with_ep) +{ + picking = true; + picking_track = 0; + picking_entry = (with_ep ? 0 : -1); } int Engineer::main() @@ -144,22 +152,30 @@ void Engineer::tick() layout.tick(); event_disp.tick(Time::zero); + if(status_timeout && Time::now()>status_timeout) + { + main_panel->set_status_text(string()); + status_timeout = Time::TimeStamp(); + } + GL::clear(GL::COLOR_BUFFER_BIT|GL::DEPTH_BUFFER_BIT); pipeline.render_all(); - layout_3d.get_path_scene().render(); + { + GL::Bind depth(GL::DepthTest::lequal()); + layout_3d.get_path_scene().render(); + } { GL::Bind blend(GL::Blend::alpha()); overlay->render(0); } - if(placing_train && placing_block) + if(picking && picking_track && picking_entry>=0) { GL::PushMatrix push_mat; - const Marklin::Block::Endpoint &bep = placing_block->get_endpoints()[placing_entry]; - float rot = bep.track->get_endpoint_direction(bep.track_ep); - Point pos = bep.track->get_endpoint_position(bep.track_ep); + float rot = picking_track->get_endpoint_direction(picking_entry); + Point pos = picking_track->get_endpoint_position(picking_entry); GL::translate(pos.x, pos.y, pos.z+0.03); GL::rotate(rot*180/M_PI+180, 0, 0, 1); @@ -185,20 +201,19 @@ void Engineer::tick() void Engineer::button_press(int x, int y, unsigned btn, unsigned) { - if(placing_train) + if(picking) { - if(btn==1 && placing_block && !placing_block->get_train()) + if(btn==1 && picking_track) { - reset_block_color(*placing_block); - - placing_train->place(*placing_block, placing_entry); - placing_train = 0; - main_panel->set_status_text(string()); + picking = false; + delete picking_path; + picking_path = 0; + signal_pick_done.emit(picking_track, picking_entry); } - else if(btn==3) + else if(btn==3 && picking_entry>=0) { - const vector &endpoints = placing_block->get_endpoints(); - placing_entry = (placing_entry+1)%endpoints.size(); + picking_entry = (picking_entry+1)%picking_track->get_type().get_endpoints().size(); + picking_path->set_mask(picking_track->get_type().get_endpoints()[picking_entry].paths); } } else @@ -211,7 +226,7 @@ void Engineer::button_press(int x, int y, unsigned btn, unsigned) { Block &block = layout.get_block_by_track(track); if(block.get_train() && !block.get_train()->free_block(block)) - main_panel->set_status_text("Turnout busy"); + set_status("Turnout is busy"); else { unsigned paths = track.get_type().get_paths(); @@ -240,21 +255,23 @@ void Engineer::button_press(int x, int y, unsigned btn, unsigned) void Engineer::pointer_motion(int x, int y) { - if(placing_train) + if(picking) { Track3D *track = pick_track(x, window.get_height()-y-1); - if(track) + if(track && &track->get_track()!=picking_track) { - Block &block = layout.get_block_by_track(track->get_track()); - if(&block!=placing_block) - { - if(placing_block) - reset_block_color(*placing_block); - placing_block = █ - placing_entry = 0; - if(!placing_block->get_train()) - set_block_color(*placing_block, GL::Color(0)); - } + picking_track = &track->get_track(); + if(picking_entry>=0) + picking_entry = 0; + + delete picking_path; + picking_path = new Path3D(*track); + if(picking_entry>=0) + picking_path->set_mask(picking_track->get_type().get_endpoints()[picking_entry].paths); + else + picking_path->set_mask(picking_track->get_type().get_paths()); + picking_path->set_color(GL::Color(0)); + picking_path->set_layer(1); } } } @@ -308,6 +325,7 @@ void Engineer::view_all() camera.set_position(pos); camera.set_up_direction(up); camera.set_look_direction(GL::Vector3(0, 0, -1)); + camera.set_depth_clip(pos.z*0.5, pos.z*1.5); } void Engineer::set_block_color(const Block &block, const GL::Color &color) diff --git a/source/engineer/engineer.h b/source/engineer/engineer.h index 48d60a2..30a3825 100644 --- a/source/engineer/engineer.h +++ b/source/engineer/engineer.h @@ -29,6 +29,9 @@ class TrainProperties; class Engineer: public Msp::Application { +public: + sigc::signal signal_pick_done; + private: Options options; @@ -52,9 +55,11 @@ private: MainPanel *main_panel; std::list train_panels; - Marklin::Train *placing_train; - Marklin::Block *placing_block; - unsigned placing_entry; + Msp::Time::TimeStamp status_timeout; + bool picking; + Marklin::Track *picking_track; + int picking_entry; + Marklin::Path3D *picking_path; public: Engineer(int argc, char **argv); @@ -64,7 +69,8 @@ public: Msp::GLtk::Root &get_root() const { return *root; } const Marklin::Catalogue &get_catalogue() const { return catalogue; } Marklin::Layout &get_layout() { return layout; } - void place_train(Marklin::Train &); + void set_status(const std::string &); + void pick(bool); int main(); void quit() { exit(0); } private: diff --git a/source/engineer/trainpanel.cpp b/source/engineer/trainpanel.cpp index 24a182b..dce1387 100644 --- a/source/engineer/trainpanel.cpp +++ b/source/engineer/trainpanel.cpp @@ -92,6 +92,7 @@ TrainPanel::TrainPanel(Engineer &e, const GLtk::Resources &r, Train &t): add(*(btn=new GLtk::Button(res, "GoTo"))); btn->set_geometry(GLtk::Geometry(geom.w-130, 10, 40, 24)); + btn->signal_clicked.connect(sigc::mem_fun(this, &TrainPanel::goto_clicked)); add(*(btn=new GLtk::Button(res, "Route"))); btn->set_geometry(GLtk::Geometry(geom.w-170, 10, 40, 24)); @@ -136,7 +137,8 @@ void TrainPanel::train_status_changed(const string &s) void TrainPanel::place_clicked() { - engineer.place_train(train); + engineer.pick(true); + pick_conn = engineer.signal_pick_done.connect(sigc::mem_fun(this, &TrainPanel::place)); } void TrainPanel::edit_clicked() @@ -155,6 +157,12 @@ void TrainPanel::route_clicked() dialog->set_visible(true); } +void TrainPanel::goto_clicked() +{ + engineer.pick(false); + pick_conn = engineer.signal_pick_done.connect(sigc::mem_fun(this, &TrainPanel::go_to)); +} + void TrainPanel::forward_toggled(bool value) { train.set_reverse(!value); @@ -164,3 +172,47 @@ void TrainPanel::func_toggled(bool value, unsigned func) { train.set_function(func, value); } + +void TrainPanel::place(Track *track, unsigned ep) +{ + pick_conn.disconnect(); + + Block &block = engineer.get_layout().get_block_by_track(*track); + + while(1) + { + const vector &eps = block.get_endpoints(); + bool ok = false; + for(unsigned i=0; (!ok && iget_links()[ep]; + ep = next->traverse(next->get_endpoint_by_link(*track), 0); + track = next; + if(!block.get_tracks().count(track)) + break; + } + } +} + +void TrainPanel::go_to(Track *track, unsigned) +{ + pick_conn.disconnect(); + + try + { + train.go_to(*track); + } + catch(const Exception &e) + { + engineer.set_status(e.what()); + } +} diff --git a/source/engineer/trainpanel.h b/source/engineer/trainpanel.h index c8825af..b4bd386 100644 --- a/source/engineer/trainpanel.h +++ b/source/engineer/trainpanel.h @@ -31,6 +31,7 @@ private: Msp::GLtk::Label *lbl_status; Msp::GLtk::Toggle *tgl_forward; std::map tgl_funcs; + sigc::connection pick_conn; public: TrainPanel(Engineer &, const Msp::GLtk::Resources &, Marklin::Train &); @@ -44,8 +45,11 @@ private: void place_clicked(); void edit_clicked(); void route_clicked(); + void goto_clicked(); void forward_toggled(bool); void func_toggled(bool, unsigned); + void place(Marklin::Track *, unsigned); + void go_to(Marklin::Track *, unsigned); }; #endif diff --git a/source/engineer/trainproperties.cpp b/source/engineer/trainproperties.cpp index 8780dc4..46b9541 100644 --- a/source/engineer/trainproperties.cpp +++ b/source/engineer/trainproperties.cpp @@ -66,7 +66,6 @@ void TrainProperties::on_ok_clicked() unsigned addr = lexical_cast(ent_addr->get_text()); train = new Train(engineer.get_layout(), *i->second, addr); - engineer.place_train(*train); } train->set_name(ent_name->get_text()); diff --git a/source/libmarklin/route.cpp b/source/libmarklin/route.cpp index 7db3f32..13f4e3e 100644 --- a/source/libmarklin/route.cpp +++ b/source/libmarklin/route.cpp @@ -5,6 +5,8 @@ Copyright © 2007-2010 Mikkosoft Productions, Mikko Rasa Distributed under the GPL */ +#include +#include #include "layout.h" #include "route.h" #include "track.h" @@ -13,6 +15,98 @@ Distributed under the GPL using namespace std; using namespace Msp; +namespace { + +using namespace Marklin; + +struct Node +{ + const Track *track; + unsigned ep; + Node *prev; + float dist; + + Node(): + track(0), ep(0), prev(0), dist(0) + { } + + Node(const Track &t, unsigned e): + track(&t), ep(e), prev(0), dist(0) + { } + + Node(const Track &t, unsigned e, Node &r, float d): + track(&t), ep(e), prev(&r), dist(prev->dist+d) + { } + + bool operator<(const Node &other) const + { return dist>other.dist; } +}; + +struct TrackMatch +{ + const Track &track; + + TrackMatch(const Track &t): track(t) { } + + bool operator()(const Track &t) { return &t==&track; } +}; + +template +list dijkstra(const Track &from, unsigned ep, Pred goal) +{ + // XXX Fails to find some routes - should use track+ep as key + map track_nodes; + priority_queue nodes; + Node *final = 0; + + nodes.push(Node(from, ep)); + + while(!nodes.empty()) + { + Node lowest = nodes.top(); + nodes.pop(); + + if(track_nodes.count(lowest.track)) + continue; + + Node &ref = track_nodes[lowest.track] = lowest; + if(goal(*lowest.track)) + { + final = &ref; + break; + } + + const TrackType &type = lowest.track->get_type(); + const vector &eps = type.get_endpoints(); + const vector &links = lowest.track->get_links(); + for(unsigned i=0; i>=1) ; + nodes.push(Node(*links[i], links[i]->get_endpoint_by_link(*lowest.track), ref, type.get_path_length(path))); + } + } + + if(!final) + throw InvalidParameterValue("Could not find a route"); + + list result; + for(Node *node=final; node; node=node->prev) + result.push_front(node->track); + + return result; +} + +} + + namespace Marklin { Route::Route(Layout &l, const string &n): @@ -199,6 +293,19 @@ void Route::track_removed(Track &t) tracks.erase(&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 route; +} + Route::Loader::Loader(Route &r): DataFile::BasicLoader(r) diff --git a/source/libmarklin/route.h b/source/libmarklin/route.h index 98b64f5..14ac94c 100644 --- a/source/libmarklin/route.h +++ b/source/libmarklin/route.h @@ -51,6 +51,9 @@ private: void update_turnouts(); unsigned check_validity(const Track &) const; void track_removed(Track &); + +public: + static Route *find(const Track &, unsigned, const Track &); }; } // namespace Marklin diff --git a/source/libmarklin/track.h b/source/libmarklin/track.h index c1c9581..7a36a2d 100644 --- a/source/libmarklin/track.h +++ b/source/libmarklin/track.h @@ -50,6 +50,7 @@ public: Track(Layout &, const TrackType &); ~Track(); + Layout &get_layout() const { return layout; } const TrackType &get_type() const { return type; } void set_position(const Point &); diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp index bb36bcf..69e7212 100644 --- a/source/libmarklin/train.cpp +++ b/source/libmarklin/train.cpp @@ -29,7 +29,9 @@ Train::Train(Layout &l, const LocoType &t, unsigned a): target_speed(0), current_speed(0), reverse(false), + functions(0), route(0), + end_of_route(false), status("Unplaced"), travel_dist(0), travel_speed(0), @@ -124,10 +126,50 @@ bool Train::get_function(unsigned func) const void Train::set_route(const Route *r) { + if(!rsv_blocks.empty()) + { + for(list::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) + if(i->block->get_sensor_id()) + { + release_blocks(rsv_blocks, ++i, rsv_blocks.end()); + break; + } + } + route = r; + end_of_route = false; + + if(target_speed && reserve_more()<2) + update_speed(); + signal_route_changed.emit(route); } +void Train::go_to(const Track &to) +{ + BlockRef *last = 0; + if(rsv_blocks.empty()) + last = &cur_blocks.back(); + else + { + for(list::iterator i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !last); ++i) + if(i->block->get_sensor_id()) + 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]; + + set_route(Route::find(*ep.track, ep.track_ep, to)); +} + void Train::place(Block &block, unsigned entry) { if(target_speed) @@ -184,6 +226,7 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) if(stop_timeout && t>=stop_timeout) { release_blocks(rsv_blocks); + end_of_route = false; stop_timeout = Time::TimeStamp(); } @@ -307,8 +350,17 @@ void Train::sensor_event(unsigned addr, bool state) cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i); // Try to get more blocks if we're moving - if(target_speed && reserve_more()<2) - update_speed(); + if(target_speed) + { + unsigned nsens = reserve_more(); + if(!nsens && end_of_route) + { + set_speed(0); + set_route(0); + } + else if(nsens<2) + update_speed(); + } } } else @@ -335,8 +387,13 @@ void Train::sensor_event(unsigned addr, bool state) void Train::turnout_event(unsigned addr, bool) { - if(pending_block && addr==pending_block->get_turnout_id()) - reserve_more(); + if(pending_block) + { + unsigned pending_addr = pending_block->get_turnout_id(); + bool double_addr = (*pending_block->get_tracks().begin())->get_type().is_double_address(); + if(addr==pending_addr || (double_addr && addr==pending_addr+1)) + reserve_more(); + } } void Train::block_reserved(const Block &block, const Train *train) @@ -357,12 +414,13 @@ unsigned Train::reserve_more() pending_block = 0; - // See how many blocks we already have + // See how many sensor blocks we already have unsigned nsens = 0; for(list::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i) if(i->block->get_sensor_id()) ++nsens; + 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; @@ -377,6 +435,16 @@ unsigned Train::reserve_more() int entry = link->get_endpoint_by_link(*last->block); if(entry<0) throw LogicError("Block links are inconsistent!"); + + if(on_route && !route->get_tracks().count(link->get_endpoints()[entry].track)) + { + // Keep the blocks if we arrived at the end of the route + good = last; + good_sens = nsens; + end_of_route = true; + break; + } + if(!link->reserve(this)) { // If we found another train and it's not headed straight for us, we can keep the blocks we got @@ -431,6 +499,9 @@ 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 @@ -445,7 +516,7 @@ unsigned Train::reserve_more() if(got_more) update_speed(); - return nsens; + return good_sens; } void Train::update_speed() diff --git a/source/libmarklin/train.h b/source/libmarklin/train.h index 60a0c08..a89dc93 100644 --- a/source/libmarklin/train.h +++ b/source/libmarklin/train.h @@ -75,6 +75,7 @@ private: Msp::Time::TimeStamp stop_timeout; unsigned functions; const Route *route; + bool end_of_route; std::string status; Msp::Time::TimeStamp last_entry_time; @@ -107,6 +108,7 @@ public: unsigned get_functions() const { return functions; } void set_route(const Route *); + void go_to(const Track &); const Route *get_route() const { return route; } void place(Block &, unsigned); bool is_placed() const { return !cur_blocks.empty(); } -- 2.45.2