From e75f12451201fb6540ad2155e8796c2b5aee8d4c Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sat, 26 Jan 2013 22:37:54 +0200
Subject: [PATCH] Add functions to resolve an arbitrary point to the nearest
 point on a track

---
 source/libr2c2/track.cpp     | 13 +++++++++++++
 source/libr2c2/track.h       |  1 +
 source/libr2c2/trackpart.cpp | 31 +++++++++++++++++++++++++++++++
 source/libr2c2/trackpart.h   |  1 +
 source/libr2c2/tracktype.cpp | 19 +++++++++++++++++++
 source/libr2c2/tracktype.h   |  1 +
 6 files changed, 66 insertions(+)

diff --git a/source/libr2c2/track.cpp b/source/libr2c2/track.cpp
index f21aac4..540c247 100644
--- a/source/libr2c2/track.cpp
+++ b/source/libr2c2/track.cpp
@@ -326,6 +326,19 @@ TrackPoint Track::get_point(unsigned epi, float d) const
 	return get_point(epi, active_path, d);
 }
 
+TrackPoint Track::get_nearest_point(const Vector &p) const
+{
+	Vector local(p.x-pos.x, p.y-pos.y, p.z-pos.z);
+	float c = cos(rot);
+	float s = sin(rot);
+	local = Vector(c*local.x+s*local.y, c*local.y-s*local.x, local.z);
+
+	TrackPoint tp = type.get_nearest_point(local);
+	tp.pos = Vector(pos.x+tp.pos.x*c-tp.pos.y*s, pos.y+tp.pos.y*c+tp.pos.x*s, pos.z+tp.pos.z);
+	tp.dir += rot;
+	return tp;
+}
+
 bool Track::collide_ray(const Vector &start, const Vector &ray)
 {
 	Vector local_start(start.x-pos.x, start.y-pos.y, start.z-pos.z);
diff --git a/source/libr2c2/track.h b/source/libr2c2/track.h
index 480b1a3..3ae5d60 100644
--- a/source/libr2c2/track.h
+++ b/source/libr2c2/track.h
@@ -81,6 +81,7 @@ public:
 	Track *get_link(unsigned) const;
 	TrackPoint get_point(unsigned, unsigned, float) const;
 	TrackPoint get_point(unsigned, float) const;
+	TrackPoint get_nearest_point(const Vector &) const;
 
 	bool collide_ray(const Vector &, const Vector &);
 
diff --git a/source/libr2c2/trackpart.cpp b/source/libr2c2/trackpart.cpp
index 8c0c2d0..71845bb 100644
--- a/source/libr2c2/trackpart.cpp
+++ b/source/libr2c2/trackpart.cpp
@@ -48,6 +48,37 @@ TrackPoint TrackPart::get_point(float d) const
 	return result;
 }
 
+TrackPoint TrackPart::get_nearest_point(const Vector &p) const
+{
+	TrackPoint tp;
+	float c = cos(dir);
+	float s = sin(dir);
+	if(radius)
+	{
+		Vector v(p.x-pos.x+radius*s, p.y-pos.y-radius*c);
+		float a = atan2(v.y, v.x)+M_PI/2-dir;
+		if(radius<0)
+			a = M_PI-a;
+		while(a<length/2-M_PI)
+			a += M_PI*2;
+		while(a>length/2+M_PI)
+			a -= M_PI*2;
+		a = min(max(a, 0.0f), length);
+		if(radius<0)	
+			a = -a;
+		tp.pos = Vector(pos.x+radius*(sin(dir+a)-s), pos.y+radius*(c-cos(dir+a)));
+		tp.dir = dir+a;
+	}
+	else
+	{
+		Vector v(p.x-pos.x, p.y-pos.y);
+		float d = min(max(c*v.x+s*v.y, 0.0f), length);
+		tp.pos = Vector(pos.x+c*d, pos.y+s*d);
+		tp.dir = dir;
+	}
+	return tp;
+}
+
 void TrackPart::check_link(TrackPart &other)
 {
 	unsigned n_eps = (dead_end ? 1 : 2);
diff --git a/source/libr2c2/trackpart.h b/source/libr2c2/trackpart.h
index 687be4b..787a6ca 100644
--- a/source/libr2c2/trackpart.h
+++ b/source/libr2c2/trackpart.h
@@ -33,6 +33,7 @@ public:
 	float get_length() const;
 	bool is_curved() const { return radius; }
 	TrackPoint get_point(float) const;
+	TrackPoint get_nearest_point(const Vector &) const;
 	unsigned get_path() const { return path; }
 	bool is_dead_end() const { return dead_end; }
 	void check_link(TrackPart &);
diff --git a/source/libr2c2/tracktype.cpp b/source/libr2c2/tracktype.cpp
index 29bf47b..23c636c 100644
--- a/source/libr2c2/tracktype.cpp
+++ b/source/libr2c2/tracktype.cpp
@@ -113,6 +113,25 @@ TrackPoint TrackType::get_point(unsigned epi, unsigned path, float d) const
 	}
 }
 
+TrackPoint TrackType::get_nearest_point(const Vector &p) const
+{
+	TrackPoint result;
+	float dist = -1;
+
+	for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
+	{
+		TrackPoint n = i->get_nearest_point(p);
+		float d = distance(n.pos, p);
+		if(d<dist || dist<0)
+		{
+			result = n;
+			dist = d;
+		}
+	}
+
+	return result;
+}
+
 bool TrackType::collide_ray(const Vector &start, const Vector &dir, float width) const
 {
 	for(vector<TrackPart>::const_iterator i=parts.begin(); i!=parts.end(); ++i)
diff --git a/source/libr2c2/tracktype.h b/source/libr2c2/tracktype.h
index 1b1c78c..a292e55 100644
--- a/source/libr2c2/tracktype.h
+++ b/source/libr2c2/tracktype.h
@@ -61,6 +61,7 @@ public:
 	const std::vector<Endpoint> &get_endpoints() const { return endpoints; }
 	const Endpoint &get_endpoint(unsigned) const;
 	TrackPoint get_point(unsigned, unsigned, float) const;
+	TrackPoint get_nearest_point(const Vector &) const;
 	bool collide_ray(const Vector &, const Vector &, float) const;
 
 private:
-- 
2.45.2