QGCGeoBoundingCube.cc 4.98 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9 10 11
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "QGCGeoBoundingCube.h"
#include <QDebug>
12
#include <cmath>
13 14 15 16 17 18 19 20

double QGCGeoBoundingCube::MaxAlt    =  1000000.0;
double QGCGeoBoundingCube::MinAlt    = -1000000.0;
double QGCGeoBoundingCube::MaxNorth  =  90.0;
double QGCGeoBoundingCube::MaxSouth  = -90.0;
double QGCGeoBoundingCube::MaxWest   = -180.0;
double QGCGeoBoundingCube::MaxEast   =  180.0;

21 22 23 24 25 26 27
//-----------------------------------------------------------------------------
QGCGeoBoundingCube::QGCGeoBoundingCube(QObject* parent)
    : QObject(parent)
{
    reset();
}

28 29 30 31 32
//-----------------------------------------------------------------------------
bool
QGCGeoBoundingCube::isValid() const
{
    return pointNW.isValid() && pointSE.isValid() && pointNW.latitude() != MaxSouth && pointSE.latitude() != MaxNorth && \
Gus Grubba's avatar
Gus Grubba committed
33
           pointNW.longitude() != MaxEast && pointSE.longitude() != MaxWest && pointNW.altitude() < MaxAlt && pointSE.altitude() > MinAlt;
34 35
}

36 37 38 39
//-----------------------------------------------------------------------------
void
QGCGeoBoundingCube::reset()
{
40 41
    pointNW = QGeoCoordinate(MaxSouth, MaxEast, MaxAlt);
    pointSE = QGeoCoordinate(MaxNorth, MaxWest, MinAlt);
42 43
}

44 45 46 47
//-----------------------------------------------------------------------------
QGeoCoordinate
QGCGeoBoundingCube::center() const
{
48 49
    if(!isValid())
        return QGeoCoordinate();
50 51 52 53 54 55 56 57
    double lat = (((pointNW.latitude()  + 90.0)  + (pointSE.latitude()  + 90.0))  / 2.0) - 90.0;
    double lon = (((pointNW.longitude() + 180.0) + (pointSE.longitude() + 180.0)) / 2.0) - 180.0;
    double alt = (pointNW.altitude() + pointSE.altitude()) / 2.0;
    return QGeoCoordinate(lat, lon, alt);
}

//-----------------------------------------------------------------------------
QList<QGeoCoordinate>
58
QGCGeoBoundingCube::polygon2D(double clipTo) const
59 60
{
    QList<QGeoCoordinate> coords;
61
    if(isValid()) {
62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82
        //-- Should we clip it?
        if(clipTo > 0.0 && area() > clipTo) {
            //-- Clip it to a square of given area centered on current bounding box center.
            double side = sqrt(clipTo);
            QGeoCoordinate c = center();
            double a = pow((side * 0.5), 2);
            double h = sqrt(a + a) * 1000.0;
            QGeoCoordinate nw = c.atDistanceAndAzimuth(h, 315.0);
            QGeoCoordinate se = c.atDistanceAndAzimuth(h, 135.0);
            coords.append(QGeoCoordinate(nw.latitude(), nw.longitude(), se.altitude()));
            coords.append(QGeoCoordinate(nw.latitude(), se.longitude(), se.altitude()));
            coords.append(QGeoCoordinate(se.latitude(), se.longitude(), se.altitude()));
            coords.append(QGeoCoordinate(se.latitude(), nw.longitude(), se.altitude()));
            coords.append(QGeoCoordinate(nw.latitude(), nw.longitude(), se.altitude()));
        } else {
            coords.append(QGeoCoordinate(pointNW.latitude(), pointNW.longitude(), pointSE.altitude()));
            coords.append(QGeoCoordinate(pointNW.latitude(), pointSE.longitude(), pointSE.altitude()));
            coords.append(QGeoCoordinate(pointSE.latitude(), pointSE.longitude(), pointSE.altitude()));
            coords.append(QGeoCoordinate(pointSE.latitude(), pointNW.longitude(), pointSE.altitude()));
            coords.append(QGeoCoordinate(pointNW.latitude(), pointNW.longitude(), pointSE.altitude()));
        }
83
    }
84 85 86 87
    return coords;
}

//-----------------------------------------------------------------------------
88 89 90 91 92 93 94 95 96 97 98 99
bool
QGCGeoBoundingCube::operator ==(const QList<QGeoCoordinate>& coords) const
{
    QList<QGeoCoordinate> c = polygon2D();
    if(c.size() != coords.size()) return false;
    for(int i = 0; i < c.size(); i++) {
        if(c[i] != coords[i]) return false;
    }
    return true;
}

//-----------------------------------------------------------------------------
100 101 102
double
QGCGeoBoundingCube::width() const
{
103 104
    if(!isValid())
        return 0.0;
105 106 107 108 109 110 111 112
    QGeoCoordinate ne = QGeoCoordinate(pointNW.latitude(), pointSE.longitude());
    return pointNW.distanceTo(ne);
}

//-----------------------------------------------------------------------------
double
QGCGeoBoundingCube::height() const
{
113 114
    if(!isValid())
        return 0.0;
115 116 117 118 119 120 121 122
    QGeoCoordinate sw = QGeoCoordinate(pointSE.latitude(), pointNW.longitude());
    return pointNW.distanceTo(sw);
}

//-----------------------------------------------------------------------------
double
QGCGeoBoundingCube::area() const
{
123 124
    if(!isValid())
        return 0.0;
125 126 127 128 129 130 131 132 133
    // Area in km^2
    double a = (height() / 1000.0) * (width() / 1000.0);
    return a;
}

//-----------------------------------------------------------------------------
double
QGCGeoBoundingCube::radius() const
{
134 135
    if(!isValid())
        return 0.0;
136 137
    return pointNW.distanceTo(pointSE) / 2.0;
}