From: Mikko Rasa <tdb@tdb.fi>
Date: Sat, 11 Dec 2010 01:25:03 +0000 (+0000)
Subject: Make the axles of vehicles rotate when moving
X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;ds=inline;h=37af7970d9cefcf40ae58ca06ca8469f56b0cc13;p=r2c2.git

Make the axles of vehicles rotate when moving
Add a system to simulate steam locomotive running gear
---

diff --git a/source/3d/vehicle.cpp b/source/3d/vehicle.cpp
index f14f3c2..50dbcb0 100644
--- a/source/3d/vehicle.cpp
+++ b/source/3d/vehicle.cpp
@@ -64,6 +64,7 @@ void Vehicle3D::render(const GL::Tag &tag) const
 			{
 				GL::PushMatrix push_mat2;
 				GL::translate(axles[i].position, 0, axles[i].wheel_dia/2);
+				GL::rotate(vehicle.get_axle_angle(i)*180/M_PI, 0, 1, 0);
 				obj->render(tag);
 			}
 
@@ -80,6 +81,7 @@ void Vehicle3D::render(const GL::Tag &tag) const
 				{
 					GL::PushMatrix push_mat3;
 					GL::translate(bogies[i].axles[j].position, 0, bogies[i].axles[j].wheel_dia/2);
+					GL::rotate(vehicle.get_bogie_axle_angle(i, j)*180/M_PI, 0, 1, 0);
 					obj->render(tag);
 				}
 
@@ -88,6 +90,20 @@ void Vehicle3D::render(const GL::Tag &tag) const
 			if(const GL::Object *obj = type.get_bogie_object(i))
 				obj->render(tag);
 		}
+
+		const vector<VehicleType::Rod> &rods = vehicle.get_type().get_rods();
+		for(unsigned i=0; i<rods.size(); ++i)
+			if(const GL::Object *obj = type.get_rod_object(i))
+			{
+				GL::PushMatrix push_mat2;
+				const Point &rpos = vehicle.get_rod_position(i);
+				float angle = vehicle.get_rod_angle(i);
+				GL::translate(rpos.x, rpos.y, rpos.z);
+				if(rods[i].mirror_object)
+					GL::scale(1, -1, 1);
+				GL::rotate(angle*180/M_PI, 0, -1, 0);
+				obj->render(tag);
+			}
 	}
 }
 
diff --git a/source/3d/vehicletype.cpp b/source/3d/vehicletype.cpp
index 93601f7..2ddd34d 100644
--- a/source/3d/vehicletype.cpp
+++ b/source/3d/vehicletype.cpp
@@ -51,6 +51,10 @@ VehicleType3D::VehicleType3D(const Catalogue3D &c, const VehicleType &t):
 		for(vector<VehicleType::Axle>::const_iterator j=i->axles.begin(); j!=i->axles.end(); ++j)
 			axle_objects.back().push_back(get_object(j->object));
 	}
+
+	const vector<VehicleType::Rod> &rods = type.get_rods();
+	for(vector<VehicleType::Rod>::const_iterator i=rods.begin(); i!=rods.end(); ++i)
+		rod_objects.push_back(get_object(i->object));
 }
 
 VehicleType3D::~VehicleType3D()
@@ -82,6 +86,13 @@ const GL::Object *VehicleType3D::get_bogie_axle_object(unsigned i, unsigned j) c
 	return axle_objects[i+1][j];
 }
 
+const GL::Object *VehicleType3D::get_rod_object(unsigned i) const
+{
+	if(i>=rod_objects.size())
+		throw InvalidParameterValue("Rod index out of range");
+	return rod_objects[i];
+}
+
 GL::Object *VehicleType3D::get_object(const string &name)
 {
 	if(name.empty())
diff --git a/source/3d/vehicletype.h b/source/3d/vehicletype.h
index 72c2c23..bef461c 100644
--- a/source/3d/vehicletype.h
+++ b/source/3d/vehicletype.h
@@ -25,6 +25,7 @@ private:
 	Msp::GL::Object *body_object;
 	std::vector<Msp::GL::Object *> bogie_objects;
 	std::vector<std::vector<Msp::GL::Object *> > axle_objects;
+	std::vector<Msp::GL::Object *> rod_objects;
 
 public:
 	VehicleType3D(const Catalogue3D &, const VehicleType &);
@@ -34,6 +35,7 @@ public:
 	const Msp::GL::Object *get_axle_object(unsigned) const;
 	const Msp::GL::Object *get_bogie_object(unsigned) const;
 	const Msp::GL::Object *get_bogie_axle_object(unsigned, unsigned) const;
+	const Msp::GL::Object *get_rod_object(unsigned) const;
 private:
 	Msp::GL::Object *get_object(const std::string &);
 	Msp::GL::Technique *create_technique(const std::map<std::string, std::string> &);
diff --git a/source/libr2c2/vehicle.cpp b/source/libr2c2/vehicle.cpp
index e0a5c96..578707a 100644
--- a/source/libr2c2/vehicle.cpp
+++ b/source/libr2c2/vehicle.cpp
@@ -27,10 +27,17 @@ Vehicle::Vehicle(Layout &l, const VehicleType &t):
 	prev(0),
 	direction(0),
 	bogie_dirs(type.get_bogies().size()),
+	axle_angles(1),
+	rods(type.get_rods().size()),
 	front_sensor(0),
 	back_sensor(0)
 {
 	layout.add_vehicle(*this);
+
+	axle_angles.front().resize(type.get_axles().size(), 0.0f);
+	const vector<VehicleType::Bogie> &bogies = type.get_bogies();
+	for(vector<VehicleType::Bogie>::const_iterator i=bogies.begin(); i!=bogies.end(); ++i)
+		axle_angles.push_back(vector<float>(i->axles.size(), 0.0f));
 }
 
 Vehicle::~Vehicle()
@@ -118,6 +125,7 @@ void Vehicle::advance(float d)
 {
 	track_pos.advance(d);
 	update_position();
+	turn_axles(d);
 	propagate_position();
 }
 
@@ -128,6 +136,36 @@ float Vehicle::get_bogie_direction(unsigned i) const
 	return bogie_dirs[i];
 }
 
+float Vehicle::get_axle_angle(unsigned i) const
+{
+	if(i>=axle_angles[0].size())
+		throw InvalidParameterValue("Axle index out of range");
+	return axle_angles[0][i];
+}
+
+float Vehicle::get_bogie_axle_angle(unsigned i, unsigned j) const
+{
+	if(i+1>=axle_angles.size())
+		throw InvalidParameterValue("Bogie index out of range");
+	if(j>=axle_angles[i+1].size())
+		throw InvalidParameterValue("Axle index out of range");
+	return axle_angles[i+1][j];
+}
+
+const Point &Vehicle::get_rod_position(unsigned i) const
+{
+	if(i>=rods.size())
+		throw InvalidParameterValue("Rod index out of range");
+	return rods[i].position;
+}
+
+float Vehicle::get_rod_angle(unsigned i) const
+{
+	if(i>=rods.size())
+		throw InvalidParameterValue("Rod index out of range");
+	return rods[i].angle;
+}
+
 void Vehicle::update_position()
 {
 	TrackPoint tp;
@@ -193,6 +231,7 @@ void Vehicle::update_position_from(const Vehicle &veh)
 
 	track_pos.advance(sign*(tdist-dist));
 	update_position();
+	turn_axles(sign*(tdist-dist));
 }
 
 void Vehicle::propagate_position()
@@ -245,6 +284,81 @@ void Vehicle::check_sensor(float offset, unsigned &sensor)
 	}
 }
 
