Skip to content

Commit

Permalink
Geo: Update CMake GeographicLib Inclusion
Browse files Browse the repository at this point in the history
  • Loading branch information
@HTRamsey
HTRamsey committed Sep 9, 2024
1 parent b4a11c5 commit 3139615
Show file tree
Hide file tree
Showing 5 changed files with 258 additions and 190 deletions.
41 changes: 41 additions & 0 deletions cmake/FindGeographicLib.cmake
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
# Look for GeographicLib
#
# Set
# GeographicLib_FOUND = TRUE
# GeographicLib_INCLUDE_DIRS = /usr/local/include
# GeographicLib_LIBRARIES = /usr/local/lib/libGeographicLib.so
# GeographicLib_LIBRARY_DIRS = /usr/local/lib

find_library (GeographicLib_LIBRARIES GeographicLib
PATHS "${CMAKE_INSTALL_PREFIX}/../GeographicLib/lib")

if (GeographicLib_LIBRARIES)
get_filename_component (GeographicLib_LIBRARY_DIRS
"${GeographicLib_LIBRARIES}" PATH)
get_filename_component (_ROOT_DIR "${GeographicLib_LIBRARY_DIRS}" PATH)
set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include")
set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin")
if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h")
# On Debian systems the library is in e.g.,
# /usr/lib/x86_64-linux-gnu/libGeographicLib.so
# so try stripping another element off _ROOT_DIR
get_filename_component (_ROOT_DIR "${_ROOT_DIR}" PATH)
set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include")
set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin")
if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h")
unset (GeographicLib_INCLUDE_DIRS)
unset (GeographicLib_LIBRARIES)
unset (GeographicLib_LIBRARY_DIRS)
unset (GeographicLib_BINARY_DIRS)
endif ()
endif ()
unset (_ROOT_DIR)
endif ()

