diff --git a/src/gps/GeoCoord.cpp b/src/gps/GeoCoord.cpp index c3bac700a..d09b1addb 100644 --- a/src/gps/GeoCoord.cpp +++ b/src/gps/GeoCoord.cpp @@ -390,4 +390,61 @@ float GeoCoord::bearing(double lat1, double lon1, double lat2, double lon2) double y = sin(deltaLonRad) * cos(lat2Rad); double x = cos(lat1Rad) * sin(lat2Rad) - (sin(lat1Rad) * cos(lat2Rad) * cos(deltaLonRad)); return atan2(y, x); +} + +/** + * Ported from http://www.edwilliams.org/avform147.htm#Intro + * @brief Convert from meters to range in radians on a great circle + * @param range_meters + * The range in meters + * @return range in radians on a great circle + */ +float GeoCoord::rangeMetersToRadians(double range_meters) { + // 1 nm is 1852 meters + double distance_nm = range_meters * 1852; + return (PI / (180 * 60)) *distance_nm; +} + +/** + * Ported from http://www.edwilliams.org/avform147.htm#Intro + * @brief Convert from radians to range in meters on a great circle + * @param range_radians + * The range in radians + * @return Range in meters on a great circle + */ +float GeoCoord::rangeRadiansToMeters(double range_radians) { + double distance_nm = ((180 * 60) / PI) * range_radians; + // 1 meter is 0.000539957 nm + return distance_nm * 0.000539957; +} + +// Find distance from point to passed in point +int32_t GeoCoord::distanceTo(GeoCoord pointB) { + return latLongToMeter(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7, pointB.getLongitude() * 1e-7); +} + +// Find bearing from point to passed in point +int32_t GeoCoord::bearingTo(GeoCoord pointB) { + return bearing(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7, pointB.getLongitude() * 1e-7); +} + +/** + * Create a new point bassed on the passed in poin + * Ported from http://www.edwilliams.org/avform147.htm#LL + * @param bearing + * The bearing in raidans + * @param range_meters + * range in meters + * @return GeoCoord object of point at bearing and range from initial point +*/ +std::shared_ptr GeoCoord::pointAtDistance(double bearing, double range_meters) { + double range_radians = rangeMetersToRadians(range_meters); + double lat1 = this->getLatitude() * 1e-7; + double lon1 = this->getLongitude() * 1e-7; + double lat = asin(sin(lat1) * cos(range_radians) + cos(lat1) * sin(range_radians) * cos(bearing)); + double dlon = atan2(sin(bearing) * sin(range_radians) * cos(lat1), cos(range_radians) - sin(lat1) * sin(lat)); + double lon = fmod(lon1 - dlon + PI, 2 * PI) - PI; + + return std::make_shared(double(lat), double(lon), this->getAltitude()); + } \ No newline at end of file diff --git a/src/gps/GeoCoord.h b/src/gps/GeoCoord.h index 7a2c1aa9d..f3795c847 100644 --- a/src/gps/GeoCoord.h +++ b/src/gps/GeoCoord.h @@ -7,6 +7,7 @@ #include #include #include +#include #define PI 3.1415926535897932384626433832795 #define OLC_CODE_LEN 11 @@ -114,6 +115,13 @@ class GeoCoord { static void convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude); static float latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b); static float bearing(double lat1, double lon1, double lat2, double lon2); + static float rangeRadiansToMeters(double range_radians); + static float rangeMetersToRadians(double range_meters); + + // Point to point conversions + int32_t distanceTo(GeoCoord pointB); + int32_t bearingTo(GeoCoord pointB); + std::shared_ptr pointAtDistance(double bearing, double range); // Lat lon alt getters int32_t getLatitude() const { return _latitude; }