From: Mikko Rasa <tdb@tdb.fi>
Date: Fri, 12 Mar 2010 09:29:16 +0000 (+0000)
Subject: Turn TrackPart into a proper class
X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=47bff0c8939de3ae20b5ced1ee2f6a264397dcee;p=r2c2.git

Turn TrackPart into a proper class
Link TrackParts to each other for better efficiency in get_point
Move most of get_point to TrackType
Return direction and grade from get_point in addition to position
---

diff --git a/source/3d/tracktype.cpp b/source/3d/tracktype.cpp
index 94a941a..1a898d4 100644
--- a/source/3d/tracktype.cpp
+++ b/source/3d/tracktype.cpp
@@ -151,18 +151,15 @@ void TrackType3D::render(const GL::Tag &tag) const
 
 void TrackType3D::build_part(const TrackPart &part, const Profile &profile, const Point &offset, GL::MeshBuilder &bld, unsigned &base_index)
 {
-	unsigned nsegs = (part.radius ? static_cast<unsigned>(part.length*16)+1 : 1);
-	float plen = part.length;
-	if(part.radius)
-		plen *= abs(part.radius);
+	float plen = part.get_length();
+	unsigned nsegs = (part.is_curved() ? static_cast<unsigned>(plen*16)+1 : 1);
 
 	unsigned n_points = profile.get_n_points();
 	for(unsigned i=0; i<=nsegs; ++i)
 	{
-		float a = part.dir+(part.radius ? i*plen/nsegs/part.radius : 0);
-		float c = cos(a);
-		float s = sin(a);
-		Point basep = part.get_point(i*plen/nsegs);
+		TrackPoint basep = part.get_point(i*plen/nsegs);
+		float c = cos(basep.dir);
+		float s = sin(basep.dir);
 
 		Point p;
 		for(unsigned j=0; j<n_points; ++j)
@@ -170,9 +167,9 @@ void TrackType3D::build_part(const TrackPart &part, const Profile &profile, cons
 			// TODO: smoothing - only duplicate vertex if the angle is large enough
 
 			p = profile.get_point(j);
-			p.z = basep.z+p.y+offset.y;
-			p.y = basep.y-c*(p.x+offset.x);
-			p.x = basep.x+s*(p.x+offset.x);
+			p.z = basep.pos.z+p.y+offset.y;
+			p.y = basep.pos.y-c*(p.x+offset.x);
+			p.x = basep.pos.x+s*(p.x+offset.x);
 			if(j>0)
 				bld.vertex(p.x, p.y, p.z);
 
diff --git a/source/libmarklin/geometry.h b/source/libmarklin/geometry.h
index e9b9e27..fab5cea 100644
--- a/source/libmarklin/geometry.h
+++ b/source/libmarklin/geometry.h
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2010  Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -21,6 +21,15 @@ struct Point
 	Point(float x_, float y_, float z_): x(x_), y(y_), z(z_) { }
 };
 
+struct TrackPoint
+{
+	Point pos;
+	float dir;
+	float grade;
+
+	TrackPoint(): dir(0), grade(0) { }
+};
+
 } // namespace Marklin
 
 #endif
diff --git a/source/libmarklin/track.cpp b/source/libmarklin/track.cpp
index 5843e22..250888d 100644
--- a/source/libmarklin/track.cpp
+++ b/source/libmarklin/track.cpp
@@ -55,7 +55,8 @@ void Track::set_rotation(float r)
 
 void Track::set_slope(float s)
 {
-	if(links.size()!=2) return;
+	if(links.size()!=2)
+		return;
 
 	slope = s;
 }
@@ -286,79 +287,30 @@ unsigned Track::traverse(unsigned i, unsigned path) const
 	throw Exception("Track endpoint did not have a counterpart");
 }
 
