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 f21aac4d05a438ed6e2dde5b454b548e8e2d86c5..540c24743d05136f121816cba463a92c8f395c7f 100644 (file)
--- 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 480b1a33d42abd072ea8cc6f1f59523b65c5d486..3ae5d602295dcd1fdcbf074b8d47880e8d5e45c8 100644 (file)
--- 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 8c0c2d0038fdf3a6cf7c1a50b0f747c46cf95208..71845bb73c2c22591d3f99b59aaf4d455b2d7988 100644 (file)
--- 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 687be4b9870d75e54abe162285297175801b56cf..787a6ca5777bd4543d53c16d82992cda0f1fc6a9 100644 (file)
--- 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 29bf47be5409d42eb79fbd9644fbf1a62e08e1bc..23c636c6d1cd1689d4a809c7fb23f3929f27c6b7 100644 (file)
--- 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 1b1c78ce998de86b736d252774643bce2346bfba..a292e5532b625439de120555899ff3637391047b 100644 (file)
--- 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: