Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add vector2f::closest_distance_between_line_and_point_squared 

also add vector2f::closest_distance_between_line_and_point and vector2f::closest_distance_between_lines_squared
This commit is contained in:
Andrew Tridgell 2019-05-29 21:05:24 +09:00 committed by Randy Mackay
parent 2457bf71d4
commit a5fd7ac5ca
2 changed files with 63 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Math/vector2.cpp
View File

@ -353,6 +353,45 @@ Vector2<T> Vector2<T>::closest_point(const Vector2<T> &p, const Vector2<T> &w)
}
}
// closest distance between a line segment and a point
// https://stackoverflow.com/questions/2824478/shortest-distance-between-two-line-segments
template <typename T>
float Vector2<T>::closest_distance_between_line_and_point_squared(const Vector2<T> &w1,
const Vector2<T> &w2,
const Vector2<T> &p)
{
return closest_distance_between_radial_and_point_squared(w2-w1, p-w1);
}
// w1 and w2 define a line segment
// p is a point
// returns the closest distance between the line segment and the point
template <typename T>
float Vector2<T>::closest_distance_between_line_and_point(const Vector2<T> &w1,
const Vector2<T> &w2,
const Vector2<T> &p)
{
return sqrtf(closest_distance_between_line_and_point_squared(w1, w2, p));
}
// a1->a2 and b2->v2 define two line segments
// returns the square of the closest distance between the two line segments
// see https://stackoverflow.com/questions/2824478/shortest-distance-between-two-line-segments
template <typename T>
float Vector2<T>::closest_distance_between_lines_squared(const Vector2<T> &a1,
const Vector2<T> &a2,
const Vector2<T> &b1,
const Vector2<T> &b2)
{
const float dist1 = Vector2<T>::closest_distance_between_line_and_point_squared(b1,b2,a1);
const float dist2 = Vector2<T>::closest_distance_between_line_and_point_squared(b1,b2,a2);
const float dist3 = Vector2<T>::closest_distance_between_line_and_point_squared(a1,a2,b1);
const float dist4 = Vector2<T>::closest_distance_between_line_and_point_squared(a1,a2,b2);
const float m1 = MIN(dist1,dist2);
const float m2 = MIN(dist3,dist4);
return MIN(m1,m2);
}
// w defines a line segment from the origin
// p is a point
// returns the square of the closest distance between the radial and the point
@ -401,6 +440,9 @@ template bool Vector2<float>::circle_segment_intersection(const Vector2<float>&
template Vector2<float> Vector2<float>::closest_point(const Vector2<float> &p, const Vector2<float> &v, const Vector2<float> &w);
template float Vector2<float>::closest_distance_between_radial_and_point_squared(const Vector2<float> &w, const Vector2<float> &p);
template float Vector2<float>::closest_distance_between_radial_and_point(const Vector2<float> &w, const Vector2<float> &p);
template float Vector2<float>::closest_distance_between_line_and_point(const Vector2<float> &w1, const Vector2<float> &w2, const Vector2<float> &p);
template float Vector2<float>::closest_distance_between_line_and_point_squared(const Vector2<float> &w1, const Vector2<float> &w2, const Vector2<float> &p);
template float Vector2<float>::closest_distance_between_lines_squared(const Vector2<float> &a1,const Vector2<float> &a2,const Vector2<float> &b1,const Vector2<float> &b2);
template bool Vector2<long>::operator ==(const Vector2<long> &v) const;

21
libraries/AP_Math/vector2.h
View File

@ -170,6 +170,27 @@ struct Vector2
*/
static Vector2<T> closest_point(const Vector2<T> &p, const Vector2<T> &w);
// w1 and w2 define a line segment
// p is a point
// returns the square of the closest distance between the line segment and the point
static float closest_distance_between_line_and_point_squared(const Vector2<T> &w1,
const Vector2<T> &w2,
const Vector2<T> &p);
// w1 and w2 define a line segment
// p is a point
// returns the closest distance between the line segment and the point
static float closest_distance_between_line_and_point(const Vector2<T> &w1,
const Vector2<T> &w2,
const Vector2<T> &p);
// a1->a2 and b2->v2 define two line segments
// returns the square of the closest distance between the two line segments
static float closest_distance_between_lines_squared(const Vector2<T> &a1,
const Vector2<T> &a2,
const Vector2<T> &b1,
const Vector2<T> &b2);
// w defines a line segment from the origin
// p is a point
// returns the square of the closest distance between the radial and the point