+void Vehicle::turn_axles(float d)
+{
+	const vector<VehicleType::Axle> &axles = type.get_axles();
+	const vector<VehicleType::Bogie> &bogies = type.get_bogies();
+	for(unsigned i=0; i<axle_angles.size(); ++i)
+		for(unsigned j=0; j<axle_angles[i].size(); ++j)
+		{
+			const VehicleType::Axle &axle = (i==0 ? axles[j] : bogies[i-1].axles[j]);
+			axle_angles[i][j] += d*2/axle.wheel_dia;
+		}
+
+	update_rods();
+}
+
+void Vehicle::update_rods()
+{
+	const vector<VehicleType::Rod> &trods = type.get_rods();
+	for(unsigned i=0; i<trods.size(); ++i)
+	{
+		const VehicleType::Rod &rod = trods[i];
+		if(rod.pivot==VehicleType::Rod::BODY)
+			rods[i].position = rod.pivot_point;
+		else if(rod.pivot==VehicleType::Rod::AXLE)
+		{
+			const VehicleType::Axle &axle = type.get_axles()[rod.pivot_index];
+			float angle = axle_angles[0][rod.pivot_index];
+			float c = cos(angle);
+			float s = sin(angle);
+			const Point &pp = rod.pivot_point;
+			rods[i].position = Point(axle.position+pp.x*c+pp.z*s, pp.y, axle.wheel_dia/2+pp.z*c-pp.x*s);
+		}
+		else if(rod.pivot==VehicleType::Rod::ROD)
+		{
+			float angle = rods[rod.pivot_index].angle;
+			float c = cos(angle);
+			float s = sin(angle);
+			const Point &pos = rods[rod.pivot_index].position;
+			const Point &off = rod.pivot_point;
+			rods[i].position = Point(pos.x+off.x*c-off.z*s, pos.y+off.y, pos.z+off.z*c+off.x*s);
+		}
+
+		if(rod.connect_index>=0)
+		{
+			const VehicleType::Rod &crod = trods[rod.connect_index];
+			if(rod.limit==VehicleType::Rod::ROTATE && crod.limit==VehicleType::Rod::SLIDE_X)
+			{
+				float dx = (rods[rod.connect_index].position.x+rod.connect_offset.x)-rods[i].position.x;
+				float dz = (rods[rod.connect_index].position.z+rod.connect_offset.z)-rods[i].position.z;
+				float cd = sqrt(rod.connect_point.x*rod.connect_point.x+rod.connect_point.z*rod.connect_point.z);
+				float ca = atan2(rod.connect_point.z, rod.connect_point.x);
+				dx = sqrt(cd*cd-dz*dz)*(dx>0 ? 1 : -1);
+				rods[i].angle = atan2(dz, dx)-ca;
+				rods[rod.connect_index].position.x = rods[i].position.x+dx-rod.connect_offset.x;
+			}
+			else if(rod.limit==VehicleType::Rod::ROTATE && crod.limit==VehicleType::Rod::ROTATE)
+			{
+				float dx = rods[rod.connect_index].position.x-rods[i].position.x;
+				float dz = rods[rod.connect_index].position.z-rods[i].position.z;
+				float d = sqrt(dx*dx+dz*dz);
+				float cd1 = sqrt(rod.connect_point.x*rod.connect_point.x+rod.connect_point.z*rod.connect_point.z);
+				float cd2 = sqrt(rod.connect_offset.x*rod.connect_offset.x+rod.connect_offset.z*rod.connect_offset.z);
+				float a = (d*d+cd1*cd1-cd2*cd2)/(2*d);
+				float b = sqrt(cd1*cd1-a*a);
+				float sign = (dx*rod.connect_point.z-dz*rod.connect_point.x>0 ? 1 : -1);
+				float cx = (dx*a-dz*b*sign)/d;
+				float cz = (dz*a+dx*b*sign)/d;
+				float ca1 = atan2(rod.connect_point.z, rod.connect_point.x);
+				float ca2 = atan2(rod.connect_offset.z, rod.connect_offset.x);
+				rods[i].angle = atan2(cz, cx)-ca1;
+				rods[rod.connect_index].angle = atan2(cz-dz, cx-dx)-ca2;
+			}
+		}
+	}
+}
+
 void Vehicle::adjust_for_distance(TrackPosition &front, TrackPosition &back, float tdist, float ratio) const
 {
 	float margin = 0.01*layout.get_catalogue().get_scale();
@@ -361,4 +475,9 @@ TrackPoint Vehicle::TrackPosition::get_point() const
 		return TrackPoint();
 }
 
+
+Vehicle::Rod::Rod():
+	angle(0)
+{ }
+
 } // namespace R2C2
diff --git a/source/libr2c2/vehicle.h b/source/libr2c2/vehicle.h
index ec33ede..2e91962 100644
--- a/source/libr2c2/vehicle.h
+++ b/source/libr2c2/vehicle.h
@@ -41,6 +41,14 @@ private:
 		TrackPoint get_point() const;
 	};
 
+	struct Rod
+	{
+		Point position;
+		float angle;
+
+		Rod();
+	};
+
 	Layout &layout;
 	const VehicleType &type;
 	Vehicle *next;
@@ -49,6 +57,8 @@ private:
 	Point position;
 	float direction;
 	std::vector<float> bogie_dirs;
+	std::vector<std::vector<float> > axle_angles;
+	std::vector<Rod> rods;
 	unsigned front_sensor;
 	unsigned back_sensor;
 
