Merge pull request #8959 from DonLakeFlyer/TerrainRounding

Terrain data: Deal with double imprecision errors in elevation tile calculations

src/QtLocationPlugin/ElevationMapProvider.cpp

@@ -4,6 +4,7 @@
 #include <QStandardPaths>
 #endif
 #include "QGCMapEngine.h"
+#include "TerrainTile.h"
 
 ElevationProvider::ElevationProvider(const QString& imageFormat, quint32 averageSize, QGeoMapType::MapStyle mapType, QObject* parent)
     : MapProvider(QStringLiteral("https://api.airmap.com/"), imageFormat, averageSize, mapType, parent) {}
@@ -11,23 +12,23 @@ ElevationProvider::ElevationProvider(const QString& imageFormat, quint32 average
 //-----------------------------------------------------------------------------
 int AirmapElevationProvider::long2tileX(const double lon, const int z) const {
     Q_UNUSED(z)
-    return static_cast<int>(floor((lon + 180.0) / srtm1TileSize));
+    return static_cast<int>(floor((lon + 180.0) / TerrainTile::tileSizeDegrees));
 }
 
 //-----------------------------------------------------------------------------
 int AirmapElevationProvider::lat2tileY(const double lat, const int z) const {
     Q_UNUSED(z)
-    return static_cast<int>(floor((lat + 90.0) / srtm1TileSize));
+    return static_cast<int>(floor((lat + 90.0) / TerrainTile::tileSizeDegrees));
 }
 
 QString AirmapElevationProvider::_getURL(const int x, const int y, const int zoom, QNetworkAccessManager* networkManager) {
     Q_UNUSED(networkManager)
     Q_UNUSED(zoom)
     return QString("https://api.airmap.com/elevation/v1/ele/carpet?points=%1,%2,%3,%4")
-        .arg(static_cast<double>(y) * srtm1TileSize - 90.0)
-        .arg(static_cast<double>(x) * srtm1TileSize - 180.0)
-        .arg(static_cast<double>(y + 1) * srtm1TileSize - 90.0)
-        .arg(static_cast<double>(x + 1) * srtm1TileSize - 180.0);
+        .arg(static_cast<double>(y) * TerrainTile::tileSizeDegrees - 90.0)
+        .arg(static_cast<double>(x) * TerrainTile::tileSizeDegrees - 180.0)
+        .arg(static_cast<double>(y + 1) * TerrainTile::tileSizeDegrees - 90.0)
+        .arg(static_cast<double>(x + 1) * TerrainTile::tileSizeDegrees - 180.0);
 }
 
 QGCTileSet AirmapElevationProvider::getTileCount(const int zoom, const double topleftLon,

src/QtLocationPlugin/ElevationMapProvider.h

@@ -11,8 +11,6 @@
 #include <QString>
 
 static const quint32 AVERAGE_AIRMAP_ELEV_SIZE = 2786;
-//-----------------------------------------------------------------------------
-static const double srtm1TileSize = 0.01;
 
 class ElevationProvider : public MapProvider {
     Q_OBJECT

src/Terrain/TerrainQuery.cc

@@ -370,7 +370,7 @@ void TerrainTileManager::addPathQuery(TerrainOfflineAirMapQuery* terrainQueryInt
     QList<QGeoCoordinate> coordinates;
     double lat = startPoint.latitude();
     double lon = startPoint.longitude();
-    double steps = ceil(endPoint.distanceTo(startPoint) / TerrainTile::terrainAltitudeSpacing);
+    double steps = ceil(endPoint.distanceTo(startPoint) / TerrainTile::tileValueSpacingMeters);
     double latDiff = endPoint.latitude() - lat;
     double lonDiff = endPoint.longitude() - lon;
 

src/TerrainTile.cc

@@ -10,6 +10,7 @@
 #include "TerrainTile.h"
 #include "JsonHelper.h"
 #include "QGCMapEngine.h"
+#include "QGC.h"
 
 #include <QJsonDocument>
 #include <QJsonObject>
@@ -107,17 +108,46 @@ TerrainTile::TerrainTile(QByteArray byteArray)
     return;
 }
 
+double TerrainTile::_swCornerClampedLatitude(double latitude) const
+{
+    double swCornerLat = _southWest.latitude();
+    if (QGC::fuzzyCompare(latitude, swCornerLat)) {
+        latitude = swCornerLat;
+    }
+    return latitude;
+}
+
+double TerrainTile::_swCornerClampedLongitude (double longitude) const
+{
+    double swCornerLon = _southWest.longitude();
+    if (QGC::fuzzyCompare(longitude, swCornerLon)) {
+        longitude = swCornerLon;
+    }
+    return longitude;
+}
 
 bool TerrainTile::isIn(const QGeoCoordinate& coordinate) const
 {
     if (!_isValid) {
-        qCWarning(TerrainTileLog) << "isIn requested, but tile not valid";
+        qCWarning(TerrainTileLog) << "isIn: Internal Error - invalid tile";
         return false;
     }
-    bool ret = coordinate.latitude() >= _southWest.latitude() && coordinate.longitude() >= _southWest.longitude() &&
-            coordinate.latitude() <= _northEast.latitude() && coordinate.longitude() <= _northEast.longitude();
-    qCDebug(TerrainTileLog) << "Checking isIn: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast << ": " << ret;
-    return ret;
+
+    // We have to be careful of double value imprecision for lat/lon values.
+    // Don't trust _northEast corner values because of this (they are set from airmap query response).
+    // Calculate everything from swCorner values only
+
+    double testLat      = _swCornerClampedLatitude(coordinate.latitude());
+    double testLon      = _swCornerClampedLongitude(coordinate.longitude());
+    double swCornerLat  = _southWest.latitude();
+    double swCornerLon  = _southWest.longitude();
+    double neCornerLat  = swCornerLat + (_gridSizeLat * tileSizeDegrees);
+    double neCornerLon  = swCornerLon + (_gridSizeLon * tileSizeDegrees);
+
+    bool coordinateIsInTile = testLat >= swCornerLat && testLon >= swCornerLon && testLat <= neCornerLat && testLon <= neCornerLon;
+    qCDebug(TerrainTileLog) << "isIn - coordinateIsInTile::coordinate:testLast:testLon:swCornerlat:swCornerLon:neCornerLat:neCornerLon" << coordinateIsInTile << coordinate << testLat << testLon << swCornerLat << swCornerLon << neCornerLat << neCornerLon;
+
+    return coordinateIsInTile;
 }
 
 double TerrainTile::elevation(const QGeoCoordinate& coordinate) const
@@ -128,13 +158,13 @@ double TerrainTile::elevation(const QGeoCoordinate& coordinate) const
         int indexLat = _latToDataIndex(coordinate.latitude());
         int indexLon = _lonToDataIndex(coordinate.longitude());
         if (indexLat == -1 || indexLon == -1) {
-            qCWarning(TerrainTileLog) << "Internal error indexLat:indexLon == -1" << indexLat << indexLon;
+            qCWarning(TerrainTileLog) << "elevation: Internal error - indexLat:indexLon == -1" << indexLat << indexLon;
             return qQNaN();
         }
-        qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon << "elevation" << _data[indexLat][indexLon];
+        qCDebug(TerrainTileLog) << "elevation: indexLat:indexLon" << indexLat << indexLon << "elevation" << _data[indexLat][indexLon];
         return static_cast<double>(_data[indexLat][indexLon]);
     } else {
-        qCWarning(TerrainTileLog) << "Asking for elevation, but no valid data.";
+        qCWarning(TerrainTileLog) << "elevation: Internal error - invalid tile";
         return qQNaN();
     }
 }
@@ -224,8 +254,6 @@ QByteArray TerrainTile::serialize(QByteArray input)
     const QJsonArray& carpetArray = dataObject[_jsonCarpetKey].toArray();
     int gridSizeLat = carpetArray.count();
     int gridSizeLon = carpetArray[0].toArray().count();
-    qCDebug(TerrainTileLog) << "Received tile has size in latitude direction: " << gridSizeLat;
-    qCDebug(TerrainTileLog) << "Received tile has size in longitued direction: " << gridSizeLon;
 
     TileInfo_t tileInfo;
 
@@ -239,6 +267,16 @@ QByteArray TerrainTile::serialize(QByteArray input)
     tileInfo.gridSizeLat = static_cast<int16_t>(gridSizeLat);
     tileInfo.gridSizeLon = static_cast<int16_t>(gridSizeLon);
 
+    // We require 1-arc second value spacing
+    double neCornerLatExpected = tileInfo.swLat + ((tileInfo.gridSizeLat - 1) * tileValueSpacingDegrees);
+    double neCornerLonExpected = tileInfo.swLon + ((tileInfo.gridSizeLon - 1) * tileValueSpacingDegrees);
+    if (!QGC::fuzzyCompare(tileInfo.neLat, neCornerLatExpected) || !QGC::fuzzyCompare(tileInfo.neLon, neCornerLonExpected)) {
+        qCWarning(TerrainTileLog) << QStringLiteral("serialize: Internal error - distance between values incorrect neExpected(%1, %2) neActual(%3, %4) sw(%5, %6) gridSize(%7, %8)")
+                                     .arg(neCornerLatExpected).arg(neCornerLonExpected).arg(tileInfo.neLat).arg(tileInfo.neLon).arg(tileInfo.swLat).arg(tileInfo.swLon).arg(tileInfo.gridSizeLat).arg(tileInfo.gridSizeLon);
+        QByteArray emptyArray;
+        return emptyArray;
+    }
+
     int cTileHeaderBytes = static_cast<int>(sizeof(TileInfo_t));
     int cTileDataBytes = static_cast<int>(sizeof(int16_t)) * gridSizeLat * gridSizeLon;
 
@@ -268,20 +306,38 @@ QByteArray TerrainTile::serialize(QByteArray input)
 
 int TerrainTile::_latToDataIndex(double latitude) const
 {
+    int latIndex = -1;
+
+    // We have to be careful of double value imprecision for lat/lon values.
+    // Don't trust _northEast corner values because of this (they are set from airmap query response).
+    // Calculate everything from swCorner values only
+
     if (isValid() && _southWest.isValid() && _northEast.isValid()) {
-        return qRound((latitude - _southWest.latitude()) / (_northEast.latitude() - _southWest.latitude()) * (_gridSizeLat - 1));
+        double clampedLatitude = _swCornerClampedLatitude(latitude);
+        latIndex = qRound((clampedLatitude - _southWest.latitude()) / tileValueSpacingDegrees);
+        qCDebug(TerrainTileLog) << "_latToDataIndex: latIndex:latitude:clampedLatitude:_southWest" << latIndex << latitude << clampedLatitude << _southWest;
     } else {
-        qCWarning(TerrainTileLog) << "TerrainTile::_latToDataIndex internal error" << isValid() << _southWest.isValid() << _northEast.isValid();
-        return -1;
+        qCWarning(TerrainTileLog) << "_latToDataIndex: Internal error - isValid:_southWest.isValid:_northEast.isValid" << isValid() << _southWest.isValid() << _northEast.isValid();
     }
+
+    return latIndex;
 }
 
 int TerrainTile::_lonToDataIndex(double longitude) const
 {
+    int lonIndex = -1;
+
+    // We have to be careful of double value imprecision for lat/lon values.
+    // Don't trust _northEast corner values because of this (they are set from airmap query response).
+    // Calculate everything from swCorner values only
+
     if (isValid() && _southWest.isValid() && _northEast.isValid()) {
-        return qRound((longitude - _southWest.longitude()) / (_northEast.longitude() - _southWest.longitude()) * (_gridSizeLon - 1));
+        double clampledLongitude = _swCornerClampedLongitude(longitude);
+        lonIndex = qRound((clampledLongitude - _southWest.longitude()) / tileValueSpacingDegrees);
+        qCDebug(TerrainTileLog) << "_lonToDataIndex: lonIndex:longitude:clampledLongitude:_southWest" << lonIndex << longitude << clampledLongitude << _southWest;
     } else {
-        qCWarning(TerrainTileLog) << "TerrainTile::_lonToDataIndex internal error" << isValid() << _southWest.isValid() << _northEast.isValid();
-        return -1;
+        qCWarning(TerrainTileLog) << "_lonToDataIndex: Internal error - isValid:_southWest.isValid:_northEast.isValid" << isValid() << _southWest.isValid() << _northEast.isValid();
     }
+
+    return lonIndex;
 }

src/TerrainTile.h

@@ -91,8 +91,9 @@ public:
     */
     static QByteArray serialize(QByteArray input);
 
-    /// Approximate spacing of the elevation data measurement points
-    static constexpr double terrainAltitudeSpacing = 30.0;
+    static constexpr double tileSizeDegrees         = 0.01;         ///< Each terrain tile represents a square area .01 degrees in lat/lon
+    static constexpr double tileValueSpacingDegrees = 1.0 / 3600;   ///< 1 Arc-Second spacing of elevation values
+    static constexpr double tileValueSpacingMeters  = 30.0;
 
 private:
     typedef struct {
@@ -104,8 +105,10 @@ private:
         int16_t gridSizeLon;
     } TileInfo_t;
 
-    inline int _latToDataIndex(double latitude) const;
-    inline int _lonToDataIndex(double longitude) const;
+    double  _swCornerClampedLatitude    (double latitude) const;
+    double  _swCornerClampedLongitude   (double longitude) const;
+    int     _latToDataIndex             (double latitude) const;
+    int     _lonToDataIndex             (double longitude) const;
 
     QGeoCoordinate      _southWest;                                     /// South west corner of the tile
     QGeoCoordinate      _northEast;                                     /// North east corner of the tile