Files
pilotclient/src/blackmisc/geo/coordinategeodetic.cpp

501 lines
23 KiB
C++

/* 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. 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.
*/
#include "blackmisc/geo/coordinategeodetic.h"
#include "blackmisc/logmessage.h"
#include "blackmisc/propertyindex.h"
#include "blackmisc/variant.h"
#include "blackmisc/verify.h"
#include <QByteArray>
#include <QtGlobal>
#include <QStringBuilder>
#include <cmath>
using namespace BlackMisc::Aviation;
using namespace BlackMisc::PhysicalQuantities;
using namespace BlackMisc::Math;
namespace BlackMisc
{
namespace Geo
{
ICoordinateGeodetic::~ICoordinateGeodetic()
{ }
QString CCoordinateGeodetic::convertToQString(bool i18n) const
{
return ICoordinateGeodetic::convertToQString(i18n);
}
CCoordinateGeodetic CCoordinateGeodetic::fromWgs84(const QString &latitudeWgs84, const QString &longitudeWgs84, const CAltitude &geodeticHeight)
{
const CLatitude lat = CLatitude::fromWgs84(latitudeWgs84);
const CLongitude lon = CLongitude::fromWgs84(longitudeWgs84);
return CCoordinateGeodetic(lat, lon, geodeticHeight);
}
const CCoordinateGeodetic &CCoordinateGeodetic::null()
{
static const CCoordinateGeodetic n;
return n;
}
CLength calculateGreatCircleDistance(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
{
if (coordinate1.isNull() || coordinate2.isNull()) { return CLength::null(); }
// if (coordinate1.equalNormalVectorDouble(coordinate2)) { return CLength(0, CLengthUnit::defaultUnit()); }
constexpr float earthRadiusMeters = 6371000.8f;
const QVector3D v1 = coordinate1.normalVector();
const QVector3D v2 = coordinate2.normalVector();
Q_ASSERT_X(std::isfinite(v1.x()) && std::isfinite(v1.y()) && std::isfinite(v1.z()), Q_FUNC_INFO, "Distance calculation: v1 non-finite argument");
Q_ASSERT_X(std::isfinite(v2.x()) && std::isfinite(v2.y()) && std::isfinite(v2.z()), Q_FUNC_INFO, "Distance calculation: v2 non-finite argument");
const float d = earthRadiusMeters * std::atan2(QVector3D::crossProduct(v1, v2).length(), QVector3D::dotProduct(v1, v2));
BLACK_VERIFY_X(!std::isnan(d), Q_FUNC_INFO, "Distance calculation: NaN in result");
if (std::isnan(d))
{
CLogMessage().debug(u"Distance calculation: NaN in result (given arguments %1 %2 %3; %4 %5 %6)") << static_cast<double>(v1.x()) << static_cast<double>(v1.y()) << static_cast<double>(v1.z()) << static_cast<double>(v2.x()) << static_cast<double>(v2.y()) << static_cast<double>(v2.z());
return CLength::null();
}
return { static_cast<double>(d), CLengthUnit::m() };
}
CAngle calculateBearing(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
{
if (coordinate1.isNull() || coordinate2.isNull()) { return CAngle::null(); }
// if (coordinate1.equalNormalVectorDouble(coordinate2)) { return CAngle(0, CAngleUnit::defaultUnit()); } // null or 0?
static const QVector3D northPole { 0, 0, 1 };
const QVector3D c1 = QVector3D::crossProduct(coordinate1.normalVector(), coordinate2.normalVector());
const QVector3D c2 = QVector3D::crossProduct(coordinate1.normalVector(), northPole);
const QVector3D cross = QVector3D::crossProduct(c1, c2);
const float sinTheta = std::copysign(cross.length(), QVector3D::dotProduct(cross, coordinate1.normalVector()));
const float cosTheta = QVector3D::dotProduct(c1, c2);
const float theta = std::atan2(sinTheta, cosTheta);
return { static_cast<double>(theta), CAngleUnit::rad() };
}
double calculateEuclideanDistance(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
{
return static_cast<double>(coordinate1.normalVector().distanceToPoint(coordinate2.normalVector()));
}
double calculateEuclideanDistanceSquared(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
{
return static_cast<double>((coordinate1.normalVector() - coordinate2.normalVector()).lengthSquared());
}
bool ICoordinateGeodetic::equalNormalVectorDouble(const std::array<double, 3> &otherVector) const
{
static const double epsilon = std::numeric_limits<double>::epsilon();
const std::array<double, 3> thisVector = this->normalVectorDouble();
for (unsigned int i = 0; i < otherVector.size(); i++)
{
const double d = thisVector[i] - otherVector[i];
if (qAbs(d) > epsilon) { return false; }
}
return true;
}
bool ICoordinateGeodetic::equalNormalVectorDouble(const ICoordinateGeodetic &otherCoordinate) const
{
return this->equalNormalVectorDouble(otherCoordinate.normalVectorDouble());
}
CLength ICoordinateGeodetic::calculateGreatCircleDistance(const ICoordinateGeodetic &otherCoordinate) const
{
return Geo::calculateGreatCircleDistance((*this), otherCoordinate);
}
bool ICoordinateGeodetic::isWithinRange(const ICoordinateGeodetic &otherCoordinate, const CLength &range) const
{
if (range.isNull()) { return false; }
const CLength distance = this->calculateGreatCircleDistance(otherCoordinate);
if (distance.isNull()) { return false; }
return distance <= range;
}
CAngle ICoordinateGeodetic::calculateBearing(const ICoordinateGeodetic &otherCoordinate) const
{
return Geo::calculateBearing((*this), otherCoordinate);
}
bool ICoordinateGeodetic::canHandleIndex(const CPropertyIndex &index)
{
const int i = index.frontCasted<int>();
return (i >= static_cast<int>(IndexLatitude)) && (i <= static_cast<int>(IndexNormalVector));
}
CVariant ICoordinateGeodetic::propertyByIndex(const BlackMisc::CPropertyIndex &index) const
{
if (!index.isMyself())
{
const ColumnIndex i = index.frontCasted<ColumnIndex>();
switch (i)
{
case IndexLatitude: return this->latitude().propertyByIndex(index.copyFrontRemoved());
case IndexLongitude: return this->longitude().propertyByIndex(index.copyFrontRemoved());
case IndexLatitudeAsString: return CVariant(this->latitudeAsString());
case IndexLongitudeAsString: return CVariant(this->longitudeAsString());
case IndexGeodeticHeight: return this->geodeticHeight().propertyByIndex(index.copyFrontRemoved());
case IndexGeodeticHeightAsString: return CVariant(this->geodeticHeightAsString());
case IndexNormalVector: return CVariant::fromValue(this->normalVector());
default: break;
}
}
const QString m = QString("no property, index ").append(index.toQString());
BLACK_VERIFY_X(false, Q_FUNC_INFO, qUtf8Printable(m));
return CVariant::fromValue(m);
}
int ICoordinateGeodetic::comparePropertyByIndex(const CPropertyIndex &index, const ICoordinateGeodetic &compareValue) const
{
if (!index.isMyself())
{
const ColumnIndex i = index.frontCasted<ColumnIndex>();
switch (i)
{
case IndexLatitude: return this->latitude().comparePropertyByIndex(index.copyFrontRemoved(), compareValue.latitude());
case IndexLongitude: return this->longitude().comparePropertyByIndex(index.copyFrontRemoved(), compareValue.longitude());
case IndexLatitudeAsString: return this->latitudeAsString().compare(compareValue.latitudeAsString());
case IndexLongitudeAsString: return this->longitudeAsString().compare(compareValue.longitudeAsString());
case IndexGeodeticHeight: return this->geodeticHeight().comparePropertyByIndex(index.copyFrontRemoved(), compareValue.geodeticHeight());
case IndexGeodeticHeightAsString: return this->geodeticHeightAsString().compare(compareValue.geodeticHeightAsString());
default: break;
}
}
const QString m = QString("no property, index ").append(index.toQString());
BLACK_VERIFY_X(false, Q_FUNC_INFO, qUtf8Printable(m));
return 0;
}
QString ICoordinateGeodetic::convertToQString(bool i18n) const
{
const CLatitude lat = this->latitude();
const CLongitude lng = this->longitude();
return QStringLiteral("Geodetic: {%1/%2, %3/%4, %5}").arg(lat.valueRoundedWithUnit(CAngleUnit::deg(), 6, i18n),
lat.valueRoundedWithUnit(CAngleUnit::rad(), 6, i18n),
lng.valueRoundedWithUnit(CAngleUnit::deg(), 6, i18n),
lng.valueRoundedWithUnit(CAngleUnit::rad(), 6, i18n),
this->geodeticHeight().valueRoundedWithUnit(CLengthUnit::ft(), 2, i18n));
}
bool ICoordinateGeodetic::isNaNVector() const
{
const QVector3D v = this->normalVector();
return std::isnan(v.x()) || std::isnan(v.y()) || std::isnan(v.z());
}
bool ICoordinateGeodetic::isNaNVectorDouble() const
{
const std::array<double, 3> v = this->normalVectorDouble();
return std::isnan(v[0]) || std::isnan(v[1]) || std::isnan(v[2]);
}
bool ICoordinateGeodetic::isInfVector() const
{
const QVector3D v = this->normalVector();
return std::isinf(v.x()) || std::isinf(v.y()) || std::isinf(v.z());
}
bool ICoordinateGeodetic::isInfVectorDouble() const
{
const std::array<double, 3> v = this->normalVectorDouble();
return std::isinf(v[0]) || std::isinf(v[1]) || std::isinf(v[2]);
}
bool ICoordinateGeodetic::isValidVectorRange() const
{
// inf is out of range, comparing nans is always false
const std::array<double, 3> v = this->normalVectorDouble();
return isValidVector(v);
}
bool ICoordinateGeodetic::isValidVector(const std::array<double, 3> &v)
{
constexpr double l = 1.00001; // because of interpolation
return v[0] <= l && v[1] <= l && v[2] <= l &&
v[0] >= -l && v[1] >= -l && v[2] >= -l;
}
int CCoordinateGeodetic::clampVector()
{
int c = 0;
// *INDENT-OFF*
if (m_x < -1.0) { m_x = -1.0; c++; } else if (m_x > 1.0) { m_x = 1.0; c++; }
if (m_y < -1.0) { m_y = -1.0; c++; } else if (m_y > 1.0) { m_y = 1.0; c++; }
if (m_z < -1.0) { m_z = -1.0; c++; } else if (m_z > 1.0) { m_z = 1.0; c++; }
// *INDENT-ON*
return c;
}
CVariant CCoordinateGeodetic::propertyByIndex(const BlackMisc::CPropertyIndex &index) const
{
if (index.isMyself()) { return CVariant::from(*this); }
return (ICoordinateGeodetic::canHandleIndex(index)) ?
ICoordinateGeodetic::propertyByIndex(index) :
CValueObject::propertyByIndex(index);
}
void CCoordinateGeodetic::setPropertyByIndex(const CPropertyIndex &index, const CVariant &variant)
{
if (index.isMyself()) { (*this) = variant.to<CCoordinateGeodetic>(); return; }
const ICoordinateGeodetic::ColumnIndex i = index.frontCasted<ICoordinateGeodetic::ColumnIndex>();
switch (i)
{
case IndexGeodeticHeight: m_geodeticHeight.setPropertyByIndex(index.copyFrontRemoved(), variant); break;
case IndexLatitude: this->setLatitude(variant.value<CLatitude>()); break;
case IndexLongitude: this->setLongitude(variant.value<CLongitude>()); break;
case IndexLatitudeAsString: this->setLatitude(CLatitude::fromWgs84(variant.toQString())); break;
case IndexLongitudeAsString: this->setLongitude(CLongitude::fromWgs84(variant.toQString())); break;
case IndexGeodeticHeightAsString: m_geodeticHeight.parseFromString(variant.toQString()); break;
case IndexNormalVector: this->setNormalVector(variant.value<QVector3D>()); break;
default: CValueObject::setPropertyByIndex(index, variant); break;
}
}
int CCoordinateGeodetic::comparePropertyByIndex(const CPropertyIndex &index, const CCoordinateGeodetic &compareValue) const
{
return ICoordinateGeodetic::canHandleIndex(index) ?
ICoordinateGeodetic::comparePropertyByIndex(index, compareValue) :
CValueObject::comparePropertyByIndex(index, compareValue);
}
CCoordinateGeodetic::CCoordinateGeodetic(const std::array<double, 3> &normalVector)
{
this->setNormalVector(normalVector);
}
CCoordinateGeodetic::CCoordinateGeodetic(const CLatitude &latitude, const CLongitude &longitude) : CCoordinateGeodetic(latitude, longitude, CAltitude::null())
{
// void
}
CCoordinateGeodetic::CCoordinateGeodetic(const CLatitude &latitude, const CLongitude &longitude, const CAltitude &geodeticHeight) :
m_x(latitude.cos() * longitude.cos()),
m_y(latitude.cos() * longitude.sin()),
m_z(latitude.sin()),
m_geodeticHeight(geodeticHeight)
{}
CCoordinateGeodetic::CCoordinateGeodetic(double latitudeDegrees, double longitudeDegrees) :
CCoordinateGeodetic({ latitudeDegrees, CAngleUnit::deg() }, { longitudeDegrees, CAngleUnit::deg() }, { 0, nullptr }) {}
CCoordinateGeodetic::CCoordinateGeodetic(double latitudeDegrees, double longitudeDegrees, double heightFeet) :
CCoordinateGeodetic({ latitudeDegrees, CAngleUnit::deg() }, { longitudeDegrees, CAngleUnit::deg() }, { heightFeet, CLengthUnit::ft() }) {}
CCoordinateGeodetic::CCoordinateGeodetic(const ICoordinateGeodetic &coordinate) :
m_geodeticHeight(coordinate.geodeticHeight())
{
this->setNormalVector(coordinate.normalVectorDouble());
}
CCoordinateGeodetic CCoordinateGeodetic::calculatePosition(const CLength &distance, const CAngle &relBearing) const
{
if (this->isNull()) { return CCoordinateGeodetic::null(); }
if (distance.isNull() || distance.isNegativeWithEpsilonConsidered() || relBearing.isNull()) { return CCoordinateGeodetic::null(); }
if (distance.isZeroEpsilonConsidered()) { return *this; }
// http://www.movable-type.co.uk/scripts/latlong.html#destPoint
// https://stackoverflow.com/a/879531/356726
// https://www.cosmocode.de/en/blog/gohr/2010-06/29-calculate-a-destination-coordinate-based-on-distance-and-bearing-in-php
constexpr double earthRadiusMeters = 6371000.8;
const double startLatRad = this->latitude().value(CAngleUnit::rad());
const double startLngRad = this->longitude().value(CAngleUnit::rad());
const double bearingRad = relBearing.value(CAngleUnit::rad());
const double distRatio = distance.value(CLengthUnit::m()) / earthRadiusMeters;
const double newLatRad = asin(sin(startLatRad) * cos(distRatio) + cos(startLatRad) * sin(distRatio) * cos(bearingRad));
double newLngRad = 0;
constexpr double epsilon = 1E-06;
if (cos(newLatRad) == 0 || qAbs(cos(newLatRad)) < epsilon)
newLngRad = startLngRad;
else
{
// λ1 + Math.atan2(Math.sin(brng)*Math.sin(d/R)*Math.cos(φ1), Math.cos(d/R)-Math.sin(φ1)*Math.sin(φ2));
newLngRad = startLngRad + atan2(sin(bearingRad) * sin(distRatio) * cos(startLatRad), cos(distRatio) - sin(startLatRad) * sin(newLatRad));
newLngRad = fmod(newLngRad + 3 * M_PI, 2 * M_PI) - M_PI; // normalize +-180deg
}
CCoordinateGeodetic copy = *this;
const CLatitude lat(newLatRad, CAngleUnit::rad());
const CLongitude lng(newLngRad, CAngleUnit::rad());
copy.setLatLong(lat, lng);
return copy;
}
CLatitude CCoordinateGeodetic::latitude() const
{
return { std::atan2(m_z, std::hypot(m_x, m_y)), CAngleUnit::rad() };
}
CLongitude CCoordinateGeodetic::longitude() const
{
// in mathematics atan2 of 0,0 is undefined, with IEEE floating-point atan2(0,0) is either 0 or ±180°
return { std::atan2(m_y, m_x), CAngleUnit::rad() };
}
QVector3D CCoordinateGeodetic::normalVector() const
{
return
{
static_cast<float>(m_x),
static_cast<float>(m_y),
static_cast<float>(m_z)
};
}
std::array<double, 3> CCoordinateGeodetic::normalVectorDouble() const
{
return { { m_x, m_y, m_z } };
}
void CCoordinateGeodetic::setLatitude(const CLatitude &latitude)
{
this->setLatLong(latitude, this->longitude());
}
void CCoordinateGeodetic::setLatitudeFromWgs84(const QString &wgs)
{
this->setLatitude(CLatitude::fromWgs84(wgs));
}
void CCoordinateGeodetic::setLongitude(const CLongitude &longitude)
{
this->setLatLong(this->latitude(), longitude);
}
void CCoordinateGeodetic::setLongitudeFromWgs84(const QString &wgs)
{
this->setLongitude(CLongitude::fromWgs84(wgs));
}
void CCoordinateGeodetic::setLatLong(const CLatitude &latitude, const CLongitude &longitude)
{
m_x = latitude.cos() * longitude.cos();
m_y = latitude.cos() * longitude.sin();
m_z = latitude.sin();
}
void CCoordinateGeodetic::setLatLongFromWgs84(const QString &latitude, const QString &longitude)
{
this->setLatitudeFromWgs84(latitude);
this->setLongitudeFromWgs84(longitude);
}
void CCoordinateGeodetic::setGeodeticHeightToNull()
{
this->setGeodeticHeight(CAltitude::null());
}
void CCoordinateGeodetic::setNormalVector(const std::array<double, 3> &normalVector)
{
Q_ASSERT_X(normalVector.size() == 3, Q_FUNC_INFO, "Wrong vector size");
m_x = normalVector[0];
m_y = normalVector[1];
m_z = normalVector[2];
}
CCoordinateGeodetic &CCoordinateGeodetic::switchUnit(const CLengthUnit &unit)
{
m_geodeticHeight.switchUnit(unit);
return *this;
}
CLength ICoordinateWithRelativePosition::calculcateAndUpdateRelativeDistance(const ICoordinateGeodetic &position)
{
m_relativeDistance = Geo::calculateGreatCircleDistance(*this, position);
return m_relativeDistance;
}
CLength ICoordinateWithRelativePosition::calculcateAndUpdateRelativeDistanceAndBearing(const ICoordinateGeodetic &position)
{
m_relativeDistance = Geo::calculateGreatCircleDistance(*this, position);
m_relativeBearing = Geo::calculateBearing(*this, position);
return m_relativeDistance;
}
CVariant ICoordinateWithRelativePosition::propertyByIndex(const CPropertyIndex &index) const
{
if (ICoordinateGeodetic::canHandleIndex(index)) { return ICoordinateGeodetic::propertyByIndex(index); }
if (!index.isMyself())
{
const ColumnIndex i = index.frontCasted<ColumnIndex>();
switch (i)
{
case IndexRelativeBearing: return this->getRelativeBearing().propertyByIndex(index.copyFrontRemoved());
case IndexRelativeDistance: return this->getRelativeDistance().propertyByIndex(index.copyFrontRemoved());
default: break;
}
}
const QString m = QString("no property, index ").append(index.toQString());
BLACK_VERIFY_X(false, Q_FUNC_INFO, qUtf8Printable(m));
return CVariant::fromValue(m);
}
void ICoordinateWithRelativePosition::setPropertyByIndex(const CPropertyIndex &index, const CVariant &variant)
{
if (ICoordinateGeodetic::canHandleIndex(index)) { return; }
if (!index.isMyself())
{
const ColumnIndex i = index.frontCasted<ColumnIndex>();
switch (i)
{
case IndexRelativeBearing: m_relativeBearing.setPropertyByIndex(index.copyFrontRemoved(), variant); break;
case IndexRelativeDistance: m_relativeDistance.setPropertyByIndex(index.copyFrontRemoved(), variant); break;
default:
const QString m = QString("no property, index ").append(index.toQString());
BLACK_VERIFY_X(false, Q_FUNC_INFO, qUtf8Printable(m));
break;
}
}
}
int ICoordinateWithRelativePosition::comparePropertyByIndex(const CPropertyIndex &index, const ICoordinateWithRelativePosition &compareValue) const
{
if (ICoordinateGeodetic::canHandleIndex(index)) { return ICoordinateGeodetic::comparePropertyByIndex(index, compareValue); }
if (!index.isMyself())
{
const ColumnIndex i = index.frontCasted<ColumnIndex>();
switch (i)
{
case IndexRelativeBearing: return m_relativeBearing.comparePropertyByIndex(index.copyFrontRemoved(), compareValue.getRelativeBearing());
case IndexRelativeDistance: return m_relativeDistance.comparePropertyByIndex(index.copyFrontRemoved(), compareValue.getRelativeDistance());
default:
const QString m = QString("no property, index ").append(index.toQString());
Q_ASSERT_X(false, Q_FUNC_INFO, m.toLocal8Bit().constData());
break;
}
}
return 0;
}
QString ICoordinateWithRelativePosition::convertToQString(bool i18n) const
{
return m_relativeBearing.toQString(i18n) % u' ' %
m_relativeDistance.toQString(i18n) % u' ' %
ICoordinateGeodetic::convertToQString(i18n);
}
ICoordinateWithRelativePosition::ICoordinateWithRelativePosition()
{ }
bool ICoordinateWithRelativePosition::canHandleIndex(const CPropertyIndex &index)
{
if (ICoordinateGeodetic::canHandleIndex(index)) { return true; }
const int i = index.frontCasted<int>();
return (i >= static_cast<int>(IndexRelativeDistance)) && (i <= static_cast<int>(IndexRelativeBearing));
}
} // namespace
} // namespace