@@ -73,7 +83,11 @@ public:
 	float get_offset() const { return track_pos.offs; }
 	const Point &get_position() const { return position; }
 	float get_direction() const { return direction; }
+	float get_axle_angle(unsigned) const;
 	float get_bogie_direction(unsigned) const;
+	float get_bogie_axle_angle(unsigned, unsigned) const;
+	const Point &get_rod_position(unsigned) const;
+	float get_rod_angle(unsigned) const;
 private:
 	void update_position();
 	void update_position_from(const Vehicle &);
@@ -81,6 +95,8 @@ private:
 	void propagate_forward();
 	void propagate_backward();
 	void check_sensor(float, unsigned &);
+	void turn_axles(float);
+	void update_rods();
 
 	void adjust_for_distance(TrackPosition &, TrackPosition &, float, float = 0.5) const;
 	TrackPoint get_point(const Point &, const Point &, float = 0.5) const;
diff --git a/source/libr2c2/vehicletype.cpp b/source/libr2c2/vehicletype.cpp
index 3d695f8..b9c6ed2 100644
--- a/source/libr2c2/vehicletype.cpp
+++ b/source/libr2c2/vehicletype.cpp
@@ -67,6 +67,16 @@ VehicleType::Bogie::Bogie():
 { }
 
 
+VehicleType::Rod::Rod():
+	pivot(BODY),
+	pivot_index(0),
+	pivot_index2(0),
+	limit(ROTATE),
+	connect_index(-1),
+	mirror_object(false)
+{ }
+
+
 VehicleType::Loader::Loader(VehicleType &vt):
 	DataFile::ObjectLoader<VehicleType>(vt)
 {
@@ -76,8 +86,9 @@ VehicleType::Loader::Loader(VehicleType &vt):
 	add("height",     &Loader::height);
 	add("length",     &Loader::length);
 	add("locomotive", &VehicleType::locomotive);
-	add("object",     &VehicleType::object);
 	add("name",       &VehicleType::name);
+	add("object",     &VehicleType::object);
+	add("rod",        &Loader::rod);
 	add("width",      &Loader::width);
 }
 
@@ -110,6 +121,17 @@ void VehicleType::Loader::length(float l)
 	obj.length = l/1000;
 }
 
+void VehicleType::Loader::rod()
+{
+	Rod rd;
+	Rod::Loader ldr(rd, rod_tags);
+	load_sub_with(ldr);
+	obj.rods.push_back(rd);
+	if(!ldr.get_tag().empty())
+		rod_tags[ldr.get_tag()] = obj.rods.size()-1;
+	rod_tags["previous"] = obj.rods.size()-1;
+}
+
 void VehicleType::Loader::width(float w)
 {
 	obj.width = w/1000;
@@ -157,4 +179,82 @@ void VehicleType::Bogie::Loader::position(float p)
 	obj.position = p/1000;
 }
 
+
+VehicleType::Rod::Loader::Loader(Rod &r, const map<string, unsigned> &t):
+	DataFile::ObjectLoader<Rod>(r),
+	tags(t)
+{
+	add("connect",       &Loader::connect);
+	add("limit",         &Rod::limit);
+	add("mirror_object", &Rod::mirror_object);
+	add("object",        &Rod::object);
+	add("pivot_body",    &Loader::pivot_body);
+	add("pivot_axle",    &Loader::pivot_axle);
+	add("pivot_axle",    &Loader::pivot_bogie_axle);
+	add("pivot_rod",     &Loader::pivot_rod);
+	add("position",      &Loader::position);
+	add("tag",           &Loader::set_tag);
+}
+
+void VehicleType::Rod::Loader::connect(const string &t, float px, float pz, float ox, float oz)
+{
+	map<string, unsigned>::const_iterator i = tags.find(t);
+	if(i==tags.end())
+		throw KeyError("Unknown rod tag", t);
+	obj.connect_index = i->second;
+	obj.connect_point = Point(px/1000, 0, pz/1000);
+	obj.connect_offset = Point(ox/1000, 0, oz/1000);
+}
+
+void VehicleType::Rod::Loader::pivot_body()
+{
+	obj.pivot = BODY;
+}
+
+void VehicleType::Rod::Loader::pivot_axle(unsigned i)
+{
+	obj.pivot = AXLE;
+	obj.pivot_index = i;
+}
+
+void VehicleType::Rod::Loader::pivot_bogie_axle(unsigned i, unsigned j)
+{
+	obj.pivot = BOGIE_AXLE;
+	obj.pivot_index = i;
+	obj.pivot_index2 = j;
+}
+
+void VehicleType::Rod::Loader::pivot_rod(const string &t)
+{
+	map<string, unsigned>::const_iterator i = tags.find(t);
+	if(i==tags.end())
+		throw KeyError("Unknown rod tag", t);
+	obj.pivot = ROD;
+	obj.pivot_index = i->second;
+}
+
+void VehicleType::Rod::Loader::position(float x, float y, float z)
+{
+	obj.pivot_point = Point(x/1000, y/1000, z/1000);
+}
+
+void VehicleType::Rod::Loader::set_tag(const string &t)
+{
+	tag = t;
+}
+
+
+void operator>>(const LexicalConverter &c, VehicleType::Rod::Limit &l)
+{
+	const string &s = c.get();
+	if(s=="FIXED")
+		l = VehicleType::Rod::FIXED;
+	else if(s=="ROTATE")
+		l = VehicleType::Rod::ROTATE;
+	else if(s=="SLIDE_X")
+		l = VehicleType::Rod::SLIDE_X;
+	else
+		throw LexicalError("Invalid value for Rod::Limit");
+}
+
 } // namespace R2C2