-Point Track::get_point(unsigned epi, unsigned path, float d) const
+TrackPoint Track::get_point(unsigned epi, unsigned path, float d) const
 {
-	const vector<Endpoint> &eps = type.get_endpoints();
-	if(epi>=eps.size())
-		throw InvalidParameterValue("Endpoint index out of range");
+	TrackPoint p = type.get_point(epi, path, d);
+	float c = cos(rot);
+	float s = sin(rot);
 
-	float x = eps[epi].pos.x;
-	float y = eps[epi].pos.y;
-	float z = 0;
-	float slope_norm = 0;
-	if(eps.size()==2)
+	p.pos = Point(pos.x+c*p.pos.x-s*p.pos.y, pos.y+s*p.pos.x+c*p.pos.y, 0);
+	if(type.get_endpoints().size()==2)
 	{
-		slope_norm = slope/type.get_total_length();
-		if(epi==1)
+		float len = type.get_path_length(path);
+		float grade = slope/len;
+		if(epi==0)
 		{
-			z = slope;
-			slope_norm = -slope_norm;
+			p.pos.z = pos.z+grade*d;
+			p.grade = grade;
 		}
-	}
-
-	const vector<TrackPart> &parts = type.get_parts();
-	const TrackPart *last_part = 0;
-	while(1)
-	{
-		for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
+		else
 		{
-			if((eps[epi].paths&(1<<path)) && i->path!=path)
-				continue;
-			if(&*i==last_part)
-				continue;
-
-			vector<Endpoint> part_eps;
-			i->collect_endpoints(part_eps);
-			for(unsigned j=0; j<part_eps.size(); ++j)
-			{
-				float dx = part_eps[j].pos.x-x;
-				float dy = part_eps[j].pos.y-y;
-				if(dx*dx+dy*dy<1e-6)
-				{
-					float plen = i->length;
-					if(i->radius)
-						plen *= abs(i->radius);
-					if(d<plen)
-					{
-						z += d*slope_norm;
-						if(j==1)
-							d = plen-d;
-						Point p = i->get_point(d);
-						float c = cos(rot);
-						float s = sin(rot);
-						return Point(pos.x+c*p.x-s*p.y, pos.y+c*p.y+s*p.x, pos.z+z);
-					}
-					else if(part_eps.size()>1)
-					{
-						d -= plen;
-						x = part_eps[1-j].pos.x;
-						y = part_eps[1-j].pos.y;
-						z += plen*slope_norm;
-						last_part = &*i;
-						i = parts.begin();
-						break;
-					}
-					else
-						return pos;
-				}
-			}
+			p.pos.z = pos.z+slope-grade*d;
+			p.grade = -grade;
 		}
-
-		if(!last_part)
-			throw Exception("Internal error (Endpoint does not match any part)");
-		else
-			return pos;
 	}
+
+	return p;
 }
 
 void Track::save(list<DataFile::Statement> &st) const
diff --git a/source/libmarklin/track.h b/source/libmarklin/track.h
index 7ef5119..c1c9581 100644
--- a/source/libmarklin/track.h
+++ b/source/libmarklin/track.h
@@ -79,7 +79,7 @@ public:
 	const std::vector<Track *> &get_links() const { return links; }
 	Track *get_link(unsigned) const;
 	unsigned traverse(unsigned, unsigned) const;
-	Point get_point(unsigned, unsigned, float) const;
+	TrackPoint get_point(unsigned, unsigned, float) const;
 
 	void save(std::list<Msp::DataFile::Statement> &) const;
 private:
diff --git a/source/libmarklin/trackpart.cpp b/source/libmarklin/trackpart.cpp
index f2b61be..938ae31 100644
--- a/source/libmarklin/trackpart.cpp
+++ b/source/libmarklin/trackpart.cpp
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2010  Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -9,6 +9,9 @@ Distributed under the GPL
 #include "trackpart.h"
 
 using namespace std;