# This sets GeographicLib_FOUND (and GEOGRAPHICLIB_FOUND, deprecated) to TRUE
include (FindPackageHandleStandardArgs)
find_package_handle_standard_args (GeographicLib DEFAULT_MSG
GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES
GeographicLib_INCLUDE_DIRS)
mark_as_advanced (GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES
GeographicLib_INCLUDE_DIRS)
41 changes: 32 additions & 9 deletions src/Geo/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,26 +15,49 @@ target_link_libraries(Geo

target_include_directories(Geo PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

set(MINIMUM_GEOGRAPHICLIB_VERSION 2.3)

if(NOT CMAKE_CROSSCOMPILING)
find_package(PkgConfig)
if(PkgConfig_FOUND)
pkg_check_modules(geographiclib IMPORTED_TARGET geographiclib>=2.3)
if(geographiclib_FOUND)
target_link_libraries(Geo PRIVATE PkgConfig::geographiclib)
return()
find_package(GeographicLib ${MINIMUM_GEOGRAPHICLIB_VERSION} COMPONENTS STATIC)
if(GeographicLib_FOUND)
message(STATUS "Found GeographicLib")
target_link_libraries(Geo PRIVATE ${GeographicLib_LIBRARIES})
target_include_directories(Geo PRIVATE ${GeographicLib_INCLUDE_DIRS})
target_link_directories(Geo PRIVATE ${GeographicLib_LIBRARY_DIRS})
cmake_print_variables(GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS GeographicLib_LIBRARY_DIRS)
return()
else()
find_package(PkgConfig)
if(PkgConfig_FOUND)
pkg_check_modules(GeographicLib IMPORTED_TARGET GeographicLib>=${MINIMUM_GEOGRAPHICLIB_VERSION})
if(GeographicLib_FOUND)
message(STATUS "Found GeographicLib ${GeographicLib_VERSION}")
target_link_libraries(Geo PRIVATE PkgConfig::GeographicLib)
return()
endif()
endif()
endif()
endif()

message(STATUS "Building GeographicLib")
include(FetchContent)
FetchContent_Declare(geographiclib
FetchContent_Declare(GeographicLib
GIT_REPOSITORY https://github.com/geographiclib/geographiclib.git
GIT_TAG r2.4
GIT_SHALLOW TRUE
)
set(BUILD_BOTH_LIBS OFF CACHE INTERNAL "" FORCE)
set(BUILD_DOCUMENTATION OFF CACHE INTERNAL "" FORCE)
set(BUILD_MANPAGES OFF CACHE INTERNAL "" FORCE)
FetchContent_MakeAvailable(geographiclib)

set(INCDIR "" CACHE INTERNAL "" FORCE)
set(BINDIR "" CACHE INTERNAL "" FORCE)
set(SBINDIR "" CACHE INTERNAL "" FORCE)
set(LIBDIR "${CMAKE_INSTALL_LIBDIR}" CACHE STRING "" FORCE)
set(DLLDIR "${CMAKE_INSTALL_BINDIR}" CACHE INTERNAL "" FORCE)
set(MANDIR "" CACHE INTERNAL "" FORCE)
set(CMAKEDIR "" CACHE INTERNAL "" FORCE)
set(PKGDIR "" CACHE INTERNAL "" FORCE)
set(DOCDIR "" CACHE INTERNAL "" FORCE)
set(EXAMPLEDIR "" CACHE INTERNAL "" FORCE)
FetchContent_MakeAvailable(GeographicLib)
target_link_libraries(Geo PRIVATE GeographicLib::GeographicLib)
223 changes: 113 additions & 110 deletions src/Geo/QGCGeo.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,137 +27,140 @@ static constexpr double epsilon = std::numeric_limits<double>::epsilon();

namespace QGCGeo {

void convertGeoToNed(const QGeoCoordinate &coord, const QGeoCoordinate &origin, double &x, double &y, double &z) {
if (coord == origin) {
// Prevent NaNs in calculation
x = y = z = 0;
return;
}

const double lat_rad = qDegreesToRadians(coord.latitude());
const double lon_rad = qDegreesToRadians(coord.longitude());

const double ref_lon_rad = qDegreesToRadians(origin.longitude());
const double ref_lat_rad = qDegreesToRadians(origin.latitude());

const double sin_lat = sin(lat_rad);
const double cos_lat = cos(lat_rad);
const double cos_d_lon = cos(lon_rad - ref_lon_rad);

const double ref_sin_lat = sin(ref_lat_rad);
const double ref_cos_lat = cos(ref_lat_rad);

const double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
const double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c));

x = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * GeographicLib::Constants::WGS84_a();
y = k * cos_lat * sin(lon_rad - ref_lon_rad) * GeographicLib::Constants::WGS84_a();
z = -(coord.altitude() - origin.altitude());
void convertGeoToNed(const QGeoCoordinate &coord, const QGeoCoordinate &origin, double &x, double &y, double &z)
{
if (coord == origin) {
// Prevent NaNs in calculation
x = y = z = 0;
return;
}

void convertNedToGeo(double x, double y, double z, const QGeoCoordinate &origin, QGeoCoordinate &coord) {
const double x_rad = x / GeographicLib::Constants::WGS84_a();
const double y_rad = y / GeographicLib::Constants::WGS84_a();
const double c = sqrt(x_rad * x_rad + y_rad * y_rad);
const double sin_c = sin(c);
const double cos_c = cos(c);
const double lat_rad = qDegreesToRadians(coord.latitude());
const double lon_rad = qDegreesToRadians(coord.longitude());

const double ref_lon_rad = qDegreesToRadians(origin.longitude());
const double ref_lat_rad = qDegreesToRadians(origin.latitude());
const double ref_lon_rad = qDegreesToRadians(origin.longitude());
const double ref_lat_rad = qDegreesToRadians(origin.latitude());

const double ref_sin_lat = sin(ref_lat_rad);
const double ref_cos_lat = cos(ref_lat_rad);
const double sin_lat = sin(lat_rad);
const double cos_lat = cos(lat_rad);
const double cos_d_lon = cos(lon_rad - ref_lon_rad);

double lat_rad;
double lon_rad;
const double ref_sin_lat = sin(ref_lat_rad);
const double ref_cos_lat = cos(ref_lat_rad);

if (fabs(c) > epsilon) {
lat_rad = asin(cos_c * ref_sin_lat + (x_rad * sin_c * ref_cos_lat) / c);
lon_rad = (ref_lon_rad + atan2(y_rad * sin_c, c * ref_cos_lat * cos_c - x_rad * ref_sin_lat * sin_c));
} else {
lat_rad = ref_lat_rad;
lon_rad = ref_lon_rad;
}
const double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
const double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c));

coord.setLatitude(qRadiansToDegrees(lat_rad));
coord.setLongitude(qRadiansToDegrees(lon_rad));
coord.setAltitude(-z + origin.altitude());
}
x = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * GeographicLib::Constants::WGS84_a();
y = k * cos_lat * sin(lon_rad - ref_lon_rad) * GeographicLib::Constants::WGS84_a();
z = -(coord.altitude() - origin.altitude());
}

int convertGeoToUTM(const QGeoCoordinate& coord, double &easting, double &northing)
{
try {
int zone;
bool northp;
GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, easting, northing);
return zone;
} catch(const GeographicLib::GeographicErr& e) {
qCDebug(QGCGeoLog) << Q_FUNC_INFO << e.what();
return 0;
}
void convertNedToGeo(double x, double y, double z, const QGeoCoordinate &origin, QGeoCoordinate &coord)
{
const double x_rad = x / GeographicLib::Constants::WGS84_a();
const double y_rad = y / GeographicLib::Constants::WGS84_a();
const double c = sqrt(x_rad * x_rad + y_rad * y_rad);
const double sin_c = sin(c);
const double cos_c = cos(c);

const double ref_lon_rad = qDegreesToRadians(origin.longitude());
const double ref_lat_rad = qDegreesToRadians(origin.latitude());

const double ref_sin_lat = sin(ref_lat_rad);
const double ref_cos_lat = cos(ref_lat_rad);

double lat_rad;
double lon_rad;

if (fabs(c) > epsilon) {
lat_rad = asin(cos_c * ref_sin_lat + (x_rad * sin_c * ref_cos_lat) / c);
lon_rad = (ref_lon_rad + atan2(y_rad * sin_c, c * ref_cos_lat * cos_c - x_rad * ref_sin_lat * sin_c));
} else {
lat_rad = ref_lat_rad;
lon_rad = ref_lon_rad;
}

bool convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate &coord)
{
double lat, lon;
coord.setLatitude(qRadiansToDegrees(lat_rad));
coord.setLongitude(qRadiansToDegrees(lon_rad));
coord.setAltitude(-z + origin.altitude());
}

try {
GeographicLib::UTMUPS::Reverse(zone, !southhemi, easting, northing, lat, lon);
} catch(const GeographicLib::GeographicErr& e) {
qCDebug(QGCGeoLog) << Q_FUNC_INFO << e.what();
return false;
}
int convertGeoToUTM(const QGeoCoordinate& coord, double &easting, double &northing)
{
try {
int zone;
bool northp;
GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, easting, northing);
return zone;
} catch(const GeographicLib::GeographicErr& e) {
qCDebug(QGCGeoLog) << Q_FUNC_INFO << e.what();
return 0;
}
}

