12 const std::pair<int, int> &
p1,
13 const std::pair<int, int> &
p2)
15 std::pair<float, float> eten;
17 std::pair<float, float> ete;
18 ete.first = p1.first - p2.first;
19 ete.second = p1.second - p2.second;
21 eten.first = ete.first /
sqrt(ete.first*ete.first + ete.second*ete.second);
22 eten.second = ete.second/
sqrt(ete.first*ete.first + ete.second*ete.second);
25 std::pair<float, float> etp;
26 etp.first = p1.first -
x;
27 etp.second = p1.second -
y;
29 const double detca =
fabs(eten.first*etp.first + eten.second*etp.second);
31 std::pair<float, float> closest_app;
33 closest_app.first = p1.first - eten.first *detca;
34 closest_app.second = p1.second - eten.second*detca;
37 (p1.first < closest_app.first && closest_app.first < p2.first) ||
38 (p1.first > closest_app.first && closest_app.first > p2.first);
40 if(between)
return sqrt(
pow(closest_app.first - x, 2) +
41 pow(closest_app.second - y, 2));
45 static float min(
const float a,
const float b,
const float c)
47 if(a < b && a < c)
return a;
48 if(b < a && b < c)
return b;
53 const std::pair<int, int> &
p1,
54 const std::pair<int, int> &
p2)
57 const float dist_to_p1 =
sqrt(
pow(x - p1.first, 2) +
pow(y - p1.second, 2));
58 const float dist_to_p2 =
sqrt(
pow(x - p2.first, 2) +
pow(y - p2.second, 2));
59 return min(dist_to_inf_line, dist_to_p1, dist_to_p2);
63 const std::vector< std::pair<int, int> > &
track)
65 float mindist = FLT_MAX;
66 for(
unsigned int i = 0;
i <
track.size()-1;
i++){
68 if(dist < mindist) mindist =
dist;
fvar< T > fabs(const fvar< T > &x)
static float point_to_line(const int x, const int y, const std::pair< int, int > &p1, const std::pair< int, int > &p2)
static float min(const float a, const float b, const float c)
static float point_to_line_segment(const int x, const int y, const std::pair< int, int > &p1, const std::pair< int, int > &p2)
float screen_dist_to_track(const int x, const int y, const std::vector< std::pair< int, int > > &track)