From 6968273080fa2a1cbcfc506610d5f249299611e9 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sat, 17 Apr 2010 11:00:55 +0000
Subject: [PATCH] Support trains with multiple vehicles Correct positioning of
 vehicles with bogies

---
 source/3d/catalogue.cpp             |   7 +-
 source/3d/catalogue.h               |   2 +-
 source/engineer/engineer.cpp        |   1 +
 source/engineer/trainproperties.cpp |  23 +++-
 source/libmarklin/catalogue.cpp     |  48 +++++---
 source/libmarklin/catalogue.h       |  10 +-
 source/libmarklin/geometry.h        |   4 +
 source/libmarklin/train.cpp         |  14 ++-
 source/libmarklin/train.h           |   1 +
 source/libmarklin/vehicle.cpp       | 167 ++++++++++++++++++++++++----
 source/libmarklin/vehicle.h         |  16 ++-
 source/libmarklin/vehicletype.h     |   1 +
 wagons.dat                          |  38 +++++++
 13 files changed, 278 insertions(+), 54 deletions(-)
 create mode 100644 wagons.dat

diff --git a/source/3d/catalogue.cpp b/source/3d/catalogue.cpp
index 1068fea..57bf053 100644
--- a/source/3d/catalogue.cpp
+++ b/source/3d/catalogue.cpp
@@ -6,7 +6,6 @@ Distributed under the GPL
 */
 
 #include <msp/gl/meshbuilder.h>
-#include "libmarklin/locotype.h"
 #include "catalogue.h"
 #include "tracktype.h"
 #include "vehicletype.h"
@@ -21,7 +20,7 @@ Catalogue3D::Catalogue3D(Catalogue &c):
 	endpoint_mesh((GL::NORMAL3, GL::VERTEX3))
 {
 	catalogue.signal_track_added.connect(sigc::mem_fun(this, &Catalogue3D::track_added));
-	catalogue.signal_loco_added.connect(sigc::mem_fun(this, &Catalogue3D::loco_added));
+	catalogue.signal_vehicle_added.connect(sigc::mem_fun(this, &Catalogue3D::vehicle_added));
 
 	const map<unsigned, TrackType *> &trks = catalogue.get_tracks();
 	for(map<unsigned, TrackType *>::const_iterator i=trks.begin(); i!=trks.end(); ++i)
@@ -62,9 +61,9 @@ void Catalogue3D::track_added(const TrackType &track)
 	tracks[&track] = new TrackType3D(*this, track);
 }
 
-void Catalogue3D::loco_added(const LocoType &loco)
+void Catalogue3D::vehicle_added(const VehicleType &veh)
 {
-	vehicles[&loco] = new VehicleType3D(*this, loco);
+	vehicles[&veh] = new VehicleType3D(*this, veh);
 }
 
 void Catalogue3D::build_endpoint_mesh()
diff --git a/source/3d/catalogue.h b/source/3d/catalogue.h
index d826a78..1f33ebb 100644
--- a/source/3d/catalogue.h
+++ b/source/3d/catalogue.h
@@ -39,7 +39,7 @@ public:
 	const Msp::GL::Mesh &get_endpoint_mesh() const { return endpoint_mesh; }
 private:
 	void track_added(const TrackType &);
-	void loco_added(const LocoType &);
+	void vehicle_added(const VehicleType &);
 	void build_endpoint_mesh();
 };
 
diff --git a/source/engineer/engineer.cpp b/source/engineer/engineer.cpp
index 92f0f0f..d4a0e44 100644
--- a/source/engineer/engineer.cpp
+++ b/source/engineer/engineer.cpp
@@ -72,6 +72,7 @@ Engineer::Engineer(int argc, char **argv):
 	// Setup railroad control
 	DataFile::load(catalogue, "tracks.dat");
 	DataFile::load(catalogue, "locos.dat");
+	DataFile::load(catalogue, "wagons.dat");
 	DataFile::load(layout, options.layout_fn);
 
 	layout.signal_train_added.connect(sigc::mem_fun(this, &Engineer::train_added));
