Add functions to resolve an arbitrary point to the nearest point on a track

[r2c2.git] / source / libr2c2 / trackpart.cpp

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);