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

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

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)