From f98ec8068084ca7d48d6a161755d6e7f0035a762 Mon Sep 17 00:00:00 2001 From: Klaus Basan Date: Fri, 19 Apr 2013 19:17:48 +0200 Subject: [PATCH] Geodetic conversions transfered to Transformer class --- src/blackmisc/coordinatetransformation.cpp | 141 +++++++++++++++++++-- src/blackmisc/coordinatetransformation.h | 68 ++++++++++ src/blackmisc/pqunits.h | 5 + 3 files changed, 203 insertions(+), 11 deletions(-) diff --git a/src/blackmisc/coordinatetransformation.cpp b/src/blackmisc/coordinatetransformation.cpp index eeb8b0cbe..c688276b2 100644 --- a/src/blackmisc/coordinatetransformation.cpp +++ b/src/blackmisc/coordinatetransformation.cpp @@ -62,7 +62,8 @@ CCoordinateEcef CCoordinateTransformation::toEcef(const CCoordinateNed &ned) /* * Convert to NED */ -CCoordinateNed toNed(const CCoordinateEcef &ecef, const CCoordinateGeodetic &geo) { +CCoordinateNed toNed(const CCoordinateEcef &ecef, const CCoordinateGeodetic &geo) +{ CLatitude lat = geo.latitude(); CLongitude lon = geo.longitude(); @@ -74,18 +75,18 @@ CCoordinateNed toNed(const CCoordinateEcef &ecef, const CCoordinateGeodetic &geo CMatrix3x3 dcm(0.0); dcm1.setToIdentity(); - dcm2(0,0) = cos( angleRad ); - dcm2(0,2) = -sin( angleRad ); - dcm2(1,1) = 1; - dcm2(2,0) = sin( angleRad ); - dcm2(2,2) = cos( angleRad ); + dcm2(0, 0) = cos(angleRad); + dcm2(0, 2) = -sin(angleRad); + dcm2(1, 1) = 1; + dcm2(2, 0) = sin(angleRad); + dcm2(2, 2) = cos(angleRad); angleRad = lon.value(CAngleUnit::rad()); - dcm3(0,0) = cos(angleRad ); - dcm3(0,1) = sin(angleRad ); - dcm3(1,0) = -sin(angleRad ); - dcm3(1,1) = cos(angleRad ); - dcm3(2,2) = 1; + dcm3(0, 0) = cos(angleRad); + dcm3(0, 1) = sin(angleRad); + dcm3(1, 0) = -sin(angleRad); + dcm3(1, 1) = cos(angleRad); + dcm3(2, 2) = 1; dcm = dcm1 * dcm2 * dcm3; @@ -94,5 +95,123 @@ CCoordinateNed toNed(const CCoordinateEcef &ecef, const CCoordinateGeodetic &geo return result; } +/* + * ECEF to geodetic + */ +CCoordinateGeodetic CCoordinateTransformation::toGeodetic(const CCoordinateEcef &ecef) +{ + double R = CMath::hypot(ecef.x(), ecef.y()); + double slam = 0; + double clam = 1; + + if (R != 0) + { + slam = ecef.y() / R; + clam = ecef.x() / R; + } + + // Calculate the distance to the earth + double h = CMath::hypot(R, ecef.z()); + + double sphi = 0; + double cphi = 0; + + double p = CMath::square(R / EarthRadiusMeters()); + double q = e2m() * CMath::square(ecef.z() / EarthRadiusMeters()); + double r = (p + q - e4()) / 6.0; + + if (!(e4() *q == 0 && r <= 0)) + { + // Avoid possible division by zero when r = 0 by multiplying + // equations for s and t by r^3 and r, resp. + + double S = e4() * p * q / 4; //! S = r^3 * s + double r2 = CMath::square(r); + double r3 = r * r2; + double disc = S * (2 * r3 + S); + double u = r; + + if (disc >= 0) + { + double T3 = S + r3; + /* + Pick the sign on the sqrt to maximize abs(T3). This minimizes + loss of precision due to cancellation. The result is unchanged + because of the way the T is used in definition of u. + */ + T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3 + + //!N.B. cubicRootReal always returns the real root. cubicRootReal(-8) = -2. + double T = CMath::cubicRootReal(T3); + + // T can be zero; but then r2 / T -> 0. + u += T + (T != 0 ? r2 / T : 0); + } + else + { + // T is complex, but the way u is defined the result is real. + double ang = atan2(sqrt(-disc), -(S + r3)); + /* + There are three possible cube roots. We choose the root which + avoids cancellation. Note that disc < 0 implies that r < 0. + */ + u += 2 * r * cos(ang / 3); + } + + // This is garanteed positive + double V = sqrt(CMath::square(u) + e4() * q); + + /* + Avoid loss of accuracy when u < 0. Underflow doesn't occur in + e4 * q / (v - u) because u ~ e^4 when q is small and u < 0. + */ + double uv = u < 0 ? e4() * q / (V - u) : u + V; //! u+v, guaranteed positive + + // Need to guard against w going negative due to roundoff in uv - q. + double w = std::max(double(0), e2abs() * (uv - q) / (2 * V)); + + /* + Rearrange expression for k to avoid loss of accuracy due to + subtraction. Division by 0 not possible because uv > 0, w >= 0. + */ + double k = uv / (sqrt(uv + CMath::square(w)) + w); + double k1 = k; + double k2 = k + e2(); + double d = k1 * R / k2; + double H = CMath::hypot((ecef.z()) / k1, R / k2); + + sphi = (ecef.z() / k1) / H; + cphi = (R / k2) / H; + + h = (1 - e2m() / k1) * CMath::hypot(d, ecef.z()); + } + else // e4 * q == 0 && r <= 0 + { + /* + This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0 + (prolate, rotation axis) and the generation of 0/0 in the general + formulas for phi and h. using the general formula and division by 0 + in formula for h. So handle this case by taking the limits: + f > 0: z -> 0, k -> e2 * sqrt(q)/sqrt(e4 - p) + f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p) + */ + double zz = sqrt((e4() - p) / e2m()); + double xx = sqrt(p); + double H = CMath::hypot(zz, xx); + sphi = zz / H; + cphi = xx / H; + if (ecef.z() < 0) sphi = -sphi; // for tiny negative Z (not for prolate) + h = - EarthRadiusMeters() * (e2m()) * H / e2abs(); + } + + double latRad = atan2(sphi, cphi); + double lonRad = -atan2(-slam, clam); // Negative signs return lon in [-180, 180) + CCoordinateGeodetic result( + CLatitude(latRad, CAngleUnit::rad()), + CLongitude(lonRad, CAngleUnit::rad()), + CLength(h, CLengthUnit::m())); + return result; +} + } // namespace } // namespace diff --git a/src/blackmisc/coordinatetransformation.h b/src/blackmisc/coordinatetransformation.h index c0522b4e3..142682dc2 100644 --- a/src/blackmisc/coordinatetransformation.h +++ b/src/blackmisc/coordinatetransformation.h @@ -30,11 +30,72 @@ namespace Geo class CCoordinateTransformation { private: + /*! + * \brief Equatorial radius of WGS84 ellipsoid (6378137 m) + * \return + */ + static const qreal &EarthRadiusMeters() + { + static qreal erm = 6378137.0; + return erm; + } + + /*! + * \brief Flattening of WGS84 ellipsoid (1/298.257223563) + * \return + */ + static const qreal &Flattening() + { + static qreal f = 1/298.257223563; + return f; + } + + /*! + * \brief First eccentricity squared + * \return + */ + static const qreal &e2() + { + static qreal e2 = (Flattening() * (2 - Flattening())); + return e2; + } + + /*! + * \brief First eccentricity to power of four + * \return + */ + static const qreal &e4() + { + static qreal e4 = BlackMisc::Math::CMath::square(e2()); + return e4; + } + + /*! + * \brief First eccentricity squared absolute + * \return + */ + static const qreal &e2abs() + { + static qreal e2abs = abs(e2()); + return e2abs; + } + + /*! + * \brief Eccentricity e2m + * \return + */ + static const qreal &e2m() + { + static qreal e2m = BlackMisc::Math::CMath::square(1 - Flattening()); + return e2m; + } + /*! * \brief Default constructor, avoid object instantiation */ CCoordinateTransformation() {} + public: /*! * \brief NED to ECEF @@ -50,6 +111,13 @@ public: */ static CCoordinateNed toNed(const CCoordinateEcef &ecef, const CCoordinateGeodetic &geo); + /*! + * \brief ECEF to Geodetic + * \param geo + * \return + */ + static CCoordinateGeodetic toGeodetic(const CCoordinateEcef &ecef); + }; } // namespace diff --git a/src/blackmisc/pqunits.h b/src/blackmisc/pqunits.h index 63ccfc9c8..058a7c901 100644 --- a/src/blackmisc/pqunits.h +++ b/src/blackmisc/pqunits.h @@ -48,6 +48,7 @@ public: { // void } + /*! * \brief Meter m * \return @@ -57,6 +58,7 @@ public: static CLengthUnit m("meter", "m", true, true); return m; } + /*! * \brief Nautical miles NM * \return @@ -66,6 +68,7 @@ public: static CLengthUnit NM("nautical miles", "NM", false, false, 1000.0 * 1.85200, CMeasurementPrefix::One(), 3); return NM; } + /*! * \brief Foot ft * \return @@ -75,6 +78,7 @@ public: static CLengthUnit ft("foot", "ft", false, false, 0.3048, CMeasurementPrefix::One(), 0); return ft; } + /*! * \brief Kilometer km * \return @@ -84,6 +88,7 @@ public: static CLengthUnit km("kilometer", "km", true, false, CMeasurementPrefix::k().getFactor(), CMeasurementPrefix::k(), 3); return km; } + /*! * \brief Centimeter cm * \return