From: Mikko Rasa Date: Sat, 26 Jan 2013 20:37:54 +0000 (+0200) Subject: Add functions to resolve an arbitrary point to the nearest point on a track X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=e75f12451201fb6540ad2155e8796c2b5aee8d4c;p=r2c2.git Add functions to resolve an arbitrary point to the nearest point on a track --- 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(alength/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::const_iterator i=parts.begin(); i!=parts.end(); ++i) + { + TrackPoint n = i->get_nearest_point(p); + float d = distance(n.pos, p); + if(d::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 &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: