From: Mikko Rasa <tdb@tdb.fi>
Date: Sat, 20 Mar 2010 11:42:43 +0000 (+0000)
Subject: Add a pathfinder function to Route
X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=d27edb207810f0b4e44be8c3632d421faa5fed80;p=r2c2.git

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
---

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 <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)
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<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:
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<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());
+	}
+}
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<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
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<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());
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 <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)
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<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()
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(); }