TerrainTile.cc 13.6 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 * (c) 2009-2020 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.
 *
 ****************************************************************************/

Andreas Bircher's avatar
Andreas Bircher committed
10 11
#include "TerrainTile.h"
#include "JsonHelper.h"
12
#include "QGCMapEngine.h"
13
#include "QGC.h"
Andreas Bircher's avatar
Andreas Bircher committed
14 15 16 17

#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
18
#include <QDataStream>
Andreas Bircher's avatar
Andreas Bircher committed
19

20
QGC_LOGGING_CATEGORY(TerrainTileLog, "TerrainTileLog");
Andreas Bircher's avatar
Andreas Bircher committed
21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36

const char*  TerrainTile::_jsonStatusKey        = "status";
const char*  TerrainTile::_jsonDataKey          = "data";
const char*  TerrainTile::_jsonBoundsKey        = "bounds";
const char*  TerrainTile::_jsonSouthWestKey     = "sw";
const char*  TerrainTile::_jsonNorthEastKey     = "ne";
const char*  TerrainTile::_jsonStatsKey         = "stats";
const char*  TerrainTile::_jsonMaxElevationKey  = "max";
const char*  TerrainTile::_jsonMinElevationKey  = "min";
const char*  TerrainTile::_jsonAvgElevationKey  = "avg";
const char*  TerrainTile::_jsonCarpetKey        = "carpet";

TerrainTile::TerrainTile()
    : _minElevation(-1.0)
    , _maxElevation(-1.0)
    , _avgElevation(-1.0)
37
    , _data(nullptr)
38 39
    , _gridSizeLat(-1)
    , _gridSizeLon(-1)
Andreas Bircher's avatar
Andreas Bircher committed
40 41 42 43 44 45 46
    , _isValid(false)
{

}

TerrainTile::~TerrainTile()
{
47 48 49 50
    if (_data) {
        for (int i = 0; i < _gridSizeLat; i++) {
            delete _data[i];
        }
Andreas Bircher's avatar
Andreas Bircher committed
51
        delete _data;
52
        _data = nullptr;
53
    }
Andreas Bircher's avatar
Andreas Bircher committed
54 55
}

56
TerrainTile::TerrainTile(QByteArray byteArray)
Andreas Bircher's avatar
Andreas Bircher committed
57 58 59
    : _minElevation(-1.0)
    , _maxElevation(-1.0)
    , _avgElevation(-1.0)
60
    , _data(nullptr)
61 62
    , _gridSizeLat(-1)
    , _gridSizeLon(-1)
Andreas Bircher's avatar
Andreas Bircher committed
63 64
    , _isValid(false)
{
65 66
    int cTileHeaderBytes = static_cast<int>(sizeof(TileInfo_t));
    int cTileBytesAvailable = byteArray.size();
67

68 69
    if (cTileBytesAvailable < cTileHeaderBytes) {
        qWarning() << "Terrain tile binary data too small for TileInfo_s header";
70 71 72
        return;
    }

73 74 75 76 77 78 79 80 81 82
    const TileInfo_t* tileInfo = reinterpret_cast<const TileInfo_t*>(byteArray.constData());
    _southWest.setLatitude(tileInfo->swLat);
    _southWest.setLongitude(tileInfo->swLon);
    _northEast.setLatitude(tileInfo->neLat);
    _northEast.setLongitude(tileInfo->neLon);
    _minElevation = tileInfo->minElevation;
    _maxElevation = tileInfo->maxElevation;
    _avgElevation = tileInfo->avgElevation;
    _gridSizeLat = tileInfo->gridSizeLat;
    _gridSizeLon = tileInfo->gridSizeLon;
83

84 85 86 87 88 89
    qCDebug(TerrainTileLog) << "Loading terrain tile: " << _southWest << " - " << _northEast;
    qCDebug(TerrainTileLog) << "min:max:avg:sizeLat:sizeLon" << _minElevation << _maxElevation << _avgElevation << _gridSizeLat << _gridSizeLon;

    int cTileDataBytes = static_cast<int>(sizeof(int16_t)) * _gridSizeLat * _gridSizeLon;
    if (cTileBytesAvailable < cTileHeaderBytes + cTileDataBytes) {
        qWarning() << "Terrain tile binary data too small for tile data";
90 91
        return;
    }
92

93 94 95 96
    _data = new int16_t*[_gridSizeLat];
    for (int k = 0; k < _gridSizeLat; k++) {
        _data[k] = new int16_t[_gridSizeLon];
    }
97

98 99
    int valueIndex = 0;
    const int16_t* pTileData = reinterpret_cast<const int16_t*>(&reinterpret_cast<const uint8_t*>(byteArray.constData())[cTileHeaderBytes]);
100 101
    for (int i = 0; i < _gridSizeLat; i++) {
        for (int j = 0; j < _gridSizeLon; j++) {
102
            _data[i][j] = pTileData[valueIndex++];
103 104 105 106
        }
    }

    _isValid = true;
107 108

    return;
109 110
}