coord.setLatitude(lat);
coord.setLongitude(lon);
bool convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate &coord)
{
double lat, lon;

return true;
try {
GeographicLib::UTMUPS::Reverse(zone, !southhemi, easting, northing, lat, lon);
} catch(const GeographicLib::GeographicErr& e) {
qCDebug(QGCGeoLog) << Q_FUNC_INFO << e.what();
return false;
}

QString convertGeoToMGRS(const QGeoCoordinate &coord)
{
std::string mgrs;

try {
int zone;
bool northp;
double x, y;
GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, x, y);
GeographicLib::MGRS::Forward(zone, northp, x, y, coord.latitude(), 5, mgrs);
} catch(const GeographicLib::GeographicErr& e) {
qCDebug(QGCGeoLog) << Q_FUNC_INFO << e.what();
mgrs = "";
}
coord.setLatitude(lat);
coord.setLongitude(lon);

const QString qstr = QString::fromStdString(mgrs);
for (int i = qstr.length() - 1; i >= 0; i--) {
if (!qstr.at(i).isDigit()) {
const int l = (qstr.length() - i) / 2;
return qstr.left(i + 1) + " " + qstr.mid(i + 1, l) + " " + qstr.mid(i + 1 + l);
}
}
return true;
}

return qstr;
QString convertGeoToMGRS(const QGeoCoordinate &coord)
{
std::string mgrs;

try {
int zone;
bool northp;
double x, y;
GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, x, y);
GeographicLib::MGRS::Forward(zone, northp, x, y, coord.latitude(), 5, mgrs);
} catch(const GeographicLib::GeographicErr& e) {
qCDebug(QGCGeoLog) << Q_FUNC_INFO << e.what();
mgrs = "";
}

bool convertMGRSToGeo(const QString &mgrs, QGeoCoordinate &coord)
{
double lat, lon;

try {
int zone, prec;
bool northp;
double x, y;
GeographicLib::MGRS::Reverse(mgrs.simplified().replace(" ", "").toStdString(), zone, northp, x, y, prec);
GeographicLib::UTMUPS::Reverse(zone, northp, x, y, lat, lon);
} catch(const GeographicLib::GeographicErr& e) {
qCDebug(QGCGeoLog) << Q_FUNC_INFO << e.what();
return false;
const QString qstr = QString::fromStdString(mgrs);
for (int i = qstr.length() - 1; i >= 0; i--) {
if (!qstr.at(i).isDigit()) {
const int l = (qstr.length() - i) / 2;
return qstr.left(i + 1) + " " + qstr.mid(i + 1, l) + " " + qstr.mid(i + 1 + l);
}
}

coord.setLatitude(lat);
coord.setLongitude(lon);
return qstr;
}

return true;
bool convertMGRSToGeo(const QString &mgrs, QGeoCoordinate &coord)
{
double lat, lon;

try {
int zone, prec;
bool northp;
double x, y;
GeographicLib::MGRS::Reverse(mgrs.simplified().replace(" ", "").toStdString(), zone, northp, x, y, prec);
GeographicLib::UTMUPS::Reverse(zone, northp, x, y, lat, lon);
} catch(const GeographicLib::GeographicErr& e) {
qCDebug(QGCGeoLog) << Q_FUNC_INFO << e.what();
return false;
}

coord.setLatitude(lat);
coord.setLongitude(lon);

return true;
}

} // namespace QGCGeo
Loading

0 comments on commit 3139615

Please sign in to comment.