diff --git a/source/libr2c2/vehicletype.h b/source/libr2c2/vehicletype.h
index b97fb92..c0fe0ea 100644
--- a/source/libr2c2/vehicletype.h
+++ b/source/libr2c2/vehicletype.h
@@ -10,6 +10,7 @@ Distributed under the GPL
 
 #include <msp/datafile/objectloader.h>
 #include "articlenumber.h"
+#include "geometry.h"
 
 namespace R2C2 {
 
@@ -18,6 +19,9 @@ class VehicleType
 public:
 	class Loader: public Msp::DataFile::ObjectLoader<VehicleType>
 	{
+	private:
+		std::map<std::string, unsigned> rod_tags;
+
 	public:
 		Loader(VehicleType &);
 	private:
@@ -26,6 +30,7 @@ public:
 		void function(unsigned, const std::string &);
 		void height(float);
 		void length(float);
+		void rod();
 		void width(float);
 	};
 
@@ -67,6 +72,57 @@ public:
 		Bogie();
 	};
 
+	struct Rod
+	{
+		enum Anchor
+		{
+			BODY,
+			AXLE,
+			BOGIE_AXLE,
+			ROD
+		};
+
+		enum Limit
+		{
+			FIXED,
+			ROTATE,
+			SLIDE_X
+		};
+
+		class Loader: public Msp::DataFile::ObjectLoader<Rod>
+		{
+		private:
+			const std::map<std::string, unsigned> &tags;
+			std::string tag;
+
+		public:
+			Loader(Rod &, const std::map<std::string, unsigned> &);
+			const std::string &get_tag() const { return tag; }
+		private:
+			void connect(const std::string &, float, float, float, float);
+			void limit(Limit);
+			void pivot_body();
+			void pivot_axle(unsigned);
+			void pivot_bogie_axle(unsigned, unsigned);
+			void pivot_rod(const std::string &);
+			void position(float, float, float);
+			void set_tag(const std::string &);
+		};
+
+		Anchor pivot;
+		unsigned pivot_index;
+		unsigned pivot_index2;
+		Point pivot_point;
+		Limit limit;
+		int connect_index;
+		Point connect_point;
+		Point connect_offset;
+		std::string object;
+		bool mirror_object;
+
+		Rod();
+	};
+
 private:
 	ArticleNumber art_nr;
 	std::string name;
@@ -77,6 +133,7 @@ private:
 	float height;
 	std::vector<Axle> axles;
 	std::vector<Bogie> bogies;
+	std::vector<Rod> rods;
 	std::string object;
 
 public:
@@ -92,6 +149,7 @@ public:
 	float get_height() const { return height; }
 	const std::vector<Axle> &get_axles() const { return axles; }
 	const std::vector<Bogie> &get_bogies() const { return bogies; }
+	const std::vector<Rod> &get_rods() const { return rods; }
 	float get_front_axle_offset() const;
 	float get_back_axle_offset() const;
 	const std::string &get_object() const { return object; }