refs #484 Revise CCoordinateGeodetic implementation to use n-vectors.

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(); }

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;
 

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); }

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;
 

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
+            static const float earthRadiusMeters = 6371000.8f;
-            if (coordinate1.latitude() == coordinate2.latitude() && coordinate1.longitude() == coordinate2.longitude())
+            const QVector3D v1 = coordinate1.normalVector();
-            {
+            const QVector3D v2 = coordinate2.normalVector();
-                return CLength(0, CLengthUnit::m());
+            const float d = earthRadiusMeters * std::atan2(QVector3D::crossProduct(v1, v2).length(), QVector3D::dotProduct(v1, v2));
-            }
+            return { d, PhysicalQuantities::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());
         }
 
         PhysicalQuantities::CAngle calculateBearing(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
         {
-            // same coordinate results in 0 distance
+            static const QVector3D northPole { 0, 0, 1 };
-            if (coordinate1.latitude() == coordinate2.latitude() && coordinate1.longitude() == coordinate2.longitude())
+            const QVector3D c1 = QVector3D::crossProduct(coordinate1.normalVector(), coordinate2.normalVector());
-            {
+            const QVector3D c2 = QVector3D::crossProduct(coordinate1.normalVector(), northPole);
-                return CAngle(0, CAngleUnit::deg());
+            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 calculateEuclideanDistance(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
-            double lon1rad = coordinate1.longitude().value(CAngleUnit::rad());
+        {
-            double lon2rad = coordinate2.longitude().value(CAngleUnit::rad());
+            return coordinate1.normalVector().distanceToPoint(coordinate2.normalVector());
-            double lat1rad = coordinate1.latitude().value(CAngleUnit::rad());
+        }
-            double lat2rad = coordinate2.latitude().value(CAngleUnit::rad());
+
-            double dLon = lon1rad - lon2rad;
+        double calculateEuclideanDistanceSquared(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
-            double y = qSin(dLon) * qCos(lat2rad);
+        {
-            double x = qCos(lat1rad) * qSin(lat2rad) -
+            return (coordinate1.normalVector() - coordinate2.normalVector()).lengthSquared();
-                       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());
         }
 
         CLength ICoordinateGeodetic::calculateGreatCircleDistance(const ICoordinateGeodetic &otherCoordinate) const
@@ -98,7 +79,7 @@ namespace BlackMisc
         bool ICoordinateGeodetic::canHandleIndex(const CPropertyIndex &index)
         {
             int i = index.frontCasted<int>();
-            return (i >= static_cast<int>(IndexLatitude)) && (i <= static_cast<int>(IndexGeodeticHeightAsString));
+            return (i >= static_cast<int>(IndexLatitude)) && (i <= static_cast<int>(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<CLatitude>());
                 break;
             case IndexLongitude:
-                this->m_longitude.setPropertyByIndex(variant, index.copyFrontRemoved());
+                this->setLongitude(variant.value<CLongitude>());
                 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<QVector3D>());
+                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);
+            const QVector3D v = this->normalVector();
-            this->m_longitude.switchUnit(unit);
+            return { std::atan2(v.z(), std::hypot(v.x(), v.y())), PhysicalQuantities::CAngleUnit::rad() };
-            return *this;
+        }
+
+        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<float>(this->m_x),
+                static_cast<float>(this->m_y),
+                static_cast<float>(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)

src/blackmisc/geo/coordinategeodetic.h

@@ -17,6 +17,7 @@
 #include "blackmisc/geo/longitude.h"
 #include "blackmisc/pq/length.h"
 #include "blackmisc/propertyindex.h"
+#include <QVector3D>
 
 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) :
+            CCoordinateGeodetic(CLatitude latitude, CLongitude longitude, BlackMisc::PhysicalQuantities::CLength height);
-                m_latitude(latitude), m_longitude(longitude), m_geodeticHeight(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
+            double m_x = 0; //!< normal vector
-            BlackMisc::Geo::CLongitude m_longitude; //!< Longitude
+            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

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(); }