diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index f6b6a83e67..2d6688f488 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -328,3 +328,14 @@ bool Location::check_latlng() const { return check_lat(lat) && check_lng(lng); } + +// see if location is past a line perpendicular to +// the line between point1 and point2 and passing through point2. +// If point1 is our previous waypoint and point2 is our target waypoint +// then this function returns true if we have flown past +// the target waypoint +bool Location::past_interval_finish_line(const Location &point1, const Location &point2) const +{ + return this->line_path_proportion(point1, point2) >= 1.0f; +} + diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index f74dae138a..7b4c7890dd 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -98,6 +98,13 @@ public: // return true when lat and lng are within range bool check_latlng() const; + // see if location is past a line perpendicular to + // the line between point1 and point2 and passing through point2. + // If point1 is our previous waypoint and point2 is our target waypoint + // then this function returns true if we have flown past + // the target waypoint + bool past_interval_finish_line(const Location &point1, const Location &point2) const; + private: static AP_Terrain *_terrain; }; diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index dcc3e27c80..a289877bfb 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -39,19 +39,6 @@ float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) return bearing; } -// see if location is past a line perpendicular to -// the line between point1 and point2. If point1 is -// our previous waypoint and point2 is our target waypoint -// then this function returns true if we have flown past -// the target waypoint -bool location_passed_point(const struct Location &location, - const struct Location &point1, - const struct Location &point2) -{ - return location_path_proportion(location, point1, point2) >= 1.0f; -} - - /* return the proportion we are along the path from point1 to point2, along a line parallel to point1<->point2. @@ -73,11 +60,6 @@ float location_path_proportion(const struct Location &location, } - - - - - // return true when lat and lng are within range bool check_lat(float lat) { diff --git a/libraries/AP_Math/location.h b/libraries/AP_Math/location.h index 78b5f4b277..7c5afbf55a 100644 --- a/libraries/AP_Math/location.h +++ b/libraries/AP_Math/location.h @@ -22,15 +22,6 @@ float get_horizontal_distance_cm(const Vector3f &origin, const Vector3f & // return bearing in centi-degrees between two positions float get_bearing_cd(const Vector3f &origin, const Vector3f &destination); -// see if location is past a line perpendicular to -// the line between point1 and point2. If point1 is -// our previous waypoint and point2 is our target waypoint -// then this function returns true if we have flown past -// the target waypoint -bool location_passed_point(const struct Location & location, - const struct Location & point1, - const struct Location & point2); - /* return the proportion we are along the path from point1 to point2. This will be less than >1 if we have passed point2