diff --git a/src/blackmisc/aviation/aircraftsituation.h b/src/blackmisc/aviation/aircraftsituation.h index 6860eb514..cb112a8c4 100644 --- a/src/blackmisc/aviation/aircraftsituation.h +++ b/src/blackmisc/aviation/aircraftsituation.h @@ -77,10 +77,10 @@ namespace BlackMisc void setPosition(const BlackMisc::Geo::CCoordinateGeodetic &position) { this->m_position = position; } //! \copydoc ICoordinateGeodetic::latitude() - virtual const BlackMisc::Geo::CLatitude &latitude() const override { return this->m_position.latitude(); } + virtual BlackMisc::Geo::CLatitude latitude() const override { return this->m_position.latitude(); } //! \copydoc ICoordinateGeodetic::longitude() - virtual const BlackMisc::Geo::CLongitude &longitude() const override { return this->m_position.longitude(); } + virtual BlackMisc::Geo::CLongitude longitude() const override { return this->m_position.longitude(); } //! Guess if aircraft is "on ground" virtual bool isOnGroundGuessed() const; @@ -89,6 +89,9 @@ namespace BlackMisc //! \remarks this should be used for elevation as depicted here: http://en.wikipedia.org/wiki/Altitude#mediaviewer/File:Vertical_distances.svg const BlackMisc::PhysicalQuantities::CLength &geodeticHeight() const override { return this->m_position.geodeticHeight(); } + //! \copydoc ICoordinateGeodetic::normalVector + virtual QVector3D normalVector() const override { return this->m_position.normalVector(); } + //! Elevation //! \sa geodeticHeight const BlackMisc::PhysicalQuantities::CLength getElevation() const { return this->geodeticHeight(); } diff --git a/src/blackmisc/aviation/airport.h b/src/blackmisc/aviation/airport.h index cc032d6c3..668ab5c8b 100644 --- a/src/blackmisc/aviation/airport.h +++ b/src/blackmisc/aviation/airport.h @@ -85,17 +85,20 @@ namespace BlackMisc bool hasValidIcaoCode() const { return !this->getIcao().isEmpty(); } //! \copydoc ICoordinateGeodetic::latitude - virtual const BlackMisc::Geo::CLatitude &latitude() const override + virtual BlackMisc::Geo::CLatitude latitude() const override { return this->getPosition().latitude(); } //! \copydoc ICoordinateGeodetic::longitude - virtual const BlackMisc::Geo::CLongitude &longitude() const override + virtual BlackMisc::Geo::CLongitude longitude() const override { return this->getPosition().longitude(); } + //! \copydoc ICoordinateGeodetic::normalVector + virtual QVector3D normalVector() const override { return this->getPosition().normalVector(); } + //! \copydoc CValueObject::propertyByIndex CVariant propertyByIndex(const BlackMisc::CPropertyIndex &index) const; diff --git a/src/blackmisc/aviation/atcstation.cpp b/src/blackmisc/aviation/atcstation.cpp index 127f0432f..4210be32d 100644 --- a/src/blackmisc/aviation/atcstation.cpp +++ b/src/blackmisc/aviation/atcstation.cpp @@ -318,12 +318,12 @@ namespace BlackMisc } } - const CLatitude &CAtcStation::latitude() const + CLatitude CAtcStation::latitude() const { return this->getPosition().latitude(); } - const CLongitude &CAtcStation::longitude() const + CLongitude CAtcStation::longitude() const { return this->getPosition().longitude(); } @@ -333,6 +333,11 @@ namespace BlackMisc return this->m_position.geodeticHeight(); } + QVector3D CAtcStation::normalVector() const + { + return this->m_position.normalVector(); + } + CVariant CAtcStation::propertyByIndex(const BlackMisc::CPropertyIndex &index) const { if (index.isMyself()) { return CVariant::from(*this); } diff --git a/src/blackmisc/aviation/atcstation.h b/src/blackmisc/aviation/atcstation.h index 5d3cf183e..f7182384b 100644 --- a/src/blackmisc/aviation/atcstation.h +++ b/src/blackmisc/aviation/atcstation.h @@ -221,15 +221,18 @@ namespace BlackMisc void setBookedUntilUtc(const QDateTime &until) { this->m_bookedUntilUtc = until; } //! \copydoc ICoordinateGeodetic::latitude - virtual const BlackMisc::Geo::CLatitude &latitude() const override; + virtual BlackMisc::Geo::CLatitude latitude() const override; //! \copydoc ICoordinateGeodetic::longitude - virtual const BlackMisc::Geo::CLongitude &longitude() const override; + virtual BlackMisc::Geo::CLongitude longitude() const override; //! \copydoc ICoordinateGeodetic::geodeticHeight //! \remarks this should be used for elevation as depicted here: http://en.wikipedia.org/wiki/Altitude#mediaviewer/File:Vertical_distances.svg const BlackMisc::PhysicalQuantities::CLength &geodeticHeight() const override; + //! \copydoc ICoordinateGeodetic::normalVector + virtual QVector3D normalVector() const override; + //! \copydoc CValueObject::propertyByIndex CVariant propertyByIndex(const BlackMisc::CPropertyIndex &index) const; diff --git a/src/blackmisc/geo/coordinategeodetic.cpp b/src/blackmisc/geo/coordinategeodetic.cpp index 90b4a7eef..fdc690d9a 100644 --- a/src/blackmisc/geo/coordinategeodetic.cpp +++ b/src/blackmisc/geo/coordinategeodetic.cpp @@ -25,7 +25,7 @@ namespace BlackMisc QString CCoordinateGeodetic::convertToQString(bool i18n) const { QString s = "Geodetic: {%1, %2, %3}"; - return s.arg(this->m_latitude.valueRoundedWithUnit(6, i18n)).arg(this->m_longitude.valueRoundedWithUnit(6, i18n)).arg(this->m_geodeticHeight.valueRoundedWithUnit(6, i18n)); + return s.arg(this->latitude().valueRoundedWithUnit(6, i18n)).arg(this->longitude().valueRoundedWithUnit(6, i18n)).arg(this->m_geodeticHeight.valueRoundedWithUnit(6, i18n)); } CCoordinateGeodetic CCoordinateGeodetic::fromWgs84(const QString &latitudeWgs84, const QString &longitudeWgs84, const CLength &geodeticHeight) @@ -37,52 +37,33 @@ namespace BlackMisc PhysicalQuantities::CLength calculateGreatCircleDistance(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2) { - // same coordinates results in 0 distance - if (coordinate1.latitude() == coordinate2.latitude() && coordinate1.longitude() == coordinate2.longitude()) - { - return CLength(0, CLengthUnit::m()); - } - - // first, prelimary distance calculation - // http://www.movable-type.co.uk/scripts/latlong.html - double earthRadiusM = 6371000.8; - double lon1rad = coordinate1.longitude().value(CAngleUnit::rad()); - double lon2rad = coordinate2.longitude().value(CAngleUnit::rad()); - double lat1rad = coordinate1.latitude().value(CAngleUnit::rad()); - double lat2rad = coordinate2.latitude().value(CAngleUnit::rad()); - - double dLat = lat2rad - lat1rad; - double dLon = lon2rad - lon1rad; - double a = qSin(dLat / 2.0) * qSin(dLat / 2.0) + - qCos(lat1rad) * qCos(lat2rad) * qSin(dLon / 2.0) * qSin(dLon / 2.0); - double c = 2.0 * qAtan(qSqrt(a) / qSqrt(1.0 - a)); - double distance = earthRadiusM * c; - - Q_ASSERT_X(distance >= 0, Q_FUNC_INFO, "distance should never calculate to negative values"); - return CLength(distance, CLengthUnit::m()); + static const float earthRadiusMeters = 6371000.8f; + const QVector3D v1 = coordinate1.normalVector(); + const QVector3D v2 = coordinate2.normalVector(); + const float d = earthRadiusMeters * std::atan2(QVector3D::crossProduct(v1, v2).length(), QVector3D::dotProduct(v1, v2)); + return { d, PhysicalQuantities::CLengthUnit::m() }; } PhysicalQuantities::CAngle calculateBearing(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2) { - // same coordinate results in 0 distance - if (coordinate1.latitude() == coordinate2.latitude() && coordinate1.longitude() == coordinate2.longitude()) - { - return CAngle(0, CAngleUnit::deg()); - } + static const QVector3D northPole { 0, 0, 1 }; + const QVector3D c1 = QVector3D::crossProduct(coordinate1.normalVector(), coordinate2.normalVector()); + const QVector3D c2 = QVector3D::crossProduct(coordinate1.normalVector(), northPole); + const QVector3D cross = QVector3D::crossProduct(c1, c2); + const float sinTheta = std::copysign(cross.length(), QVector3D::dotProduct(cross, coordinate1.normalVector())); + const float cosTheta = QVector3D::dotProduct(c1, c2); + const float theta = std::atan2(sinTheta, cosTheta); + return { theta, PhysicalQuantities::CAngleUnit::rad() }; + } - // http://www.yourhomenow.com/house/haversine.html - double lon1rad = coordinate1.longitude().value(CAngleUnit::rad()); - double lon2rad = coordinate2.longitude().value(CAngleUnit::rad()); - double lat1rad = coordinate1.latitude().value(CAngleUnit::rad()); - double lat2rad = coordinate2.latitude().value(CAngleUnit::rad()); - double dLon = lon1rad - lon2rad; - double y = qSin(dLon) * qCos(lat2rad); - double x = qCos(lat1rad) * qSin(lat2rad) - - qSin(lat1rad) * qCos(lat2rad) * qCos(dLon); - double bearing = qAtan2(y, x); - bearing = CMathUtils::rad2deg(bearing); // now in deg - bearing = CMathUtils::normalizeDegrees(bearing); // normalize - return CAngle(bearing, CAngleUnit::deg()); + double calculateEuclideanDistance(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2) + { + return coordinate1.normalVector().distanceToPoint(coordinate2.normalVector()); + } + + double calculateEuclideanDistanceSquared(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2) + { + return (coordinate1.normalVector() - coordinate2.normalVector()).lengthSquared(); } CLength ICoordinateGeodetic::calculateGreatCircleDistance(const ICoordinateGeodetic &otherCoordinate) const @@ -98,7 +79,7 @@ namespace BlackMisc bool ICoordinateGeodetic::canHandleIndex(const CPropertyIndex &index) { int i = index.frontCasted(); - return (i >= static_cast(IndexLatitude)) && (i <= static_cast(IndexGeodeticHeightAsString)); + return (i >= static_cast(IndexLatitude)) && (i <= static_cast(IndexNormalVector)); } CVariant ICoordinateGeodetic::propertyByIndex(const BlackMisc::CPropertyIndex &index) const @@ -120,6 +101,8 @@ namespace BlackMisc return this->geodeticHeight().propertyByIndex(index.copyFrontRemoved()); case IndexGeodeticHeightAsString: return CVariant(this->geodeticHeightAsString()); + case IndexNormalVector: + return CVariant::fromValue(this->normalVector()); default: break; } @@ -153,10 +136,10 @@ namespace BlackMisc this->m_geodeticHeight.setPropertyByIndex(variant, index.copyFrontRemoved()); break; case IndexLatitude: - this->m_latitude.setPropertyByIndex(variant, index.copyFrontRemoved()); + this->setLatitude(variant.value()); break; case IndexLongitude: - this->m_longitude.setPropertyByIndex(variant, index.copyFrontRemoved()); + this->setLongitude(variant.value()); break; case IndexLatitudeAsString: this->setLatitude(CLatitude::fromWgs84(variant.toQString())); @@ -167,17 +150,59 @@ namespace BlackMisc case IndexGeodeticHeightAsString: this->m_geodeticHeight.parseFromString(variant.toQString()); break; + case IndexNormalVector: + this->setNormalVector(variant.value()); + break; default: CValueObject::setPropertyByIndex(variant, index); break; } } - CCoordinateGeodetic &CCoordinateGeodetic::switchUnit(const CAngleUnit &unit) + CCoordinateGeodetic::CCoordinateGeodetic(CLatitude latitude, CLongitude longitude, BlackMisc::PhysicalQuantities::CLength height) : + m_x(latitude.cos() * longitude.cos()), + m_y(latitude.cos() * longitude.sin()), + m_z(latitude.sin()), + m_geodeticHeight(height) + {} + + CLatitude CCoordinateGeodetic::latitude() const { - this->m_latitude.switchUnit(unit); - this->m_longitude.switchUnit(unit); - return *this; + const QVector3D v = this->normalVector(); + return { std::atan2(v.z(), std::hypot(v.x(), v.y())), PhysicalQuantities::CAngleUnit::rad() }; + } + + CLongitude CCoordinateGeodetic::longitude() const + { + const QVector3D v = this->normalVector(); + return { std::atan2(v.y(), v.x()), PhysicalQuantities::CAngleUnit::rad() }; + } + + QVector3D CCoordinateGeodetic::normalVector() const + { + return + { + static_cast(this->m_x), + static_cast(this->m_y), + static_cast(this->m_z) + }; + } + + void CCoordinateGeodetic::setLatitude(const CLatitude &latitude) + { + this->setLatLong(latitude, this->longitude()); + } + + void CCoordinateGeodetic::setLongitude(const CLongitude &longitude) + { + this->setLatLong(this->latitude(), longitude); + } + + void CCoordinateGeodetic::setLatLong(const CLatitude &latitude, const CLongitude &longitude) + { + this->m_x = latitude.cos() * longitude.cos(); + this->m_y = latitude.cos() * longitude.sin(); + this->m_z = latitude.sin(); } CCoordinateGeodetic &CCoordinateGeodetic::switchUnit(const CLengthUnit &unit) diff --git a/src/blackmisc/geo/coordinategeodetic.h b/src/blackmisc/geo/coordinategeodetic.h index 800d98a18..0f640bb83 100644 --- a/src/blackmisc/geo/coordinategeodetic.h +++ b/src/blackmisc/geo/coordinategeodetic.h @@ -17,6 +17,7 @@ #include "blackmisc/geo/longitude.h" #include "blackmisc/pq/length.h" #include "blackmisc/propertyindex.h" +#include namespace BlackMisc { @@ -38,17 +39,18 @@ namespace BlackMisc IndexLatitudeAsString, IndexLongitudeAsString, IndexGeodeticHeight, - IndexGeodeticHeightAsString + IndexGeodeticHeightAsString, + IndexNormalVector }; //! Destructor virtual ~ICoordinateGeodetic() {} //! Latitude - virtual const CLatitude &latitude() const = 0; + virtual CLatitude latitude() const = 0; //! Longitude - virtual const CLongitude &longitude() const = 0; + virtual CLongitude longitude() const = 0; //! Height, ellipsoidal or geodetic height (used in GPS) //! This is approximately MSL (orthometric) height, aka elevation. @@ -56,6 +58,11 @@ namespace BlackMisc //! \sa http://www.esri.com/news/arcuser/0703/geoid1of3.html virtual const BlackMisc::PhysicalQuantities::CLength &geodeticHeight() const = 0; + //! Normal vector + //! \sa https://en.wikipedia.org/wiki/N-vector + //! \sa http://www.movable-type.co.uk/scripts/latlong-vectors.html + virtual QVector3D normalVector() const = 0; + //! \copydoc CValueObject::propertyByIndex CVariant propertyByIndex(const BlackMisc::CPropertyIndex &index) const; @@ -85,6 +92,12 @@ namespace BlackMisc //! Initial bearing BLACKMISC_EXPORT BlackMisc::PhysicalQuantities::CAngle calculateBearing(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2); + //! Euclidean distance between normal vectors + BLACKMISC_EXPORT double calculateEuclideanDistance(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2); + + //! Euclidean distance squared between normal vectors, use for more efficient sorting by distance + BLACKMISC_EXPORT double calculateEuclideanDistanceSquared(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2); + //! Interface (actually more an abstract class) of coordinate and //! relative position to own aircraft class BLACKMISC_EXPORT ICoordinateWithRelativePosition : public ICoordinateGeodetic @@ -131,44 +144,52 @@ namespace BlackMisc //! Default constructor CCoordinateGeodetic() = default; + //! Constructor by normal vector + CCoordinateGeodetic(const QVector3D &normal) : m_x(normal.x()), m_y(normal.y()), m_z(normal.z()) {} + //! Constructor by values - CCoordinateGeodetic(CLatitude latitude, CLongitude longitude, BlackMisc::PhysicalQuantities::CLength height) : - m_latitude(latitude), m_longitude(longitude), m_geodeticHeight(height) {} + CCoordinateGeodetic(CLatitude latitude, CLongitude longitude, BlackMisc::PhysicalQuantities::CLength height); //! Constructor by values CCoordinateGeodetic(double latitudeDegrees, double longitudeDegrees, double heightMeters) : - m_latitude(latitudeDegrees, BlackMisc::PhysicalQuantities::CAngleUnit::deg()), m_longitude(longitudeDegrees, BlackMisc::PhysicalQuantities::CAngleUnit::deg()), m_geodeticHeight(heightMeters, BlackMisc::PhysicalQuantities::CLengthUnit::m()) {} + CCoordinateGeodetic({ latitudeDegrees, BlackMisc::PhysicalQuantities::CAngleUnit::deg() }, { longitudeDegrees, BlackMisc::PhysicalQuantities::CAngleUnit::deg() }, { heightMeters, BlackMisc::PhysicalQuantities::CLengthUnit::m() }) {} //! \copydoc ICoordinateGeodetic::latitude - virtual const CLatitude &latitude() const override { return this->m_latitude; } + virtual CLatitude latitude() const override; //! \copydoc ICoordinateGeodetic::longitude - virtual const CLongitude &longitude() const override { return this->m_longitude; } + virtual CLongitude longitude() const override; //! \copydoc ICoordinateGeodetic::geodeticHeight virtual const BlackMisc::PhysicalQuantities::CLength &geodeticHeight() const override { return this->m_geodeticHeight; } + //! \copydoc ICoordinateGeodetic::normalVector + virtual QVector3D normalVector() const; + //! \copydoc CValueObject::propertyByIndex CVariant propertyByIndex(const BlackMisc::CPropertyIndex &index) const; //! \copydoc CValueObject::setPropertyByIndex void setPropertyByIndex(const CVariant &variant, const BlackMisc::CPropertyIndex &index); - //! Switch unit of latitude / longitude - CCoordinateGeodetic &switchUnit(const BlackMisc::PhysicalQuantities::CAngleUnit &unit); - //! Switch unit of height CCoordinateGeodetic &switchUnit(const BlackMisc::PhysicalQuantities::CLengthUnit &unit); //! Set latitude - void setLatitude(const CLatitude &latitude) { this->m_latitude = latitude; } + void setLatitude(const CLatitude &latitude); //! Set longitude - void setLongitude(const CLongitude &longitude) { this->m_longitude = longitude; } + void setLongitude(const CLongitude &longitude); + + //! Set latitude and longitude + void setLatLong(const CLatitude &latitude, const CLongitude &longitude); //! Set height (ellipsoidal or geodetic height) void setGeodeticHeight(const BlackMisc::PhysicalQuantities::CLength &height) { this->m_geodeticHeight = height; } + //! Set normal vector + void setNormalVector(const QVector3D &normal) { this->m_x = normal.x(); this->m_y = normal.y(); this->m_z = normal.z(); } + //! Coordinate by WGS84 position data static CCoordinateGeodetic fromWgs84(const QString &latitudeWgs84, const QString &longitudeWgs84, const BlackMisc::PhysicalQuantities::CLength &geodeticHeight = {}); @@ -177,15 +198,16 @@ namespace BlackMisc private: BLACK_ENABLE_TUPLE_CONVERSION(CCoordinateGeodetic) - BlackMisc::Geo::CLatitude m_latitude; //!< Latitude - BlackMisc::Geo::CLongitude m_longitude; //!< Longitude + double m_x = 0; //!< normal vector + double m_y = 0; //!< normal vector + double m_z = 0; //!< normal vector BlackMisc::PhysicalQuantities::CLength m_geodeticHeight { 0, BlackMisc::PhysicalQuantities::CLengthUnit::nullUnit() }; //!< height, ellipsoidal or geodetic height }; } // namespace } // namespace -BLACK_DECLARE_TUPLE_CONVERSION(BlackMisc::Geo::CCoordinateGeodetic, (o.m_latitude, o.m_longitude, o.m_geodeticHeight)) +BLACK_DECLARE_TUPLE_CONVERSION(BlackMisc::Geo::CCoordinateGeodetic, (o.m_x, o.m_y, o.m_z, o.m_geodeticHeight)) Q_DECLARE_METATYPE(BlackMisc::Geo::CCoordinateGeodetic) #endif // guard diff --git a/src/blackmisc/simulation/simulatedaircraft.h b/src/blackmisc/simulation/simulatedaircraft.h index 070eab8ee..ab8c4ab42 100644 --- a/src/blackmisc/simulation/simulatedaircraft.h +++ b/src/blackmisc/simulation/simulatedaircraft.h @@ -158,15 +158,18 @@ namespace BlackMisc const BlackMisc::PhysicalQuantities::CSpeed &getGroundSpeed() const { return this->m_situation.getGroundSpeed(); } //! \copydoc ICoordinateGeodetic::latitude - virtual const BlackMisc::Geo::CLatitude &latitude() const override { return this->m_situation.latitude(); } + virtual BlackMisc::Geo::CLatitude latitude() const override { return this->m_situation.latitude(); } //! \copydoc ICoordinateGeodetic::longitude - virtual const BlackMisc::Geo::CLongitude &longitude() const override { return this->m_situation.longitude(); } + virtual BlackMisc::Geo::CLongitude longitude() const override { return this->m_situation.longitude(); } //! \copydoc ICoordinateGeodetic::geodeticHeight //! \remarks this should be used for elevation as depicted here: http://en.wikipedia.org/wiki/Altitude#mediaviewer/File:Vertical_distances.svg const BlackMisc::PhysicalQuantities::CLength &geodeticHeight() const override { return this->m_situation.geodeticHeight(); } + //! \copydoc ICoordinateGeodetic::normalVector + virtual QVector3D normalVector() const override { return this->m_situation.normalVector(); } + //! Elevation //! \sa geodeticHeight const BlackMisc::PhysicalQuantities::CLength getElevation() const { return this->geodeticHeight(); }