Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: move line_path_proportion to Location

Browse Source
This commit is contained in:
Pierre Kancir 2019-04-09 10:24:44 +02:00 committed by Andrew Tridgell
parent e06d7dbbf7
commit 4f31c3dcd5
4 changed files with 24 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_Common/Location.cpp
View File

@ -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 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;
}

7
libraries/AP_Common/Location.h
View File

@ -105,6 +105,13 @@ public:
// the target waypoint // the target waypoint
bool past_interval_finish_line(const Location &point1, const Location &point2) const; 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: private:
static AP_Terrain *_terrain; static AP_Terrain *_terrain;
}; };

21
libraries/AP_Math/location.cpp
View File

@ -39,27 +39,6 @@ float get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
return bearing; 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 // return true when lat and lng are within range
bool check_lat(float lat) bool check_lat(float lat)
{ {

9
libraries/AP_Math/location.h
View File

@ -22,15 +22,6 @@ float get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &
// return bearing in centi-degrees between two positions // return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f &origin, const Vector3f &destination); 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) // Converts from WGS84 geodetic coordinates (lat, lon, height)
// into WGS84 Earth Centered, Earth Fixed (ECEF) coordinates // into WGS84 Earth Centered, Earth Fixed (ECEF) coordinates
// (X, Y, Z) // (X, Y, Z)