+using namespace Msp;
+
+#include <msp/io/print.h>
 
 namespace Marklin {
 
@@ -18,26 +21,23 @@ TrackPart::TrackPart():
 	radius(0),
 	path(0),
 	dead_end(false)
-{ }
-
-void TrackPart::collect_endpoints(vector<Endpoint> &eps) const
 {
-	eps.push_back(Endpoint(pos.x, pos.y, dir+M_PI, 1<<path));
+	links[0] = 0;
+	links[1] = 0;
+}
 
-	if(dead_end)
-		;
-	else if(radius)
-	{
-		float a = ((radius<0) ? -length : length);
-		Point p = get_point(length*abs(radius));
-		eps.push_back(Endpoint(p.x, p.y, dir+a, 1<<path));
-	}
+float TrackPart::get_length() const
+{
+	if(radius)
+		return abs(radius)*length;
 	else
-		eps.push_back(Endpoint(pos.x+cos(dir)*length, pos.y+sin(dir)*length, dir, 1<<path));
+		return length;
 }
 
-Point TrackPart::get_point(float d) const
+TrackPoint TrackPart::get_point(float d) const
 {
+	TrackPoint result;
+
 	if(radius)
 	{
 		float a = d/radius;
@@ -45,10 +45,54 @@ Point TrackPart::get_point(float d) const
 		float s = sin(a);
 		float rx = radius*sin(dir);
 		float ry = -radius*cos(dir);
-		return Point(pos.x+c*rx-s*ry-rx, pos.y+c*ry+s*rx-ry);
+		result.pos = Point(pos.x+c*rx-s*ry-rx, pos.y+c*ry+s*rx-ry);
+		result.dir = dir+a;
 	}
 	else
-		return Point(pos.x+cos(dir)*d, pos.y+sin(dir)*d);
+	{
+		result.pos = Point(pos.x+cos(dir)*d, pos.y+sin(dir)*d);
+		result.dir = dir;
+	}
+
+	return result;
+}
+
+void TrackPart::check_link(TrackPart &other)
+{
+	unsigned n_eps = (dead_end ? 1 : 2);
+	unsigned n_other_eps = (other.is_dead_end() ? 1 : 2);
+	for(unsigned i=0; i<n_eps; ++i)
+	{
+		TrackPoint p1 = get_point(i ? get_length() : 0);
+		for(unsigned j=0; j<n_other_eps; ++j)
+		{
+			TrackPoint p2 = other.get_point(j ? other.get_length() : 0);
+
+			float dx = p2.pos.x-p1.pos.x;
+			float dy = p2.pos.y-p1.pos.y;
+
+			float da = p2.dir-p1.dir+M_PI*((i+j+1)%2);
+			while(da>M_PI)
+				da -= M_PI*2;
+			while(da<-M_PI)
+				da += M_PI*2;
+
+			if(dx*dx+dy*dy<1e-6 && da>=-0.01 && da<=0.01)
+			{
+				IO::print("TrackParts linked!\n");
+				links[i] = &other;
+				other.links[j] = this;
+				return;
+			}
+		}
+	}
+}
+
+TrackPart *TrackPart::get_link(unsigned i) const
+{
+	if(i>=2)
+		throw InvalidParameterValue("Index out of range");
+	return links[i];
 }
 
 
diff --git a/source/libmarklin/trackpart.h b/source/libmarklin/trackpart.h
index 0411d72..aebe7ef 100644
--- a/source/libmarklin/trackpart.h
+++ b/source/libmarklin/trackpart.h
@@ -1,7 +1,7 @@
 /* $Id$
 
 This file is part of the MSP Märklin suite
-Copyright © 2006-2009 Mikkosoft Productions, Mikko Rasa
+Copyright © 2006-2010  Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
@@ -14,8 +14,9 @@ Distributed under the GPL
 
 namespace Marklin {
 
-struct TrackPart
+class TrackPart
 {
+public:
 	class Loader: public Msp::DataFile::BasicLoader<TrackPart>
 	{
 	public:
@@ -25,17 +26,25 @@ struct TrackPart
 		void start(float, float, float);
 	};
 
+private:
 	Point pos;
 	float dir;
 	float length;
 	float radius;
 	unsigned path;
 	bool dead_end;
+	TrackPart *links[2];
 
+public:
 	TrackPart();
 
-	void collect_endpoints(std::vector<Endpoint> &) const;
-	Point get_point(float) const;
+	float get_length() const;
+	bool is_curved() const { return radius; }
+	TrackPoint get_point(float) const;
+	unsigned get_path() const { return path; }
+	bool is_dead_end() const { return dead_end; }
+	void check_link(TrackPart &);
+	TrackPart *get_link(unsigned) const;
 };
 
 } // namespace Marklin
diff --git a/source/libmarklin/tracktype.cpp b/source/libmarklin/tracktype.cpp
index 5bbd40a..314c797 100644
--- a/source/libmarklin/tracktype.cpp
+++ b/source/libmarklin/tracktype.cpp
@@ -9,6 +9,7 @@ Distributed under the GPL
 #include "tracktype.h"
 
 using namespace std;
+using namespace Msp;
 
 namespace Marklin {
 
@@ -25,13 +26,8 @@ float TrackType::get_path_length(int p) const
 {
 	float len = 0;
 	for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
-		if(p<0 || i->path==static_cast<unsigned>(p))
-		{
-			float l = i->length;
-			if(i->radius)
-				l *= abs(i->radius);
-			len += l;
-		}
+		if(p<0 || i->get_path()==static_cast<unsigned>(p))
+			len += i->get_length();
 	return len;
 }
 
@@ -39,7 +35,7 @@ unsigned TrackType::get_paths() const
 {
 	unsigned mask = 0;
 	for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
-		mask |= 1<<i->path;
+		mask |= 1<<i->get_path();
 	return mask;
 }
 
@@ -61,40 +57,99 @@ bool TrackType::is_double_address() const
 	return get_n_paths()>2;
 }
 
-void TrackType::collect_endpoints()
+TrackPoint TrackType::get_point(unsigned epi, unsigned path, float d) const
 {
-	endpoints.clear();
+	if(epi>=endpoints.size())
+		throw InvalidParameterValue("Endpoint index out of range");
 
-	for(vector<TrackPart>::iterator i=parts.begin(); i!=parts.end(); ++i)
-		i->collect_endpoints(endpoints);
-
-	for(vector<Endpoint>::iterator i=endpoints.begin(); i!=endpoints.end();)
+	const TrackPart *part = 0;
+	unsigned part_ep = 0;
+	for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
 	{
-		bool rm = false;
-		for(vector<Endpoint>::iterator j=i+1; j!=endpoints.end();)
+		if((endpoints[epi].paths&(1<<path)) && i->get_path()!=path)
+			continue;
+
+		unsigned n_part_eps = (i->is_dead_end() ? 1 : 2);
+		for(unsigned j=0; j<n_part_eps; ++j)
 		{
-			float dx = i->pos.x-j->pos.x;
-			float dy = i->pos.y-j->pos.y;
-			if(dx*dx+dy*dy<0.0001)
+			TrackPoint p = i->get_point(j ? i->get_length() : 0);
+			float dx = p.pos.x-endpoints[epi].pos.x;
+			float dy = p.pos.y-endpoints[epi].pos.y;
+			if(dx*dx+dy*dy<1e-6)
 			{
-				float da = i->dir-j->dir;
-				if(da<-M_PI)
-					da += M_PI*2;
-				if(da>M_PI)
-					da -= M_PI*2;
-				if(da<-3.1 || da>3.1)
-					rm = true;
-				i->paths |= j->paths;
-				j = endpoints.erase(j);
+				part = &*i;
+				part_ep = j;
 			}
-			else
-				++j;
 		}
+	}
 
-		if(rm)
-			i = endpoints.erase(i);
+	if(!part)
+		throw Exception("Internal error (endpoint does not match any part)");
+
+	while(1)
+	{
+		float plen = part->get_length();
+		if(d<plen)
+		{
+			if(part_ep==1)
+				d = plen-d;
+			TrackPoint p = part->get_point(d);
+			if(part_ep==1)
+				p.dir += M_PI;
+			return p;
+		}
 		else
-			++i;
+		{
+			d -= plen;
+			TrackPart *next = part->get_link(1-part_ep);
+			if(!next)
+				throw InvalidParameterValue("Distance out of range");
+			part_ep = (next->get_link(0)==part ? 0 : 1);
+			part = next;
+		}
+	}
+}
+
+void TrackType::collect_endpoints()
+{
+	endpoints.clear();
+
+	for(vector<TrackPart>::iterator i=parts.begin(); i!=parts.end(); ++i)
+	{
+		for(vector<TrackPart>::iterator j=i; ++j!=parts.end();)
+			i->check_link(*j);
+
+		unsigned n_part_eps = (i->is_dead_end() ? 1 : 2);
+		for(unsigned j=0; j<n_part_eps; ++j)
+			if(!i->get_link(j))
+			{
+				TrackPoint p = i->get_point(j ? i->get_length() : 0);
+				if(j==0)
+					p.dir += M_PI;
+
+				bool found = false;
+				for(vector<Endpoint>::iterator k=endpoints.begin(); k!=endpoints.end(); ++k)
+				{
+					float dx = k->pos.x-p.pos.x;
+					float dy = k->pos.y-p.pos.y;
+
+					float da = k->dir-p.dir;
+					while(da>M_PI)
+						da -= M_PI*2;
+					while(da<-M_PI)
+						da += M_PI*2;
+
+					if(dx*dx+dy*dy<1e-6 && da>-0.01 && da<0.01)
+					{
+						k->paths |= 1<<i->get_path();
+						found = true;
+						break;
+					}
+				}
+
+				if(!found)
+					endpoints.push_back(Endpoint(p.pos.x, p.pos.y, p.dir, 1<<i->get_path()));
+			}
 	}
 }
 
diff --git a/source/libmarklin/tracktype.h b/source/libmarklin/tracktype.h
index 6f636e0..14c9866 100644
--- a/source/libmarklin/tracktype.h
+++ b/source/libmarklin/tracktype.h
@@ -46,6 +46,7 @@ public:
 	bool is_double_address() const;
 	const std::vector<TrackPart> &get_parts() const { return parts; }
 	const std::vector<Endpoint> &get_endpoints() const { return endpoints; }
+	TrackPoint get_point(unsigned, unsigned, float) const;
 
 private:
 	void collect_endpoints();
diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 63a14e9..bb36bcf 100644
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -214,7 +214,7 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 		}
 
 		if(cur_track)
-			pos = cur_track->get_point(cur_track_ep, path, offset);
+			pos = cur_track->get_point(cur_track_ep, path, offset).pos;
 	}
 }