Skip to content

Q
qgroundcontrol
Commit 57962f3a authored by Matej Frančeškin's avatar Matej Frančeškin
Browse files
Options

Use GeographicLib UTM conversions

parent 1ee854e1
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 19 additions and 616 deletions
qgroundcontrol.pro
View file @ 57962f3a
......@@ -629,7 +629,6 @@ HEADERS += \
src/PositionManager/PositionManager.h \
src/PositionManager/SimulatedPosition.h \
src/Geo/QGCGeo.h \
src/Geo/UTM.h \
src/Geo/Constants.hpp \
src/Geo/Math.hpp \
src/Geo/Utility.hpp \
......@@ -866,7 +865,6 @@ SOURCES += \
src/PositionManager/PositionManager.cpp \
src/PositionManager/SimulatedPosition.cc \
src/Geo/QGCGeo.cc \
src/Geo/UTM.cpp \
src/Geo/Math.cpp \
src/Geo/Utility.cpp \
src/Geo/UTMUPS.cpp \
......
src/Geo/QGCGeo.cc
View file @ 57962f3a
......@@ -13,7 +13,7 @@
#include <limits>
#include "QGCGeo.h"
#include "UTM.h"
#include "UTMUPS.hpp"
// These defines are private
#define M_DEG_TO_RAD (M_PI / 180.0)
......@@ -26,7 +26,7 @@
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */
#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */
static const float epsilon = std::numeric_limits<double>::epsilon();
static const double epsilon = std::numeric_limits<double>::epsilon();
void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, double* y, double* z)
{
......@@ -50,7 +50,7 @@ void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, dou
double ref_cos_lat = cos(ref_lat_rad);
double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c));
double k = (abs(c) < epsilon) ? 1.0 : (c / sin(c));
*x = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref_lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
......@@ -61,7 +61,7 @@ void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, dou
void convertNedToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCoordinate *coord) {
double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
double c = sqrt(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
......@@ -74,7 +74,7 @@ void convertNedToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCo
double lat_rad;
double lon_rad;
if (fabs(c) > epsilon) {
if (abs(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));
......@@ -91,14 +91,16 @@ void convertNedToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCo
int convertGeoToUTM(const QGeoCoordinate& coord, double& easting, double& northing)
{
return LatLonToUTMXY(coord.latitude(), coord.longitude(), -1 /* zone */, easting, northing);
int zone;
bool northp;
GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, easting, northing);
return zone;
}
void convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate& coord)
{
double latRadians, lonRadians;
UTMXYToLatLon (easting, northing, zone, southhemi, latRadians, lonRadians);
coord.setLatitude(RadToDeg(latRadians));
coord.setLongitude(RadToDeg(lonRadians));
double lat, lon;
GeographicLib::UTMUPS::Reverse(zone, !southhemi, easting, northing, lat, lon);
coord.setLatitude(lat);
coord.setLongitude(lon);
}
src/Geo/UTM.cpp deleted 100644 → 0
View file @ 1ee854e1
This diff is collapsed.
src/Geo/UTM.h deleted 100644 → 0
View file @ 1ee854e1
// UTM.h
// Original Javascript by Chuck Taylor
// Port to C++ by Alex Hajnal
//
// This is a simple port of the code on the Geographic/UTM Coordinate Converter (1) page from Javascript to C++.
// Using this you can easily convert between UTM and WGS84 (latitude and longitude).
// Accuracy seems to be around 50cm (I suspect rounding errors are limiting precision).
// This code is provided as-is and has been minimally tested; enjoy but use at your own risk!
// The license for UTM.cpp and UTM.h is the same as the original Javascript:
// "The C++ source code in UTM.cpp and UTM.h may be copied and reused without restriction."
//
// 1) http://home.hiwaay.net/~taylorc/toolbox/geography/geoutm.html
// QGC Note: This file has been slightly modified to prevent possible conflicts with other parts of the system
#ifndef UTM_H
#define UTM_H
// DegToRad
// Converts degrees to radians.
double DegToRad(double deg);
// RadToDeg
// Converts radians to degrees.
double RadToDeg(double rad);
// ArcLengthOfMeridian
// Computes the ellipsoidal distance from the equator to a point at a
// given latitude.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994.
//
// Inputs:
// phi - Latitude of the point, in radians.
//
// Globals:
// sm_a - Ellipsoid model major axis.
// sm_b - Ellipsoid model minor axis.
//
// Returns:
// The ellipsoidal distance of the point from the equator, in meters.
double ArcLengthOfMeridian (double phi);
// UTMCentralMeridian
// Determines the central meridian for the given UTM zone.
//
// Inputs:
// zone - An integer value designating the UTM zone, range [1,60].
//
// Returns:
// The central meridian for the given UTM zone, in radians
// Range of the central meridian is the radian equivalent of [-177,+177].
double UTMCentralMeridian(int zone);
// FootpointLatitude
//
// Computes the footpoint latitude for use in converting transverse
// Mercator coordinates to ellipsoidal coordinates.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994.
//
// Inputs:
// y - The UTM northing coordinate, in meters.
//
// Returns:
// The footpoint latitude, in radians.
double FootpointLatitude(double y);
// MapLatLonToXY
// Converts a latitude/longitude pair to x and y coordinates in the
// Transverse Mercator projection. Note that Transverse Mercator is not
// the same as UTM; a scale factor is required to convert between them.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994.
//
// Inputs:
// phi - Latitude of the point, in radians.
// lambda - Longitude of the point, in radians.
// lambda0 - Longitude of the central meridian to be used, in radians.
//
// Outputs:
// x - The x coordinate of the computed point.
// y - The y coordinate of the computed point.
//
// Returns:
// The function does not return a value.
void MapLatLonToXY (double phi, double lambda, double lambda0, double &x, double &y);
// MapXYToLatLon
// Converts x and y coordinates in the Transverse Mercator projection to
// a latitude/longitude pair. Note that Transverse Mercator is not
// the same as UTM; a scale factor is required to convert between them.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
// GPS: Theory and Practice, 3rd ed. New York: Springer-Verlag Wien, 1994.
//
// Inputs:
// x - The easting of the point, in meters.
// y - The northing of the point, in meters.
// lambda0 - Longitude of the central meridian to be used, in radians.
//
// Outputs:
// phi - Latitude in radians.
// lambda - Longitude in radians.
//
// Returns:
// The function does not return a value.
//
// Remarks:
// The local variables Nf, nuf2, tf, and tf2 serve the same purpose as
// N, nu2, t, and t2 in MapLatLonToXY, but they are computed with respect
// to the footpoint latitude phif.
//
// x1frac, x2frac, x2poly, x3poly, etc. are to enhance readability and
// to optimize computations.
void MapXYToLatLon (double x, double y, double lambda0, double& phi, double& lambda);
// LatLonToUTMXY
// Converts a latitude/longitude pair to x and y coordinates in the
// Universal Transverse Mercator projection.
//
// Inputs:
// lat - Latitude of the point, in radians.
// lon - Longitude of the point, in radians.
// zone - UTM zone to be used for calculating values for x and y.
// If zone is less than 1 or greater than 60, the routine
// will determine the appropriate zone from the value of lon.
//
// Outputs:
// x - The x coordinate (easting) of the computed point. (in meters)
// y - The y coordinate (northing) of the computed point. (in meters)
//
// Returns:
// The UTM zone used for calculating the values of x and y.
int LatLonToUTMXY (double lat, double lon, int zone, double& x, double& y);
// UTMXYToLatLon
//
// Converts x and y coordinates in the Universal Transverse Mercator// The UTM zone parameter should be in the range [1,60].
// projection to a latitude/longitude pair.
//
// Inputs:
// x - The easting of the point, in meters.
// y - The northing of the point, in meters.
// zone - The UTM zone in which the point lies.
// southhemi - True if the point is in the southern hemisphere;
// false otherwise.
//
// Outputs:
// lat - The latitude of the point, in radians.
// lon - The longitude of the point, in radians.
//
// Returns:
// The function does not return a value.
void UTMXYToLatLon (double x, double y, int zone, bool southhemi, double& lat, double& lon);
#endif
src/SHPFileHelper.cc
View file @ 57962f3a
......@@ -8,7 +8,7 @@
****************************************************************************/
#include "SHPFileHelper.h"
#include "UTM.h"
#include "QGCGeo.h"
#include <QFile>
#include <QVariant>
......@@ -141,14 +141,14 @@ bool SHPFileHelper::loadPolygonFromFile(const QString& shpFile, QList<QGeoCoordi
}
for (int i=0; i<shpObject->nVertices; i++) {
double lat, lon;
QGeoCoordinate coord;
if (utmZone) {
UTMXYToLatLon(shpObject->padfX[i], shpObject->padfY[i], utmZone, utmSouthernHemisphere, lat, lon);
convertUTMToGeo(shpObject->padfX[i], shpObject->padfY[i], utmZone, utmSouthernHemisphere, coord);
} else {
lat = shpObject->padfY[i];
lon = shpObject->padfX[i];
coord.setLatitude(shpObject->padfY[i]);
coord.setLongitude(shpObject->padfX[i]);
}
vertices.append(QGeoCoordinate(lat, lon));
vertices.append(coord);
}
// Filter last vertex such that it differs from first
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment