]> git.tdb.fi Git - r2c2.git/commitdiff
Add a pathfinder function to Route
authorMikko Rasa <tdb@tdb.fi>
Sat, 20 Mar 2010 11:42:43 +0000 (11:42 +0000)
committerMikko Rasa <tdb@tdb.fi>
Sat, 20 Mar 2010 11:42:43 +0000 (11:42 +0000)
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

12 files changed:
source/3d/path.cpp
source/3d/path.h
source/engineer/engineer.cpp
source/engineer/engineer.h
source/engineer/trainpanel.cpp
source/engineer/trainpanel.h
source/engineer/trainproperties.cpp
source/libmarklin/route.cpp
source/libmarklin/route.h
source/libmarklin/track.h
source/libmarklin/train.cpp
source/libmarklin/train.h

index 1128b67bb9d61e9b4d0995da236650257e7b90f2..e413ace021d2bec89cb7b132832b5b93919621db 100644 (file)
@@ -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)
index 56e7a2f1be60279ae941aa9d55405cc312be7596..dd071f7d42053031c393c3b93cda52caffb8f986 100644 (file)
@@ -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;
 };
index 371e82e49e7c4d4fc6b69315be3e0ca1b355d4a6..ea5139d5b2e9d9e94cc7ebbdd2455bf72f959c2d 100644 (file)
@@ -23,6 +23,7 @@ Distributed under the GPL
 #include <msp/io/print.h>
 #include <msp/strings/formatter.h>
 #include <msp/time/units.h>
+#include <msp/time/utils.h>
 #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<Block::Endpoint> &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 = &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)
index 48d60a2df9ae82679c75615852ed86d6f3c9aaf3..30a3825893827539c8328487e91b76e15c8037eb 100644 (file)
@@ -29,6 +29,9 @@ class TrainProperties;
 
 class Engineer: public Msp::Application
 {
+public:
+       sigc::signal<void, Marklin::Track *, int> signal_pick_done;
+
 private:
        Options options;
 
@@ -52,9 +55,11 @@ private:
 
        MainPanel *main_panel;
        std::list<TrainPanel *> 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:
index 24a182b05c9328caca4efc318c14b7d309831ebc..dce1387f58540c604e3b8b45c4e661ace7d523a6 100644 (file)
@@ -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<Block::Endpoint> &eps = block.get_endpoints();
+               bool ok = false;
+               for(unsigned i=0; (!ok && i<eps.size()); ++i)
+                       if(eps[i].track==track && eps[i].track_ep==ep)
+                       {
+                               train.place(block, i);
+                               ok = true;
+                       }
+
+               if(ok)
+                       break;
+               else
+               {
+                       Track *next = track->get_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());
+       }
+}
index c8825af0120a5d8a98865c41fff7292ea1ba7259..b4bd386896af3aa07c3a467fb7dfe155442e9b8e 100644 (file)
@@ -31,6 +31,7 @@ private:
        Msp::GLtk::Label *lbl_status;
        Msp::GLtk::Toggle *tgl_forward;
        std::map<unsigned, Msp::GLtk::Toggle *> 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
index 8780dc4ab1cd66c8ead2a755d0cdd307ba29243e..46b9541c4331a7e9e7e951e73a23fa95f8716ffc 100644 (file)
@@ -66,7 +66,6 @@ void TrainProperties::on_ok_clicked()
 
                unsigned addr = lexical_cast<unsigned>(ent_addr->get_text());
                train = new Train(engineer.get_layout(), *i->second, addr);
-               engineer.place_train(*train);
        }
 
        train->set_name(ent_name->get_text());
index 7db3f324b61e9ce5d753786aac251982cff6ec90..13f4e3efff012e57622d2e387ac25e8613ffc6bc 100644 (file)
@@ -5,6 +5,8 @@ Copyright © 2007-2010  Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
+#include <queue>
+#include <msp/strings/formatter.h>
 #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<typename Pred>
+list<const Track *> dijkstra(const Track &from, unsigned ep, Pred goal)
+{
+       // XXX Fails to find some routes - should use track+ep as key
+       map<const Track *, Node> track_nodes;
+       priority_queue<Node> 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<Endpoint> &eps = type.get_endpoints();
+               const vector<Track *> &links = lowest.track->get_links();
+               for(unsigned i=0; i<eps.size(); ++i)
+               {
+                       if(i==lowest.ep || !links[i] || track_nodes.count(links[i]))
+                               continue;
+
+                       unsigned mask = eps[i].paths&eps[lowest.ep].paths;
+                       if(!mask)
+                               continue;
+
+                       unsigned path=0;
+                       for(; !(mask&1); ++path, mask>>=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<const Track *> 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<const Track *> tracks = dijkstra(from, ep, goal);
+
+       static unsigned n = 0;
+       Route *route = new Route(from.get_layout(), format("Route::find %d", ++n));
+       for(list<const Track *>::iterator i=tracks.begin(); i!=tracks.end(); ++i)
+               route->add_track(**i);
+
+       return route;
+}
+
 
 Route::Loader::Loader(Route &r):
        DataFile::BasicLoader<Route>(r)
index 98b64f5c9ff11e9411bb987f8ad4d5877ab8bd3c..14ac94ce9073f0dcba51d788ea140e64c8a9d811 100644 (file)
@@ -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
index c1c95810f1d4d3dac8f0ab666cb41cc2b323775d..7a36a2d6f8bdd15faa90d8958029f782869178a8 100644 (file)
@@ -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 &);
index bb36bcfc378518ab48c7bea2960dcd57892b7677..69e7212a8919937d327f027c7a314296b057b361 100644 (file)
@@ -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<BlockRef>::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<BlockRef>::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<BlockRef>::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()
index 60a0c0824ade3030e52497ee0ec6a19b4f79f679..a89dc93c6addca56ff07ab23b0bd3b10cca6e404 100644 (file)
@@ -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(); }