111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127
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;
}
128 129 130 131

bool TerrainTile::isIn(const QGeoCoordinate& coordinate) const
{
    if (!_isValid) {
132
        qCWarning(TerrainTileLog) << "isIn: Internal Error - invalid tile";
133 134
        return false;
    }
135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150

    // 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;
151 152
}

153
double TerrainTile::elevation(const QGeoCoordinate& coordinate) const
154 155 156 157 158 159
{
    if (_isValid) {
        qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast;
        // Get the index at resolution of 1 arc second
        int indexLat = _latToDataIndex(coordinate.latitude());
        int indexLon = _lonToDataIndex(coordinate.longitude());
160
        if (indexLat == -1 || indexLon == -1) {
161
            qCWarning(TerrainTileLog) << "elevation: Internal error - indexLat:indexLon == -1" << indexLat << indexLon;
162
            return qQNaN();
163
        }
164
        qCDebug(TerrainTileLog) << "elevation: indexLat:indexLon" << indexLat << indexLon << "elevation" << _data[indexLat][indexLon];
165
        return static_cast<double>(_data[indexLat][indexLon]);
166
    } else {
167
        qCWarning(TerrainTileLog) << "elevation: Internal error - invalid tile";
168
        return qQNaN();
169 170 171 172 173 174 175 176
    }
}

QGeoCoordinate TerrainTile::centerCoordinate(void) const
{
    return _southWest.atDistanceAndAzimuth(_southWest.distanceTo(_northEast) / 2.0, _southWest.azimuthTo(_northEast));
}

