flightsimulatorplatfrom/pilotclient
1
0
Fork 0
mirror of https://github.com/swift-project/pilotclient.git synced 2026-04-02 15:15:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

style changes and removals of typeid

Browse Source 
refs #81
This commit is contained in:
Klaus Basan
2013-12-22 20:28:56 +00:00
committed by Mathew Sutcliffe
parent 229d7c6068
commit 978f3c88e5
73 changed files with 7356 additions and 7130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

426
src/blackmisc/coordinatetransformation.cpp
View File

@@ -15,233 +15,233 @@ 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)
namespace Geo
{
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)
/*
* NED to ECEF
*/
CCoordinateEcef CCoordinateTransformation::toEcef(const CCoordinateNed &ned)
{
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
CLatitude lat = ned.referencePosition().latitude();
CLongitude lon = ned.referencePosition().longitude();
double angleRad = - (lat.value(CAngleUnit::rad())) - CMath::PI() / 2;
//!N.B. cubicRootReal always returns the real root. cubicRootReal(-8) = -2.
double T = CMath::cubicRootReal(T3);
CMatrix3x3 dcm1;
CMatrix3x3 dcm2;
CMatrix3x3 dcm3;
CMatrix3x3 dcm;
CMatrix3x3 invDcm;
dcm1.setToIdentity();
dcm2.setZero();
dcm3.setZero();
// 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);
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;
}
// This is garanteed positive
double V = sqrt(CMath::square(u) + e4() * q);
/*
* 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;
}
/*
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
* 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();
// Need to guard against w going negative due to roundoff in uv - q.
double w = std::max(double(0), e2abs() * (uv - q) / (2 * V));
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;
}
/*
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);
* ECEF to geodetic
*/
CCoordinateGeodetic CCoordinateTransformation::toGeodetic(const CCoordinateEcef &ecef)
{
double R = CMath::hypot(ecef.x(), ecef.y());
double slam = 0;
double clam = 1;
sphi = (ecef.z() / k1) / H;
cphi = (R / k2) / H;
if (R != 0)
{
slam = ecef.y() / R;
clam = ecef.x() / R;
}
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();
}
// Calculate the distance to the earth
double h = CMath::hypot(R, ecef.z());
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;
}
double sphi = 0;
double cphi = 0;
} // namespace
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