mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-03-22 14:55:36 +08:00
refs #396 remove obsolete coordinate classes
* CCoordinateEcef * CCoordinateNed * CCoordinateTransformation
This commit is contained in:
@@ -4,7 +4,6 @@
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/. */
|
||||
|
||||
#include "samplesgeo.h"
|
||||
#include "samplesgeodetictoecef.h"
|
||||
#include <QCoreApplication>
|
||||
|
||||
/*!
|
||||
@@ -17,6 +16,5 @@ int main(int argc, char *argv[])
|
||||
{
|
||||
QCoreApplication a(argc, argv);
|
||||
BlackMiscTest::CSamplesGeo::samples();
|
||||
BlackMiscTest::CSamplesGeodeticToEcef::samples();
|
||||
return a.exec();
|
||||
}
|
||||
|
||||
@@ -4,6 +4,8 @@
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/. */
|
||||
|
||||
#include "samplesgeo.h"
|
||||
#include "blackmisc/geolatitude.h"
|
||||
#include "blackmisc/geolongitude.h"
|
||||
|
||||
using namespace BlackMisc::Geo;
|
||||
using namespace BlackMisc::PhysicalQuantities;
|
||||
@@ -31,26 +33,6 @@ namespace BlackMiscTest
|
||||
// lat3 = lon1; //must not work
|
||||
// CGeoLongitude lonx(lat2); // must notwork
|
||||
|
||||
CCoordinateGeodetic cg(10.0, 20.0, 1000);
|
||||
CCoordinateEcef ce = CCoordinateTransformation::toEcef(cg);
|
||||
CCoordinateGeodetic cg2 = CCoordinateTransformation::toGeodetic(ce);
|
||||
cg2.switchUnit(CAngleUnit::deg());
|
||||
qDebug() << cg << ce << cg2;
|
||||
|
||||
CCoordinateNed cned = CCoordinateTransformation::toNed(ce, cg);
|
||||
CCoordinateEcef ce2 = CCoordinateTransformation::toEcef(cned);
|
||||
qDebug() << ce << cned << ce2;
|
||||
qDebug() << (cned + cned) << (ce + ce);
|
||||
|
||||
// cned += ce2; // must not work
|
||||
|
||||
// checked with http://www.movable-type.co.uk/scripts/latlong.html
|
||||
CCoordinateGeodetic coord1 = CCoordinateGeodetic::fromWgs84("50 03 59N", "005 42 53W");
|
||||
CCoordinateGeodetic coord2 = CCoordinateGeodetic::fromWgs84("50 03 59N", "005 42 53W");
|
||||
CCoordinateGeodetic coord3 = CCoordinateGeodetic::fromWgs84("58 38 38N", "003 04 12W");
|
||||
qDebug() << coord1 << coord2 << calculateGreatCircleDistance(coord1, coord2); // should be 0
|
||||
qDebug() << coord1 << coord3 << calculateGreatCircleDistance(coord1, coord3) << calculateGreatCircleDistance(coord1, coord3).switchUnit(CLengthUnit::km()) ; // should be Distance: 968.9 km (to 4 SF*)
|
||||
|
||||
// bye
|
||||
qDebug() << "-----------------------------------------------";
|
||||
return 0;
|
||||
|
||||
@@ -5,7 +5,6 @@
|
||||
|
||||
#ifndef BLACKMISCTEST_SAMPLESGEO_H
|
||||
#define BLACKMISCTEST_SAMPLESGEO_H
|
||||
#include "blackmisc/coordinatetransformation.h"
|
||||
|
||||
namespace BlackMiscTest
|
||||
{
|
||||
|
||||
@@ -1,44 +0,0 @@
|
||||
/* 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/. */
|
||||
|
||||
#include "samplesgeodetictoecef.h"
|
||||
#include <QElapsedTimer>
|
||||
|
||||
using namespace BlackMisc::Geo;
|
||||
|
||||
namespace BlackMiscTest
|
||||
{
|
||||
|
||||
/*
|
||||
* Samples
|
||||
*/
|
||||
int CSamplesGeodeticToEcef::samples()
|
||||
{
|
||||
|
||||
QElapsedTimer timer;
|
||||
qint64 duration;
|
||||
|
||||
double lat = 27.999999, lon = 86.999999, h = 8820.999999; // Mt Everest
|
||||
CCoordinateGeodetic startVec(lat, lon, h);
|
||||
std::cout << startVec << std::endl;
|
||||
|
||||
timer.start();
|
||||
CCoordinateEcef mediumvec = CCoordinateTransformation::toEcef(startVec);
|
||||
duration = timer.nsecsElapsed();
|
||||
std::cout << mediumvec << " ";
|
||||
std::cout << "Needed " << duration << " nanoseconds for the calculation!" << std::endl;
|
||||
|
||||
timer.restart();
|
||||
CCoordinateGeodetic endVec = CCoordinateTransformation::toGeodetic(mediumvec);
|
||||
duration = timer.nsecsElapsed();
|
||||
|
||||
std::cout << endVec << " ";
|
||||
std::cout << "Needed " << duration << " nanoseconds for the calculation!" << std::endl;
|
||||
|
||||
std::cout << "-----------------------------------------------" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -1,32 +0,0 @@
|
||||
/* 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/. */
|
||||
|
||||
#ifndef BLACKMISCTEST_SAMPLESGEO2ECEF_H
|
||||
#define BLACKMISCTEST_SAMPLESGEO2ECEF_H
|
||||
#include "blackmisc/coordinatetransformation.h"
|
||||
|
||||
namespace BlackMiscTest
|
||||
{
|
||||
|
||||
/*!
|
||||
* \brief Samples for vector / matrix
|
||||
*/
|
||||
class CSamplesGeodeticToEcef
|
||||
{
|
||||
public:
|
||||
/*!
|
||||
* \brief Run the samples
|
||||
*/
|
||||
static int samples();
|
||||
|
||||
private:
|
||||
/*!
|
||||
* \brief Avoid init
|
||||
*/
|
||||
CSamplesGeodeticToEcef() {}
|
||||
};
|
||||
} // namespace
|
||||
|
||||
#endif // guard
|
||||
@@ -41,8 +41,6 @@ void BlackMisc::Math::registerMetadata()
|
||||
*/
|
||||
void BlackMisc::Geo::registerMetadata()
|
||||
{
|
||||
CCoordinateEcef::registerMetadata();
|
||||
CCoordinateNed::registerMetadata();
|
||||
CCoordinateGeodetic::registerMetadata();
|
||||
CLatitude::registerMetadata();
|
||||
CLongitude::registerMetadata();
|
||||
|
||||
@@ -1,112 +0,0 @@
|
||||
/* 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/. */
|
||||
|
||||
#ifndef BLACKMISC_COORDINATEECEF_H
|
||||
#define BLACKMISC_COORDINATEECEF_H
|
||||
|
||||
//! \file
|
||||
|
||||
#include "blackmisc/mathvector3d.h"
|
||||
|
||||
namespace BlackMisc
|
||||
{
|
||||
namespace Geo { class CCoordinateEcef; }
|
||||
|
||||
//! \private
|
||||
template <> struct CValueObjectPolicy<Geo::CCoordinateEcef> : public CValueObjectPolicy<>
|
||||
{
|
||||
using Compare = Policy::Compare::None;
|
||||
using Hash = Policy::Hash::Own;
|
||||
using DBus = Policy::DBus::Own;
|
||||
using Json = Policy::Json::Own;
|
||||
};
|
||||
|
||||
namespace Geo
|
||||
{
|
||||
|
||||
/*!
|
||||
* \brief Earth centered, earth fixed position
|
||||
*/
|
||||
class CCoordinateEcef : public CValueObject<CCoordinateEcef, Math::CVector3DBase<CCoordinateEcef>>
|
||||
{
|
||||
public:
|
||||
/*!
|
||||
* \brief Default constructor
|
||||
*/
|
||||
CCoordinateEcef() = default;
|
||||
|
||||
/*!
|
||||
* \brief Constructor by values
|
||||
*/
|
||||
CCoordinateEcef(double x, double y, double z) : CValueObject(x, y, z) {}
|
||||
|
||||
/*!
|
||||
* \brief Constructor by math vector
|
||||
*/
|
||||
explicit CCoordinateEcef(const BlackMisc::Math::CVector3D vector) : CValueObject(vector.i(), vector.j(), vector.k()) {}
|
||||
|
||||
//! \brief x
|
||||
double x() const
|
||||
{
|
||||
return this->m_i;
|
||||
}
|
||||
|
||||
//! \brief y
|
||||
double y() const
|
||||
{
|
||||
return this->m_j;
|
||||
}
|
||||
|
||||
//! \brief z
|
||||
double z() const
|
||||
{
|
||||
return this->m_k;
|
||||
}
|
||||
|
||||
//! \brief Set x
|
||||
void setX(double x)
|
||||
{
|
||||
this->m_i = x;
|
||||
}
|
||||
|
||||
//! \brief Set y
|
||||
void setY(double y)
|
||||
{
|
||||
this->m_j = y;
|
||||
}
|
||||
|
||||
//! \brief Set z
|
||||
void setZ(double z)
|
||||
{
|
||||
this->m_k = z;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Concrete implementation of a 3D vector
|
||||
*/
|
||||
BlackMisc::Math::CVector3D toMathVector() const
|
||||
{
|
||||
return BlackMisc::Math::CVector3D(this->x(), this->y(), this->z());
|
||||
}
|
||||
|
||||
protected:
|
||||
//! \copydoc CValueObject::convertToQString
|
||||
virtual QString convertToQString(bool i18n = false) const override
|
||||
{
|
||||
Q_UNUSED(i18n)
|
||||
QString s = "ECEF: {x %1, y %2, z %3}";
|
||||
s = s.arg(QString::number(this->x(), 'f', 6)).
|
||||
arg(QString::number(this->y(), 'f', 6)).
|
||||
arg(QString::number(this->z(), 'f', 6));
|
||||
return s;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Q_DECLARE_METATYPE(BlackMisc::Geo::CCoordinateEcef)
|
||||
|
||||
#endif // guard
|
||||
@@ -1,160 +0,0 @@
|
||||
/* 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/. */
|
||||
|
||||
#ifndef BLACKMISC_COORDINATENED_H
|
||||
#define BLACKMISC_COORDINATENED_H
|
||||
|
||||
//! \file
|
||||
|
||||
#include "blackmisc/mathvector3d.h"
|
||||
#include "blackmisc/mathmatrix3x3.h"
|
||||
#include "blackmisc/coordinategeodetic.h"
|
||||
#include "blackmisc/blackmiscfreefunctions.h"
|
||||
|
||||
namespace BlackMisc
|
||||
{
|
||||
namespace Geo
|
||||
{
|
||||
/*!
|
||||
* \brief North, East, Down
|
||||
*/
|
||||
class CCoordinateNed : public CValueObject<CCoordinateNed, Math::CVector3DBase<CCoordinateNed>>
|
||||
{
|
||||
protected:
|
||||
//! \copydoc CValueObject::convertToQString
|
||||
virtual QString convertToQString(bool i18n = false) const override
|
||||
{
|
||||
Q_UNUSED(i18n)
|
||||
QString s = "NED: {N %1, E %2, D %3}";
|
||||
s = s.arg(QString::number(this->north(), 'f', 6)).
|
||||
arg(QString::number(this->east(), 'f', 6)).
|
||||
arg(QString::number(this->down(), 'f', 6));
|
||||
return s;
|
||||
}
|
||||
|
||||
public:
|
||||
/*!
|
||||
* \brief Default constructor
|
||||
*/
|
||||
CCoordinateNed() : m_hasReferencePosition(false) {}
|
||||
|
||||
/*!
|
||||
* \brief Constructor with reference position
|
||||
*/
|
||||
CCoordinateNed(const CCoordinateGeodetic &referencePosition) : m_referencePosition(referencePosition), m_hasReferencePosition(true) {}
|
||||
|
||||
/*!
|
||||
* \brief Constructor by values
|
||||
*/
|
||||
CCoordinateNed(const CCoordinateGeodetic &referencePosition, double north, double east, double down) : CValueObject(north, east, down), m_referencePosition(referencePosition), m_hasReferencePosition(true) {}
|
||||
|
||||
/*!
|
||||
* \brief Constructor by values
|
||||
*/
|
||||
CCoordinateNed(double north, double east, double down) : CValueObject(north, east, down), m_referencePosition(), m_hasReferencePosition(false) {}
|
||||
|
||||
/*!
|
||||
* \brief Constructor by math vector
|
||||
*/
|
||||
explicit CCoordinateNed(const BlackMisc::Math::CVector3D &vector) : CValueObject(vector.i(), vector.j(), vector.k()), m_referencePosition(), m_hasReferencePosition(false) {}
|
||||
|
||||
/*!
|
||||
* \brief Constructor by math vector and reference position
|
||||
*/
|
||||
CCoordinateNed(const CCoordinateGeodetic &referencePosition, const BlackMisc::Math::CVector3D &vector) : CValueObject(vector.i(), vector.j(), vector.k()), m_referencePosition(referencePosition), m_hasReferencePosition(true) {}
|
||||
|
||||
/*!
|
||||
* \brief Corresponding reference position
|
||||
*/
|
||||
CCoordinateGeodetic referencePosition() const
|
||||
{
|
||||
return this->m_referencePosition;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Corresponding reference position
|
||||
*/
|
||||
bool hasReferencePosition() const
|
||||
{
|
||||
return this->m_hasReferencePosition;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief North
|
||||
*/
|
||||
double north() const
|
||||
{
|
||||
return this->m_i;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief East
|
||||
*/
|
||||
double east() const
|
||||
{
|
||||
return this->m_j;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Down
|
||||
*/
|
||||
double down() const
|
||||
{
|
||||
return this->m_k;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Set north
|
||||
*/
|
||||
void setNorth(double north)
|
||||
{
|
||||
this->m_i = north;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Set east
|
||||
*/
|
||||
void setEast(double east)
|
||||
{
|
||||
this->m_j = east;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Set down
|
||||
*/
|
||||
void setDown(double down)
|
||||
{
|
||||
this->m_k = down;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Corresponding reference position
|
||||
*/
|
||||
void setReferencePosition(const CCoordinateGeodetic &referencePosition)
|
||||
{
|
||||
this->m_referencePosition = referencePosition;
|
||||
this->m_hasReferencePosition = true;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Concrete implementation of a 3D vector
|
||||
*/
|
||||
BlackMisc::Math::CVector3D toMathVector() const
|
||||
{
|
||||
return BlackMisc::Math::CVector3D(this->north(), this->east(), this->down());
|
||||
}
|
||||
|
||||
private:
|
||||
BLACK_ENABLE_TUPLE_CONVERSION(CCoordinateNed)
|
||||
CCoordinateGeodetic m_referencePosition; //!< geodetic reference position
|
||||
bool m_hasReferencePosition; //!< valid reference position?
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
BLACK_DECLARE_TUPLE_CONVERSION(BlackMisc::Geo::CCoordinateNed, (o.m_referencePosition, o.m_hasReferencePosition))
|
||||
Q_DECLARE_METATYPE(BlackMisc::Geo::CCoordinateNed)
|
||||
|
||||
#endif // guard
|
||||
@@ -1,245 +0,0 @@
|
||||
/* 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
|
||||
@@ -1,94 +0,0 @@
|
||||
/* Copyright (C) 2013
|
||||
* swift Project Community / Contributors
|
||||
*
|
||||
* This file is part of swift project. It is subject to the license terms in the LICENSE file found in the top-level
|
||||
* directory of this distribution and at http://www.swift-project.org/license.html. No part of swift project,
|
||||
* including this file, may be copied, modified, propagated, or distributed except according to the terms
|
||||
* contained in the LICENSE file.
|
||||
*/
|
||||
|
||||
//! \file
|
||||
|
||||
#ifndef BLACKMISC_COORDINATETRANSFORMATION_H
|
||||
#define BLACKMISC_COORDINATETRANSFORMATION_H
|
||||
|
||||
#include "blackmisc/coordinateecef.h"
|
||||
#include "blackmisc/coordinatened.h"
|
||||
#include "blackmisc/coordinategeodetic.h"
|
||||
#include "blackmisc/mathmatrix3x3.h"
|
||||
#include "blackmisc/mathematics.h"
|
||||
|
||||
namespace BlackMisc
|
||||
{
|
||||
namespace Geo
|
||||
{
|
||||
|
||||
//! Coordinate transformation class between different systems
|
||||
class CCoordinateTransformation
|
||||
{
|
||||
|
||||
public:
|
||||
//! NED to ECEF
|
||||
static CCoordinateEcef toEcef(const CCoordinateNed &ned);
|
||||
|
||||
//! Geodetic to ECEF
|
||||
static CCoordinateEcef toEcef(const CCoordinateGeodetic &geo);
|
||||
|
||||
//! ECEF via Geodetic to NED
|
||||
static CCoordinateNed toNed(const CCoordinateEcef &ecef, const CCoordinateGeodetic &referencePosition);
|
||||
|
||||
//! ECEF to Geodetic
|
||||
static CCoordinateGeodetic toGeodetic(const CCoordinateEcef &ecef);
|
||||
|
||||
private:
|
||||
//! Equatorial radius of WGS84 ellipsoid (6378137 m)
|
||||
static const double &EarthRadiusMeters()
|
||||
{
|
||||
static double erm = 6378137.0;
|
||||
return erm;
|
||||
}
|
||||
|
||||
//! Flattening of WGS84 ellipsoid (1/298.257223563)
|
||||
static const double &Flattening()
|
||||
{
|
||||
static double f = 1.0 / 298.257223563;
|
||||
return f;
|
||||
}
|
||||
|
||||
//! First eccentricity squared
|
||||
static const double &e2()
|
||||
{
|
||||
static double e2 = (Flattening() * (2 - Flattening()));
|
||||
return e2;
|
||||
}
|
||||
|
||||
//! First eccentricity to power of four
|
||||
static const double &e4()
|
||||
{
|
||||
static double e4 = BlackMisc::Math::CMath::square(e2());
|
||||
return e4;
|
||||
}
|
||||
|
||||
//! First eccentricity squared absolute
|
||||
static const double &e2abs()
|
||||
{
|
||||
static double e2abs = qAbs(e2());
|
||||
return e2abs;
|
||||
}
|
||||
|
||||
//! Eccentricity e2m
|
||||
static const double &e2m()
|
||||
{
|
||||
static double e2m = BlackMisc::Math::CMath::square(1.0 - Flattening());
|
||||
return e2m;
|
||||
}
|
||||
|
||||
//! Default constructor, deleted
|
||||
CCoordinateTransformation();
|
||||
|
||||
};
|
||||
|
||||
} // namespace
|
||||
} // namespace
|
||||
|
||||
#endif // guard
|
||||
@@ -10,8 +10,6 @@
|
||||
#include "blackmisc/geolatitude.h"
|
||||
#include "blackmisc/geolongitude.h"
|
||||
#include "blackmisc/geodesicgrid.h"
|
||||
#include "blackmisc/coordinateecef.h"
|
||||
#include "blackmisc/coordinatened.h"
|
||||
#include "blackmisc/coordinategeodetic.h"
|
||||
|
||||
#endif // guard
|
||||
|
||||
@@ -1,4 +1,6 @@
|
||||
#include "testgeo.h"
|
||||
#include "blackmisc/geolatitude.h"
|
||||
#include "blackmisc/geolongitude.h"
|
||||
|
||||
using namespace BlackMisc::Geo;
|
||||
using namespace BlackMisc::PhysicalQuantities;
|
||||
@@ -17,20 +19,6 @@ namespace BlackMiscTest
|
||||
QVERIFY2(lati * 2 == lati + lati, "Latitude addition should be equal");
|
||||
lati += CLatitude(20, CAngleUnit::deg());
|
||||
QVERIFY2(lati.valueRounded() == 30.0, "Latitude should be 30 degrees");
|
||||
|
||||
double lat = 27.999999, lon = 86.999999, h = 8820.999999; // Mt Everest
|
||||
CCoordinateGeodetic startGeoVec(lat, lon, h);
|
||||
CCoordinateEcef mediumEcefVec = CCoordinateTransformation::toEcef(startGeoVec);
|
||||
CCoordinateGeodetic endGeoVec = CCoordinateTransformation::toGeodetic(mediumEcefVec);
|
||||
|
||||
// this == contains some implicit rounding, since it is based on PQs
|
||||
QVERIFY2(startGeoVec == endGeoVec, "Reconverted geo vector should be equal ");
|
||||
|
||||
CCoordinateNed nedVec = CCoordinateTransformation::toNed(mediumEcefVec, startGeoVec);
|
||||
CCoordinateEcef ecefReconvert = CCoordinateTransformation::toEcef(nedVec);
|
||||
|
||||
// check against rounded reconvert
|
||||
QVERIFY2(mediumEcefVec.rounded() == ecefReconvert.rounded(), "Reconverted geo vector should be equal");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
@@ -8,7 +8,6 @@
|
||||
#ifndef BLACKMISCTEST_TESTGEO_H
|
||||
#define BLACKMISCTEST_TESTGEO_H
|
||||
|
||||
#include "blackmisc/coordinatetransformation.h"
|
||||
#include <QtTest/QtTest>
|
||||
|
||||
namespace BlackMiscTest
|
||||
|
||||
Reference in New Issue
Block a user