177
QByteArray TerrainTile::serialize(QByteArray input)
178
{
179 180 181 182 183 184 185
    QJsonParseError parseError;
    QJsonDocument document = QJsonDocument::fromJson(input, &parseError);
    if (parseError.error != QJsonParseError::NoError) {
        QByteArray emptyArray;
        return emptyArray;
    }

Andreas Bircher's avatar
Andreas Bircher committed
186
    if (!document.isObject()) {
Andreas Bircher's avatar
Andreas Bircher committed
187
        qCDebug(TerrainTileLog) << "Terrain tile json doc is no object";
188 189
        QByteArray emptyArray;
        return emptyArray;
Andreas Bircher's avatar
Andreas Bircher committed
190
    }
Andreas Bircher's avatar
Andreas Bircher committed
191
    QJsonObject rootObject = document.object();
Andreas Bircher's avatar
Andreas Bircher committed
192 193 194 195

    QString errorString;
    QList<JsonHelper::KeyValidateInfo> rootVersionKeyInfoList = {
        { _jsonStatusKey, QJsonValue::String, true },
196
        { _jsonDataKey,   QJsonValue::Object, true },
Andreas Bircher's avatar
Andreas Bircher committed
197 198 199
    };
    if (!JsonHelper::validateKeys(rootObject, rootVersionKeyInfoList, errorString)) {
        qCDebug(TerrainTileLog) << "Error in reading json: " << errorString;
200 201
        QByteArray emptyArray;
        return emptyArray;
Andreas Bircher's avatar
Andreas Bircher committed
202 203 204 205
    }

    if (rootObject[_jsonStatusKey].toString() != "success") {
        qCDebug(TerrainTileLog) << "Invalid terrain tile.";
206 207
        QByteArray emptyArray;
        return emptyArray;
Andreas Bircher's avatar
Andreas Bircher committed
208 209 210 211
    }
    const QJsonObject& dataObject = rootObject[_jsonDataKey].toObject();
    QList<JsonHelper::KeyValidateInfo> dataVersionKeyInfoList = {
        { _jsonBoundsKey, QJsonValue::Object, true },
212
        { _jsonStatsKey,  QJsonValue::Object, true },
Andreas Bircher's avatar
Andreas Bircher committed
213 214 215 216
        { _jsonCarpetKey, QJsonValue::Array, true },
    };
    if (!JsonHelper::validateKeys(dataObject, dataVersionKeyInfoList, errorString)) {
        qCDebug(TerrainTileLog) << "Error in reading json: " << errorString;
217 218
        QByteArray emptyArray;
        return emptyArray;
Andreas Bircher's avatar
Andreas Bircher committed
219 220 221 222 223 224 225 226 227 228
    }

    // Bounds
    const QJsonObject& boundsObject = dataObject[_jsonBoundsKey].toObject();
    QList<JsonHelper::KeyValidateInfo> boundsVersionKeyInfoList = {
        { _jsonSouthWestKey, QJsonValue::Array, true },
        { _jsonNorthEastKey, QJsonValue::Array, true },
    };
    if (!JsonHelper::validateKeys(boundsObject, boundsVersionKeyInfoList, errorString)) {
        qCDebug(TerrainTileLog) << "Error in reading json: " << errorString;
229 230
        QByteArray emptyArray;
        return emptyArray;
Andreas Bircher's avatar
Andreas Bircher committed
231 232 233 234 235
    }
    const QJsonArray& swArray = boundsObject[_jsonSouthWestKey].toArray();
    const QJsonArray& neArray = boundsObject[_jsonNorthEastKey].toArray();
    if (swArray.count() < 2 || neArray.count() < 2 ) {
        qCDebug(TerrainTileLog) << "Incomplete bounding location";
236 237
        QByteArray emptyArray;
        return emptyArray;
Andreas Bircher's avatar
Andreas Bircher committed
238 239 240
    }

    // Stats
241
    const QJsonObject& statsObject = dataObject[_jsonStatsKey].toObject();
Andreas Bircher's avatar
Andreas Bircher committed
242 243
    QList<JsonHelper::KeyValidateInfo> statsVersionKeyInfoList = {
        { _jsonMinElevationKey, QJsonValue::Double, true },
244
        { _jsonMaxElevationKey, QJsonValue::Double, true },
Andreas Bircher's avatar
Andreas Bircher committed
245 246 247 248
        { _jsonAvgElevationKey, QJsonValue::Double, true },
    };
    if (!JsonHelper::validateKeys(statsObject, statsVersionKeyInfoList, errorString)) {
        qCDebug(TerrainTileLog) << "Error in reading json: " << errorString;
249 250
        QByteArray emptyArray;
        return emptyArray;
Andreas Bircher's avatar
Andreas Bircher committed
251 252 253 254
    }

    // Carpet
    const QJsonArray& carpetArray = dataObject[_jsonCarpetKey].toArray();
255
    int gridSizeLat = carpetArray.count();
256 257 258 259 260 261 262 263 264 265 266 267 268 269
    int gridSizeLon = carpetArray[0].toArray().count();

    TileInfo_t tileInfo;

    tileInfo.swLat = swArray[0].toDouble();
    tileInfo.swLon = swArray[1].toDouble();
    tileInfo.neLat = neArray[0].toDouble();
    tileInfo.neLon = neArray[1].toDouble();
    tileInfo.minElevation = static_cast<int16_t>(statsObject[_jsonMinElevationKey].toInt());
    tileInfo.maxElevation = static_cast<int16_t>(statsObject[_jsonMaxElevationKey].toInt());
    tileInfo.avgElevation = statsObject[_jsonAvgElevationKey].toDouble();
    tileInfo.gridSizeLat = static_cast<int16_t>(gridSizeLat);
    tileInfo.gridSizeLon = static_cast<int16_t>(gridSizeLon);

270 271 272 273 274 275 276 277 278 279
    // 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;
    }

