/* Copyright (C) 2013 VATSIM Community / contributors * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. * * This file incorporates work covered by the following copyright and * permission notice: * Copyright (c) Charles Karney (2008-2011) and licensed * under the MIT/X11 License. For more information, see http://geographiclib.sourceforge.net/ */ #include "coordinatetransformation.h" using namespace BlackMisc::PhysicalQuantities; using namespace BlackMisc::Math; namespace BlackMisc { namespace Geo { /* * NED to ECEF */ CCoordinateEcef CCoordinateTransformation::toEcef(const CCoordinateNed &ned) { CLatitude lat = ned.referencePosition().latitude(); CLongitude lon = ned.referencePosition().longitude(); double angleRad = - (lat.value(CAngleUnit::rad())) - CMath::PI() / 2; CMatrix3x3 dcm1; CMatrix3x3 dcm2; CMatrix3x3 dcm3; CMatrix3x3 dcm; CMatrix3x3 invDcm; dcm1.setToIdentity(); dcm2.setZero(); dcm3.setZero(); 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; dcm = dcm1 * dcm2 * dcm3; bool inverse; invDcm.setZero(); invDcm = dcm.inverse(inverse); Q_ASSERT_X(inverse, "toEcef", "Inverse matrix could not be calculated"); CVector3D tempResult = invDcm * ned.toMathVector(); // to generic vector CCoordinateEcef result(tempResult); return result; } /* * Geodetic to ECEF */ CCoordinateEcef CCoordinateTransformation::toEcef(const CCoordinateGeodetic &geo) { CLatitude lat = geo.latitude(); CLongitude lon = geo.longitude(); double phi = lat.value(CAngleUnit::rad()); double lambdaRad = lon.value(CAngleUnit::rad()); double sphi = sin(phi); double cphi = cos(phi); double n = EarthRadiusMeters() / sqrt(1 - e2() * CMath::square(sphi)); double slambda = sin(lambdaRad); double clambda = cos(lambdaRad); double h = geo.height().value(CLengthUnit::m()); double x = (n + h) * cphi; double y = x * slambda; x *= clambda; double z = (e2m() * n + h) * sphi; CCoordinateEcef result(x, y, z); return result; } /* * Convert to NED */ CCoordinateNed CCoordinateTransformation::toNed(const CCoordinateEcef &ecef, const CCoordinateGeodetic &referencePosition) { CLatitude lat = referencePosition.latitude(); CLongitude lon = referencePosition.longitude(); double angleRad = - (lat.value(CAngleUnit::rad())) - CMath::PIHALF(); CMatrix3x3 dcm1; CMatrix3x3 dcm2(0.0); CMatrix3x3 dcm3(0.0); 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); 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; dcm = dcm1 * dcm2 * dcm3; CVector3D tempResult = dcm * ecef.toMathVector(); // to generic vector CCoordinateNed result(referencePosition, tempResult); 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 degrees [-180, 180) CCoordinateGeodetic result( CLatitude(latRad, CAngleUnit::rad()), CLongitude(lonRad, CAngleUnit::rad()), CLength(h, CLengthUnit::m())); result.switchUnit(CAngleUnit::deg()); return result; } } // namespace } // namespace