mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-03-22 14:55:36 +08:00
246 lines
9.0 KiB
C++
246 lines
9.0 KiB
C++
/* 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) <charles@karney.com> 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.geodeticHeight().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
|