diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 2d6688f488..55724896b3 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -339,3 +339,20 @@ bool Location::past_interval_finish_line(const Location &point1, const Location return this->line_path_proportion(point1, point2) >= 1.0f; } +/* + return the proportion we are along the path from point1 to + point2, along a line parallel to point1<->point2. + + This will be more than 1 if we have passed point2 + */ +float Location::line_path_proportion(const Location &point1, const Location &point2) const +{ + const Vector2f vec1 = point1.get_distance_NE(point2); + const Vector2f vec2 = point1.get_distance_NE(*this); + const float dsquared = sq(vec1.x) + sq(vec1.y); + if (dsquared < 0.001f) { + // the two points are very close together + return 1.0f; + } + return (vec1 * vec2) / dsquared; +} diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 7b4c7890dd..cdfeb5341b 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -105,6 +105,13 @@ public: // the target waypoint bool past_interval_finish_line(const Location &point1, const Location &point2) const; + /* + return the proportion we are along the path from point1 to + point2, along a line parallel to point1<->point2. + This will be more than 1 if we have passed point2 + */ + float line_path_proportion(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 a289877bfb..cabcc811a1 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -39,27 +39,6 @@ float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) return bearing; } -/* - return the proportion we are along the path from point1 to - point2, along a line parallel to point1<->point2. - - This will be less than >1 if we have passed point2 - */ -float location_path_proportion(const struct Location &location, - const struct Location &point1, - const struct Location &point2) -{ - const Vector2f vec1 = point1.get_distance_NE(point2); - const Vector2f vec2 = point1.get_distance_NE(location); - float dsquared = sq(vec1.x) + sq(vec1.y); - if (dsquared < 0.001f) { - // the two points are very close together - return 1.0f; - } - return (vec1 * vec2) / dsquared; -} - - // 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 7c5afbf55a..b326c9d5a7 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); -/* - return the proportion we are along the path from point1 to - point2. This will be less than >1 if we have passed point2 - */ -float location_path_proportion(const struct Location &location, - const struct Location &point1, - const struct Location &point2); - - // Converts from WGS84 geodetic coordinates (lat, lon, height) // into WGS84 Earth Centered, Earth Fixed (ECEF) coordinates // (X, Y, Z)