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

AP_Math: Polygon_closest_distance_line returns neg numbers 

value is negative distance from intersection to p2
This commit is contained in:
Randy Mackay 2019-05-13 13:00:02 +09:00
parent 6eb816fd3b
commit 071e340827
2 changed files with 18 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Math/polygon.cpp
View File

@ -128,11 +128,12 @@ template bool Polygon_complete<float>(const Vector2f *V, unsigned n);
/*
determine if the polygon of N verticies defined by points V is
intersected by a line from point p1 to point p2
intersection argument returns the intersection closest to p1
*/
bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2)
bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2, Vector2f &intersection)
{
float intersect_dist_sq = FLT_MAX;
for (uint8_t i=0; i<N-1; i++) {
Vector2f tmp;
const Vector2f &v1 = V[i];
const Vector2f &v2 = V[i+1];
// optimisations for common cases
@ -148,21 +149,28 @@ bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const
if (v1.y < p1.y && v2.y < p1.y && v1.y < p2.y && v2.y < p2.y) {
continue;
}
if (Vector2f::segment_intersection(v1,v2,p1,p2,tmp)) {
return true;
Vector2f intersect_tmp;
if (Vector2f::segment_intersection(v1,v2,p1,p2,intersect_tmp)) {
float dist_sq = sq(intersect_tmp.x - p1.x) + sq(intersect_tmp.y - p1.y);
if (dist_sq < intersect_dist_sq) {
intersect_dist_sq = dist_sq;
intersection = intersect_tmp;
}
}
}
return false;
return (intersect_dist_sq < FLT_MAX);
}
/*
return the closest distance that a line from p1 to p2 comes to an
edge of closed polygon V, defined by N points
negative numbers indicate the line cross into the polygon with the negative size being the distance from p2 to the intersection point closest to p1
*/
float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2)
{
if (Polygon_intersects(V,N,p1,p2)) {
return 0;
Vector2f intersection;
if (Polygon_intersects(V,N,p1,p2,intersection)) {
return -sqrtf(sq(intersection.x - p2.x) + sq(intersection.y - p2.y));
}
float closest_sq = FLT_MAX;
for (uint8_t i=0; i<N-1; i++) {

4
libraries/AP_Math/polygon.h
View File

@ -27,13 +27,15 @@ bool Polygon_complete(const Vector2<T> *V, unsigned n);
/*
determine if the polygon of N verticies defined by points V is
intersected by a line from point p1 to point p2
intersection argument returns the intersection closest to p1
*/
bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2);
bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2, Vector2f &intersection);
/*
return the closest distance that a line from p1 to p2 comes to an
edge of closed polygon V, defined by N points
negative numbers indicate the line cross into the polygon with the negative size being the distance from p2 to the intersection point closest to p1
*/
float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2);