QGCGeo.cc 4.46 KB
Newer Older
1 2 3 4 5 6 7 8
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
9

10
#include <QDebug>
11
#include <QString>
12 13

#include <cmath>
David Goodman's avatar
David Goodman committed
14
#include <limits>
15 16

#include "QGCGeo.h"
17
#include "UTMUPS.hpp"
18
#include "MGRS.hpp"
19 20 21 22

// These defines are private
#define M_DEG_TO_RAD (M_PI / 180.0)
#define M_RAD_TO_DEG (180.0 / M_PI)
23
#define CONSTANTS_RADIUS_OF_EARTH 6371000 // meters (m)
24

25
static const double epsilon = std::numeric_limits<double>::epsilon();
26

Don Gagne's avatar
Don Gagne committed
27 28 29 30 31 32 33
void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, double* y, double* z)
{
    if (coord == origin) {
        // Short circuit to prevent NaNs in calculation
        *x = *y = *z = 0;
        return;
    }
34 35 36 37 38 39 40 41 42 43 44 45 46 47 48

    double lat_rad = coord.latitude() * M_DEG_TO_RAD;
    double lon_rad = coord.longitude() * M_DEG_TO_RAD;

    double ref_lon_rad = origin.longitude() * M_DEG_TO_RAD;
    double ref_lat_rad = origin.latitude() * M_DEG_TO_RAD;

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

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

    double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
49
    double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c));
50 51 52 53

    *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;

54
    *z = -(coord.altitude() - origin.altitude());
55 56
}

57
void convertNedToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCoordinate *coord) {
58 59
    double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
    double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
60
    double c = sqrt(x_rad * x_rad + y_rad * y_rad);
61 62 63 64 65 66 67 68 69 70 71 72
    double sin_c = sin(c);
    double cos_c = cos(c);

    double ref_lon_rad = origin.longitude() * M_DEG_TO_RAD;
    double ref_lat_rad = origin.latitude() * M_DEG_TO_RAD;

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

    double lat_rad;
    double lon_rad;

73
    if (fabs(c) > epsilon) {
74 75 76 77 78 79 80 81 82 83 84
        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;
    }

    coord->setLatitude(lat_rad * M_RAD_TO_DEG);
    coord->setLongitude(lon_rad * M_RAD_TO_DEG);

85
    coord->setAltitude(-z + origin.altitude());
86 87
}

88
int convertGeoToUTM(const QGeoCoordinate& coord, double& easting, double& northing)
89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114
{
    try {
        int zone;
        bool northp;
        GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, easting, northing);
        return zone;
    } catch(...) {
        return 0;
    }
}

bool convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate& coord)
{
    double lat, lon;
    try {
        GeographicLib::UTMUPS::Reverse(zone, !southhemi, easting, northing, lat, lon);
    } catch(...) {
        return false;
    }
    coord.setLatitude(lat);
    coord.setLongitude(lon);

    return true;
}

QString convertGeoToMGRS(const QGeoCoordinate& coord)
115
{
116 117
    int zone;
    bool northp;
118 119 120 121 122 123 124 125 126 127
    double x, y;
    std::string mgrs;

    try {
        GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, x, y);
        GeographicLib::MGRS::Forward(zone, northp, x, y, coord.latitude(), 5, mgrs);
    } catch(...) {
        mgrs = "";
    }

128 129 130 131 132 133 134 135 136
    QString qstr = QString::fromStdString(mgrs);
    for (int i = qstr.length() - 1; i >= 0; i--) {
        if (!qstr.at(i).isDigit()) {
            int l = (qstr.length() - i) / 2;
            return qstr.left(i + 1) + " " + qstr.mid(i + 1, l) + " " + qstr.mid(i + 1 + l);
        }
    }

    return qstr;
137 138
}

139
bool convertMGRSToGeo(QString mgrs, QGeoCoordinate& coord)
140
{
141 142 143
    int zone, prec;
    bool northp;
    double x, y;
144
    double lat, lon;
145 146 147 148 149 150 151

    try {
        GeographicLib::MGRS::Reverse(mgrs.simplified().replace(" ", "").toStdString(), zone, northp, x, y, prec);
        GeographicLib::UTMUPS::Reverse(zone, northp, x, y, lat, lon);
    } catch(...) {
        return false;
    }
152 153
    coord.setLatitude(lat);
    coord.setLongitude(lon);
154 155

    return true;
156
}