diff --git a/source/engineer/trainproperties.cpp b/source/engineer/trainproperties.cpp
index 46b9541..96c8313 100644
--- a/source/engineer/trainproperties.cpp
+++ b/source/engineer/trainproperties.cpp
@@ -35,10 +35,13 @@ TrainProperties::TrainProperties(Engineer &e, const GLtk::Resources &r, Train *t
 	add(*(drp_type=new GLtk::Dropdown(res)));
 	drp_type->set_geometry(GLtk::Geometry(60, geom.h-50, geom.w-70, 20));
 
-	const map<unsigned, LocoType *> &locos = engineer.get_catalogue().get_locomotives();
+	const map<unsigned, VehicleType *> &vehs = engineer.get_catalogue().get_vehicles();
 	unsigned n = 0;
-	for(map<unsigned, LocoType *>::const_iterator i=locos.begin(); i!=locos.end(); ++i, ++n)
+	for(map<unsigned, VehicleType *>::const_iterator i=vehs.begin(); i!=vehs.end(); ++i, ++n)
 	{
+		if(!dynamic_cast<LocoType *>(i->second))
+			continue;
+
 		drp_type->append(format("%d %s", i->second->get_article_number(), i->second->get_name()));
 		if(train && i->second==&train->get_locomotive_type())
 			drp_type->set_selected_index(n);
@@ -60,12 +63,20 @@ void TrainProperties::on_ok_clicked()
 {
 	if(!train)
 	{
-		const map<unsigned, LocoType *> &locos = engineer.get_catalogue().get_locomotives();
-		map<unsigned, LocoType *>::const_iterator i = locos.begin();
-		advance(i, drp_type->get_selected_index());
+		const map<unsigned, VehicleType *> &vehs = engineer.get_catalogue().get_vehicles();
+		map<unsigned, VehicleType *>::const_iterator i = vehs.begin();
+		unsigned n = drp_type->get_selected_index();
+		while(!dynamic_cast<LocoType *>(i->second))
+			++i;
+		while(n)
+		{
+			if(dynamic_cast<LocoType *>(i->second))
+				--n;
+			++i;
+		}
 
 		unsigned addr = lexical_cast<unsigned>(ent_addr->get_text());
-		train = new Train(engineer.get_layout(), *i->second, addr);
+		train = new Train(engineer.get_layout(), *dynamic_cast<LocoType *>(i->second), addr);
 	}
 
 	train->set_name(ent_name->get_text());
diff --git a/source/libmarklin/catalogue.cpp b/source/libmarklin/catalogue.cpp
index bdd5369..286407c 100644
--- a/source/libmarklin/catalogue.cpp
+++ b/source/libmarklin/catalogue.cpp
@@ -26,7 +26,7 @@ Catalogue::~Catalogue()
 {
 	for(map<unsigned, TrackType *>::iterator i=tracks.begin(); i!=tracks.end(); ++i)
 		delete i->second;
-	for(map<unsigned, LocoType *>::iterator i=locos.begin(); i!=locos.end(); ++i)
+	for(map<unsigned, VehicleType *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
 		delete i->second;
 }
 
@@ -39,15 +39,6 @@ void Catalogue::add_track(TrackType &track)
 	signal_track_added.emit(track);
 }
 
-void Catalogue::add_locomotive(LocoType &loco)
-{
-	if(locos.count(loco.get_article_number()))
-		throw Exception("Duplicate track type");
-
-	locos[loco.get_article_number()] = &loco;
-	signal_loco_added.emit(loco);
-}
-
 const TrackType &Catalogue::get_track(unsigned art_nr) const
 {
 	map<unsigned, TrackType *>::const_iterator i=tracks.find(art_nr);
@@ -57,15 +48,33 @@ const TrackType &Catalogue::get_track(unsigned art_nr) const
 	return *i->second;
 }
 
-const LocoType &Catalogue::get_locomotive(unsigned art_nr) const
+void Catalogue::add_vehicle(VehicleType &veh)
 {
-	map<unsigned, LocoType *>::const_iterator i=locos.find(art_nr);
-	if(i==locos.end())
-		throw KeyError("Unknown locomotive type");
+	if(vehicles.count(veh.get_article_number()))
+		throw Exception("Duplicate vehicle type");
+
+	vehicles[veh.get_article_number()] = &veh;
+	signal_vehicle_added.emit(veh);
+}
+
+const VehicleType &Catalogue::get_vehicle(unsigned art_nr) const
+{
+	map<unsigned, VehicleType *>::const_iterator i = vehicles.find(art_nr);
+	if(i==vehicles.end())
+		throw KeyError("Unknown vehicle type");
 
 	return *i->second;
 }
 
+const LocoType &Catalogue::get_locomotive(unsigned art_nr) const
+{
+	const VehicleType &veh = get_vehicle(art_nr);
+	if(const LocoType *loco = dynamic_cast<const LocoType *>(&veh))
+		return *loco;
+	
+	throw Exception("Vehicle is not a locomotive");
+}
+
 
 Catalogue::Loader::Loader(Catalogue &c):
 	DataFile::BasicLoader<Catalogue>(c)
@@ -77,6 +86,7 @@ Catalogue::Loader::Loader(Catalogue &c):
 	add("rail_profile", &Loader::rail_profile);
 	add("scale", &Loader::scale);
 	add("track", &Loader::track);
+	add("vehicle", &Loader::vehicle);
 }
 
 void Catalogue::Loader::ballast_profile()
@@ -101,7 +111,7 @@ void Catalogue::Loader::locomotive(unsigned art_nr)
 {
 	RefPtr<LocoType> loco = new LocoType(art_nr);
 	load_sub(*loco);
-	obj.add_locomotive(*loco);
+	obj.add_vehicle(*loco);
 	loco.release();
 }
 
@@ -123,4 +133,12 @@ void Catalogue::Loader::track(unsigned art_nr)
 	trk.release();
 }
 
+void Catalogue::Loader::vehicle(unsigned art_nr)
+{
+	RefPtr<VehicleType> veh = new VehicleType(art_nr);
+	load_sub(*veh);
+	obj.add_vehicle(*veh);
+	veh.release();
+}
+
 } // namespace Marklin
diff --git a/source/libmarklin/catalogue.h b/source/libmarklin/catalogue.h
index 61a336f..524d3f2 100644
--- a/source/libmarklin/catalogue.h
+++ b/source/libmarklin/catalogue.h
@@ -34,10 +34,11 @@ public:
 		void rail_profile();
 		void scale(float, float);
 		void track(unsigned);
+		void vehicle(unsigned);
 	};
 
 	sigc::signal<void, const TrackType &> signal_track_added;
-	sigc::signal<void, const LocoType &> signal_loco_added;
+	sigc::signal<void, const VehicleType &> signal_vehicle_added;
 
 private:
 	float scale;
@@ -46,7 +47,7 @@ private:
 	Profile ballast_profile;
 	Profile path_profile;
 	std::map<unsigned, TrackType *> tracks;
-	std::map<unsigned, LocoType *> locos;
+	std::map<unsigned, VehicleType *> vehicles;
 	Layout layout;
 
 public:
@@ -63,9 +64,10 @@ public:
 	const TrackType &get_track(unsigned) const;
 	const std::map<unsigned, TrackType *> &get_tracks() const { return tracks; }
 
-	void add_locomotive(LocoType &);
+	void add_vehicle(VehicleType &);
+	const VehicleType &get_vehicle(unsigned) const;
 	const LocoType &get_locomotive(unsigned) const;
-	const std::map<unsigned, LocoType *> &get_locomotives() const { return locos; }
+	const std::map<unsigned, VehicleType *> &get_vehicles() const { return vehicles; }
 
 	Layout &get_layout() { return layout; }
 };
diff --git a/source/libmarklin/geometry.h b/source/libmarklin/geometry.h
index fab5cea..674a6b9 100644
--- a/source/libmarklin/geometry.h
+++ b/source/libmarklin/geometry.h
@@ -8,6 +8,7 @@ Distributed under the GPL
 #ifndef LIBMARKLIN_GEOMETRY_H_
 #define LIBMARKLIN_GEOMETRY_H_
 
+#include <cmath>
 #include <vector>
 
 namespace Marklin {
@@ -21,6 +22,9 @@ struct Point
 	Point(float x_, float y_, float z_): x(x_), y(y_), z(z_) { }
 };
 
+inline float distance(const Point &p, const Point &q)
+{ return sqrt((p.x-q.x)*(p.x-q.x) + (p.y-q.y)*(p.y-q.y) + (p.z-q.z)*(p.z-q.z)); }
+
 struct TrackPoint
 {
 	Point pos;
diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 307b1b7..fad6860 100644
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -336,7 +336,8 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 		if(!active)
 			set_active(true);
 
-		Track *track = vehicles[0]->get_track();
+		Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
+		Track *track = vehicle.get_track();
 
 		bool ok = false;
 		for(list<BlockRef>::const_iterator i=cur_blocks.begin(); (!ok && i!=cur_blocks.end()); ++i)
@@ -345,7 +346,7 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 		if(ok)
 		{
 			float d = get_real_speed(current_speed)*(dt/Time::sec);
-			vehicles[0]->advance(reverse ? -d : d);
+			vehicle.advance(reverse ? -d : d);
 		}
 	}
 	else if(end_of_route)
@@ -785,6 +786,7 @@ Train::Loader::Loader(Train &t):
 	add("real_speed",  &Loader::real_speed);
 	add("route",       &Loader::route);
 	add("timetable",   &Loader::timetable);
+	add("vehicle",     &Loader::vehicle);
 }
 
 void Train::Loader::block(unsigned id)
@@ -838,4 +840,12 @@ void Train::Loader::timetable()
 	load_sub(*obj.timetable);
 }
 
+void Train::Loader::vehicle(unsigned n)
+{
+	const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(n);
+	Vehicle *veh = new Vehicle(obj.layout, vtype);
+	obj.vehicles.back()->attach_back(*veh);
+	obj.vehicles.push_back(veh);
+}
+
 } // namespace Marklin
diff --git a/source/libmarklin/train.h b/source/libmarklin/train.h
index 5960c27..635a946 100644
--- a/source/libmarklin/train.h
+++ b/source/libmarklin/train.h
@@ -38,6 +38,7 @@ public:
 		void real_speed(unsigned, float, float);
 		void route(const std::string &);
 		void timetable();
+		void vehicle(unsigned);
 	};
 
 	sigc::signal<void, const std::string &> signal_name_changed;
diff --git a/source/libmarklin/vehicle.cpp b/source/libmarklin/vehicle.cpp
index 6011537..8d52a7f 100644
--- a/source/libmarklin/vehicle.cpp
+++ b/source/libmarklin/vehicle.cpp
@@ -14,6 +14,7 @@ Distributed under the GPL
 #include "vehicletype.h"
 
 using namespace std;
+using namespace Msp;
 
 namespace Marklin {
 
@@ -32,6 +33,42 @@ Vehicle::~Vehicle()
 	layout.remove_vehicle(*this);
 }
 
+void Vehicle::attach_back(Vehicle &veh)
+{
+	if(next || veh.prev)
+		throw InvalidState("Already attached");
+
+	next = &veh;
+	veh.prev = this;
+}
+
+void Vehicle::attach_front(Vehicle &veh)
+{
+	if(prev || veh.next)
+		throw InvalidState("Already attached");
+
+	next = &veh;
+	veh.prev = this;
+}
+
+void Vehicle::detach_back()
+{
+	if(!next)
+		throw InvalidState("Not attached");
+
+	next->prev = 0;
+	next = 0;
+}
+
+void Vehicle::detach_front()
+{
+	if(!prev)
+		throw InvalidState("Not attached");
+
+	prev->next = 0;
+	prev = 0;
+}
+
 void Vehicle::place(Track *t, unsigned e, float o, PlaceMode m)
 {
 	track_pos = TrackPosition(t, e, o);
@@ -66,12 +103,14 @@ void Vehicle::place(Track *t, unsigned e, float o, PlaceMode m)
 		track_pos.advance(type.get_length()/2);
 
 	update_position();
+	propagate_position();
 }
 
 void Vehicle::advance(float d)
 {
 	track_pos.advance(d);
 	update_position();
+	propagate_position();
 }
 
 void Vehicle::update_position()