280 281 282 283 284 285 286 287 288 289 290
    int cTileHeaderBytes = static_cast<int>(sizeof(TileInfo_t));
    int cTileDataBytes = static_cast<int>(sizeof(int16_t)) * gridSizeLat * gridSizeLon;

    QByteArray byteArray(cTileHeaderBytes + cTileDataBytes, 0);

    TileInfo_t* pTileInfo = reinterpret_cast<TileInfo_t*>(byteArray.data());
    int16_t*    pTileData = reinterpret_cast<int16_t*>(&reinterpret_cast<uint8_t*>(byteArray.data())[cTileHeaderBytes]);

    *pTileInfo = tileInfo;

    int valueIndex = 0;
291
    for (int i = 0; i < gridSizeLat; i++) {
Andreas Bircher's avatar
Andreas Bircher committed
292
        const QJsonArray& row = carpetArray[i].toArray();
293 294 295 296
        if (row.count() < gridSizeLon) {
            qCDebug(TerrainTileLog) << "Expected row array of " << gridSizeLon << ", instead got " << row.count();
            QByteArray emptyArray;
            return emptyArray;
Andreas Bircher's avatar
Andreas Bircher committed
297
        }
298
        for (int j = 0; j < gridSizeLon; j++) {
299
            pTileData[valueIndex++] = static_cast<int16_t>(row[j].toDouble());
Andreas Bircher's avatar
Andreas Bircher committed
300 301 302
        }
    }

303
    return byteArray;
Andreas Bircher's avatar
Andreas Bircher committed
304 305
}

306 307 308

int TerrainTile::_latToDataIndex(double latitude) const
{
309 310 311 312 313 314
    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

315
    if (isValid() && _southWest.isValid() && _northEast.isValid()) {
316 317 318
        double clampedLatitude = _swCornerClampedLatitude(latitude);
        latIndex = qRound((clampedLatitude - _southWest.latitude()) / tileValueSpacingDegrees);
        qCDebug(TerrainTileLog) << "_latToDataIndex: latIndex:latitude:clampedLatitude:_southWest" << latIndex << latitude << clampedLatitude << _southWest;
319
    } else {
320
        qCWarning(TerrainTileLog) << "_latToDataIndex: Internal error - isValid:_southWest.isValid:_northEast.isValid" << isValid() << _southWest.isValid() << _northEast.isValid();
321
    }
322 323

    return latIndex;
324 325 326 327
}

int TerrainTile::_lonToDataIndex(double longitude) const
{
328 329 330 331 332 333
    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

334
    if (isValid() && _southWest.isValid() && _northEast.isValid()) {
335 336 337
        double clampledLongitude = _swCornerClampedLongitude(longitude);
        lonIndex = qRound((clampledLongitude - _southWest.longitude()) / tileValueSpacingDegrees);
        qCDebug(TerrainTileLog) << "_lonToDataIndex: lonIndex:longitude:clampledLongitude:_southWest" << lonIndex << longitude << clampledLongitude << _southWest;
338
    } else {
339
        qCWarning(TerrainTileLog) << "_lonToDataIndex: Internal error - isValid:_southWest.isValid:_northEast.isValid" << isValid() << _southWest.isValid() << _northEast.isValid();
340
    }
341 342

    return lonIndex;
343
}