@@ -80,57 +119,143 @@ void Vehicle::update_position()
 
 	const vector<VehicleType::Axle> &axles = type.get_axles();
 	if(axles.size()>=2)
-		tp = get_position(axles.front().position, axles.back().position, track_pos);
+	{
+		float wheelbase = axles.front().position-axles.back().position;
+		tp = get_point(track_pos, wheelbase, -axles.back().position/wheelbase);
+	}
 	else
 	{
 		const vector<VehicleType::Bogie> &bogies = type.get_bogies();
 		if(bogies.size()>=2)
-			// XXX Calculate bogie positions correctly
-			tp = get_position(bogies.front().position, bogies.back().position, track_pos);
+		{
+			TrackPosition front = track_pos;
+			front.advance(bogies.front().position);
+			TrackPosition back = track_pos;
+			back.advance(bogies.back().position);
+			float bogie_spacing = bogies.front().position-bogies.back().position;
+			adjust_for_distance(front, back, bogie_spacing);
+
+			const vector<VehicleType::Axle> &front_axles = bogies.front().axles;
+			float wheelbase = front_axles.front().position-front_axles.back().position;
+			Point front_point = get_point(front, wheelbase, -front_axles.back().position/wheelbase).pos;
+
+			const vector<VehicleType::Axle> &back_axles = bogies.back().axles;
+			wheelbase = back_axles.front().position-back_axles.back().position;
+			Point back_point = get_point(back, wheelbase, -back_axles.back().position/wheelbase).pos;
+
+			tp = get_point(front_point, back_point, -bogies.back().position/bogie_spacing);
+		}
 		else
 			tp = track_pos.get_point();
 	}
+
 	position = tp.pos;
 	direction = tp.dir;
 }
 
-TrackPoint Vehicle::get_position(float front, float back, const TrackPosition &tpos)
+void Vehicle::update_position_from(const Vehicle &veh)
 {
-	TrackPosition front_pos = tpos;
-	front_pos.advance(front);
+	int sign = (&veh==prev ? -1 : 1);
 
-	TrackPosition back_pos = tpos;
-	back_pos.advance(back);
+	float tdist = (type.get_length()+veh.type.get_length())/2;
+	float margin = layout.get_catalogue().get_scale();
 
-	float target_dist = front-back;
+	float dist = distance(veh.position, position);
+	if(dist<tdist-margin || dist>tdist+margin)
+	{
+		track_pos = veh.track_pos;
+		track_pos.advance(sign*tdist);
+		update_position();
+
+		dist = distance(veh.position, position);
+	}
+
+	track_pos.advance(sign*(tdist-dist));
+	update_position();
+}
 
+void Vehicle::propagate_position()
+{
+	if(prev)
+		propagate_forward();
+	if(next)
+		propagate_backward();
+}
+
+void Vehicle::propagate_forward()
+{
+	prev->update_position_from(*this);
+
+	if(prev->prev)
+		prev->propagate_forward();
+}
+
+void Vehicle::propagate_backward()
+{
+	next->update_position_from(*this);
+
+	if(next->next)
+		next->propagate_backward();
+}
+
+void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, float tdist, float ratio) const
+{
+	float margin = 0.01*layout.get_catalogue().get_scale();
+	int adjust_dir = 0;
 	while(1)
 	{
-		Point front_point = front_pos.get_point().pos;
-		Point back_point = back_pos.get_point().pos;
+		Point front_point = front.get_point().pos;
+		Point back_point = back.get_point().pos;
 
 		float dx = front_point.x-back_point.x;
 		float dy = front_point.y-back_point.y;
 		float dz = front_point.z-back_point.z;
 		float dist = sqrt(dx*dx+dy*dy+dz*dz);
 
-		if(dist<target_dist)
+		float diff = tdist-dist;
+		if(diff<-margin && adjust_dir<=0)
 		{
-			float adjust = target_dist-dist+0.01*layout.get_catalogue().get_scale();
-			front_pos.advance(adjust/2);
-			back_pos.advance(-adjust/2);
+			diff -= margin;
+			adjust_dir = -1;
 		}
-		else
+		else if(diff>margin && adjust_dir>=0)
 		{
-			float f = -back/target_dist;
-			TrackPoint pt;
-			pt.pos = Point(back_point.x+dx*f, back_point.y+dy*f, back_point.z+dz*f);
-			pt.dir = atan2(dy, dx);
-			return pt;
+			diff += margin;
+			adjust_dir = 1;
 		}
+		else
+			return;
+
+		front.advance(diff*(1-ratio));
+		back.advance(-diff*ratio);
 	}
 }
 
+TrackPoint Vehicle::get_point(const Point &front, const Point &back, float ratio) const
+{
+	float dx = front.x-back.x;
+	float dy = front.y-back.y;
+	float dz = front.z-back.z;
+
+	TrackPoint tp;
+	tp.pos = Point(back.x+dx*ratio, back.y+dy*ratio, back.z+dz*ratio);
+	tp.dir = atan2(dy, dx);
+
+	return tp;
+}
+
+TrackPoint Vehicle::get_point(const TrackPosition &pos, float tdist, float ratio) const
+{
+	TrackPosition front = pos;
+	front.advance(tdist*(1-ratio));
+
+	TrackPosition back = pos;
+	back.advance(-tdist*ratio);
+
+	adjust_for_distance(front, back, tdist, ratio);
+	return get_point(front.get_point().pos, back.get_point().pos, ratio);
+}
+
 
 Vehicle::TrackPosition::TrackPosition():
 	track(0),
diff --git a/source/libmarklin/vehicle.h b/source/libmarklin/vehicle.h
index 149adda..c58df76 100644
--- a/source/libmarklin/vehicle.h
+++ b/source/libmarklin/vehicle.h
@@ -55,6 +55,13 @@ public:
 
 	const VehicleType &get_type() const { return type; }
 
+	void attach_back(Vehicle &);
+	void attach_front(Vehicle &);
+	void detach_back();
+	void detach_front();
+	Vehicle *get_next() const { return next; }
+	Vehicle *get_previous() const { return prev; }
+
 	void place(Track *, unsigned, float, PlaceMode = CENTER);
 	void advance(float);
 	Track *get_track() const { return track_pos.track; }
@@ -64,7 +71,14 @@ public:
 	float get_direction() const { return direction; }
 private:
 	void update_position();
-	TrackPoint get_position(float, float, const TrackPosition &);
+	void update_position_from(const Vehicle &);
+	void propagate_position();
+	void propagate_forward();
+	void propagate_backward();
+
+	void adjust_for_distance(TrackPosition &, TrackPosition &, float, float = 0.5) const;
+	TrackPoint get_point(const Point &, const Point &, float = 0.5) const;
+	TrackPoint get_point(const TrackPosition &, float, float = 0.5) const;
 };
 
 } // namespace Marklin
diff --git a/source/libmarklin/vehicletype.h b/source/libmarklin/vehicletype.h
index 6c33890..01e678b 100644
--- a/source/libmarklin/vehicletype.h
+++ b/source/libmarklin/vehicletype.h
@@ -69,6 +69,7 @@ private:
 
 public:
 	VehicleType(unsigned);
+	virtual ~VehicleType() { } // XXX temporary
 
 	unsigned get_article_number() const { return art_nr; }
 	const std::string &get_name() const { return name; }
diff --git a/wagons.dat b/wagons.dat
new file mode 100644
index 0000000..ee3d46b
--- /dev/null
+++ b/wagons.dat
@@ -0,0 +1,38 @@
+/* $Id$ */
+
+scale 1 87;
+gauge 16.5;
+
+vehicle 46274
+{
+	name "Saar Railroad Gmhs 54";
+
+	length 101;
+	width 33;
+	height 33;
+
+	axle { position 31; wheel_diameter 10; };
+	axle { position -31; wheel_diameter 10; };
+};
+
+vehicle 100001
+{
+	name "BR 50 tender";
+
+	length 90;
+	width 33;
+	height 33;
+
+	bogie
+	{
+		position 23;
+		axle { position 11; wheel_diameter 10; };
+		axle { position -11; wheel_diameter 10; };
+	};
+	bogie
+	{
+		position -21;
+		axle { position 11; wheel_diameter 10; };
+		axle { position -11; wheel_diameter 10; };
+	};
+};
-- 
2.43.0