Continues migration with DDS, Common
This commit is contained in:
torsten committed
1 parent
9b15cee841
commit
fa48db111e
668 files changed
+111841
-2
No files matched your search
Executable
+445
@@ -0,0 +1,445 @@
|
||||
|
||||
|
||||
|
||||
#include "lts_coord_trans.h"
|
||||
|
||||
#include <GeographicLib/Geocentric.hpp>
|
||||
#include <GeographicLib/UTMUPS.hpp>
|
||||
#include <GeographicLib/MGRS.hpp>
|
||||
#include <QPointF>
|
||||
#include <QRectF>
|
||||
#include <QString>
|
||||
#include <QStringList>
|
||||
|
||||
|
||||
#include <iomanip>
|
||||
|
||||
namespace LtsCoordTrans
|
||||
{
|
||||
|
||||
static long s_conversion_counter_MGRS_Forward = 0;
|
||||
static long s_conversion_counter_MGRS_Reverse = 0;
|
||||
|
||||
static long s_conversion_counter_WGS84_Forward = 0;
|
||||
static long s_conversion_counter_WGS84_Reverse = 0;
|
||||
|
||||
static long s_conversion_counter_UTMUPS_Forward = 0;
|
||||
static long s_conversion_counter_UTMUPS_Reverse = 0;
|
||||
|
||||
|
||||
inline void incrementCounter(long &counterRef)
|
||||
{
|
||||
#ifdef LTS_DEBUG
|
||||
++counterRef;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
static inline std::string cleanupMgrsString(const QString &in)
|
||||
{
|
||||
const QByteArray ba(in.toLatin1());
|
||||
std::string ret;
|
||||
ret.reserve(ba.size());
|
||||
for (const char c : ba)
|
||||
{
|
||||
if (c != ' ')
|
||||
ret += c;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
QString UtmCoordinate::mgrs(int prec, bool withSpaces) const
|
||||
{
|
||||
if (prec < 0)
|
||||
prec = 0;
|
||||
if (prec > 11)
|
||||
prec = 11;
|
||||
|
||||
std::string mgrs;
|
||||
try
|
||||
{
|
||||
GeographicLib::MGRS::Forward(zone, northp, x, y, prec, mgrs);
|
||||
incrementCounter(s_conversion_counter_MGRS_Forward);
|
||||
}
|
||||
catch (const std::exception &e)
|
||||
{
|
||||
fprintf(stderr, "Error while calculating mgrs from utm %d, %9.6f, %9.6f: %s\n", zone, x, y, e.what());
|
||||
mgrs.clear();
|
||||
}
|
||||
|
||||
QString ret;
|
||||
if (withSpaces && mgrs.size() == static_cast<size_t>(5 + 2 * prec))
|
||||
{
|
||||
|
||||
const char *ptr = mgrs.c_str();
|
||||
|
||||
ret = QString::fromLatin1(ptr, 3) + QLatin1Char(' ') +
|
||||
QString::fromLatin1(ptr + 3, 2);
|
||||
if (prec > 0)
|
||||
{
|
||||
ret += QLatin1Char(' ') + QString::fromLatin1(ptr + 5, prec) +
|
||||
QLatin1Char(' ') + QString::fromLatin1(ptr + 5 + prec, prec);
|
||||
}
|
||||
}
|
||||
else
|
||||
ret = QString::fromLatin1(mgrs.c_str(), mgrs.size());
|
||||
return ret;
|
||||
}
|
||||
|
||||
QPointF UtmCoordinate::toPoint() const
|
||||
{
|
||||
return {x,y};
|
||||
}
|
||||
|
||||
QPointF UtmCoordinate::geographic(double offsetX, double offsetY) const
|
||||
{
|
||||
QPointF ret;
|
||||
try
|
||||
{
|
||||
GeographicLib::UTMUPS::Reverse(zone, northp, x + offsetX, y + offsetY, ret.ry(), ret.rx());
|
||||
incrementCounter(s_conversion_counter_UTMUPS_Reverse);
|
||||
}
|
||||
catch (const std::exception &e)
|
||||
{
|
||||
fprintf(stderr, "Error while calculating geographic from utm %d, %9.6f, %9.6f: %s\n", zone, x + offsetX, y + offsetY, e.what());
|
||||
ret = QPointF();
|
||||
}
|
||||
return ret * deg2rad();
|
||||
}
|
||||
|
||||
QPointF UtmCoordinate::geographic(const QPointF &offset) const
|
||||
{
|
||||
return geographic(offset.x(), offset.y());
|
||||
}
|
||||
|
||||
QPointF UtmCoordinate::relativeTo(const UtmCoordinate &other) const
|
||||
{
|
||||
if (zone == other.zone)
|
||||
return QPointF(x - other.x, y - other.y);
|
||||
|
||||
UtmCoordinate otherInMyZone = fromGeographic(other.geographic(), zone);
|
||||
return QPointF(x - otherInMyZone.x, y - otherInMyZone.y);
|
||||
}
|
||||
|
||||
UtmCoordinate UtmCoordinate::fromGeographic(double lon, double lat, int setZone)
|
||||
{
|
||||
|
||||
lon *= rad2deg();
|
||||
lat *= rad2deg();
|
||||
|
||||
UtmCoordinate ret;
|
||||
try
|
||||
{
|
||||
|
||||
GeographicLib::UTMUPS::Forward(lat, lon, ret.zone, ret.northp, ret.x, ret.y, setZone);
|
||||
incrementCounter(s_conversion_counter_UTMUPS_Forward);
|
||||
}
|
||||
catch (const std::exception &e)
|
||||
{
|
||||
fprintf(stderr, "Error while calculating mgrs base point from %9.6f, %9.6f: %s\n", lat, lon, e.what());
|
||||
ret = UtmCoordinate();
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
UtmCoordinate UtmCoordinate::fromGeographic(const QPointF &lonLat, int setZone)
|
||||
{
|
||||
return fromGeographic(lonLat.x(), lonLat.y(), setZone);
|
||||
}
|
||||
|
||||
UtmCoordinate UtmCoordinate::fromMgrs(const QString &mgrs)
|
||||
{
|
||||
UtmCoordinate utm;
|
||||
try
|
||||
{
|
||||
int prec;
|
||||
|
||||
GeographicLib::MGRS::Reverse(cleanupMgrsString(mgrs), utm.zone, utm.northp, utm.x, utm.y, prec, true);
|
||||
incrementCounter(s_conversion_counter_MGRS_Reverse);
|
||||
|
||||
}
|
||||
catch (const std::exception &e)
|
||||
{
|
||||
fprintf(stderr, "Error while converting mgrs coordinate %s to utm: %s\n", qPrintable(mgrs), e.what());
|
||||
utm = UtmCoordinate();
|
||||
}
|
||||
return utm;
|
||||
}
|
||||
|
||||
QString mgrsFromGeographic(double lon, double lat, int prec, bool withSpaces)
|
||||
{
|
||||
return UtmCoordinate::fromGeographic(lon, lat, -1).mgrs(prec, withSpaces);
|
||||
}
|
||||
|
||||
QPointF geographicFromMgrs(const QString &mgrs)
|
||||
{
|
||||
return UtmCoordinate::fromMgrs(mgrs).geographic();
|
||||
}
|
||||
|
||||
QVector<UtmCoordinate> &staticDefaultSquaresHelper()
|
||||
{
|
||||
static QVector<UtmCoordinate> s_defaultMgrsSquares;
|
||||
return s_defaultMgrsSquares;
|
||||
}
|
||||
UtmCoordinate &staticPreferedMgrsSquareHelper()
|
||||
{
|
||||
static UtmCoordinate s_preferedMgrsSquare;
|
||||
return s_preferedMgrsSquare;
|
||||
}
|
||||
QVector<UtmCoordinate> &staticMgrsSquaresWithOffsetsHelper()
|
||||
{
|
||||
static QVector<UtmCoordinate> s_mgrsSquaresWithOffsets;
|
||||
return s_mgrsSquaresWithOffsets;
|
||||
}
|
||||
|
||||
|
||||
QRectF addMarginToArea(const QRectF &rect_LatLon_radiant, const QSize &marginMeter)
|
||||
{
|
||||
double lonMin, lonMax, latMin, latMmax;
|
||||
rect_LatLon_radiant.normalized().getCoords(&lonMin, &latMin, &lonMax, &latMmax);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
LtsCoordTrans::UtmCoordinate basePoint;
|
||||
{
|
||||
|
||||
const QPointF centerRad = rect_LatLon_radiant.center();
|
||||
QString mgrs = LtsCoordTrans::mgrsFromGeographic(centerRad.x(), centerRad.y(), -1);
|
||||
mgrs += QStringLiteral("00000000000000");
|
||||
|
||||
|
||||
basePoint = LtsCoordTrans::UtmCoordinate::fromMgrs(mgrs);
|
||||
}
|
||||
|
||||
const UtmCoordinate lowerLeft(UtmCoordinate::fromGeographic(lonMin, latMin, basePoint.zone));
|
||||
const UtmCoordinate upperRight(UtmCoordinate::fromGeographic(lonMax, latMmax, basePoint.zone));
|
||||
|
||||
int offsetX = marginMeter.width();
|
||||
int offsetY = marginMeter.height();
|
||||
|
||||
const QPointF topLeft(lowerLeft.geographic(-offsetX, -offsetY));
|
||||
const QPointF bottomRight(upperRight.geographic(offsetX, offsetY));
|
||||
|
||||
QRectF res = QRectF(topLeft, bottomRight);
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
QVector<UtmCoordinate> getMgrsSquares(const QRectF &rect, bool setAsDefaultSquares, bool setAsPreferedSquare, bool setAsSquareWithMargin)
|
||||
{
|
||||
QVector<UtmCoordinate> ret = getMgrsSquaresInternal(rect);
|
||||
|
||||
if (setAsDefaultSquares)
|
||||
staticDefaultSquaresHelper() = ret;
|
||||
|
||||
if (setAsPreferedSquare)
|
||||
staticPreferedMgrsSquareHelper() = LtsCoordTrans::UtmCoordinate::fromGeographic(rect.center());
|
||||
|
||||
if (setAsSquareWithMargin)
|
||||
staticMgrsSquaresWithOffsetsHelper() = ret;
|
||||
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
QVector<UtmCoordinate> getMgrsSquaresInternal(const QRectF &rect)
|
||||
{
|
||||
QVector<UtmCoordinate> ret;
|
||||
QVector<QString> tmpMgrsSquares;
|
||||
|
||||
|
||||
double lonMin, lonMax, latMin, latMmax;
|
||||
rect.normalized().getCoords(&lonMin, &latMin, &lonMax, &latMmax);
|
||||
|
||||
|
||||
bool withInSingleUtmSquare = true;
|
||||
{
|
||||
const UtmCoordinate upperRight(UtmCoordinate::fromGeographic(lonMax, latMmax));
|
||||
const UtmCoordinate lowerLeft(UtmCoordinate::fromGeographic(lonMin, latMin));
|
||||
withInSingleUtmSquare = upperRight.zone == lowerLeft.zone;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
LtsCoordTrans::UtmCoordinate basePoint;
|
||||
{
|
||||
|
||||
QPointF centerRad = rect.center();
|
||||
QString mgrs = LtsCoordTrans::mgrsFromGeographic(centerRad.x(), centerRad.y(), -1);
|
||||
mgrs += QStringLiteral("00000000000000");
|
||||
|
||||
|
||||
basePoint = LtsCoordTrans::UtmCoordinate::fromMgrs(mgrs);
|
||||
}
|
||||
|
||||
const UtmCoordinate upperRight(UtmCoordinate::fromGeographic(lonMax, latMmax, basePoint.zone));
|
||||
const UtmCoordinate lowerLeft(UtmCoordinate::fromGeographic(lonMin, latMin, basePoint.zone));
|
||||
|
||||
const double maxX(upperRight.x - lowerLeft.x);
|
||||
const double maxY(upperRight.y - lowerLeft.y);
|
||||
|
||||
|
||||
|
||||
const int increment = withInSingleUtmSquare ? 10000 : 1000;
|
||||
for (double x = 0; x <= maxX + increment; x += increment)
|
||||
{
|
||||
const double xOfs = std::min(x, maxX);
|
||||
for (double y = 0; y <= maxY + increment; y += increment)
|
||||
{
|
||||
const double yOfs = std::min(y, maxY);
|
||||
|
||||
const UtmCoordinate utm(UtmCoordinate::fromGeographic(lowerLeft.geographic(xOfs, yOfs)));
|
||||
const QString mgrs(utm.mgrs(0));
|
||||
|
||||
if (!tmpMgrsSquares.contains(mgrs))
|
||||
{
|
||||
tmpMgrsSquares += mgrs;
|
||||
ret += utm;
|
||||
}
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
QStringList getMgrsSurroundingSquares(const QRectF &geoRect, const QSize &offsetMeter)
|
||||
{
|
||||
|
||||
const QVector<UtmCoordinate> mainUtms = getMgrsSquaresInternal(geoRect);
|
||||
QStringList mainSquares;
|
||||
mainSquares.reserve(mainUtms.count());
|
||||
for (const auto &utm : mainUtms)
|
||||
mainSquares.append(utm.mgrs(0));
|
||||
|
||||
|
||||
const QRectF rectWithOffset = addMarginToArea(geoRect,offsetMeter);
|
||||
|
||||
QVector<UtmCoordinate> mainUtmsWithOffset = getMgrsSquaresInternal(rectWithOffset);
|
||||
QStringList mainSquaresWithOffset;
|
||||
mainSquaresWithOffset.reserve(mainUtmsWithOffset.count());
|
||||
for (const auto &utm : qAsConst(mainUtmsWithOffset))
|
||||
mainSquaresWithOffset.append(utm.mgrs(0));
|
||||
|
||||
|
||||
QStringList surroundingSquares;
|
||||
surroundingSquares.reserve(mainSquaresWithOffset.count());
|
||||
for (const auto & mgrs0 : mainSquaresWithOffset)
|
||||
{
|
||||
if (!mainSquares.contains(mgrs0))
|
||||
surroundingSquares.append(mgrs0);
|
||||
}
|
||||
|
||||
return surroundingSquares;
|
||||
}
|
||||
|
||||
QVector<UtmCoordinate> defaultMgrsSquareWithOffsets()
|
||||
{
|
||||
return staticMgrsSquaresWithOffsetsHelper();
|
||||
}
|
||||
|
||||
|
||||
void setDefaultMgrsSquares(const QVector<QString> &squares)
|
||||
{
|
||||
QVector<UtmCoordinate> coords;
|
||||
coords.reserve(squares.size());
|
||||
for (const QString &s : squares)
|
||||
coords += UtmCoordinate::fromMgrs(s + QStringLiteral("00000000000000"));
|
||||
staticDefaultSquaresHelper() = coords;
|
||||
}
|
||||
|
||||
QVector<UtmCoordinate> defaultMgrsSquares()
|
||||
{
|
||||
return staticDefaultSquaresHelper();
|
||||
}
|
||||
|
||||
UtmCoordinate preferedMgrsSquare()
|
||||
{
|
||||
return staticPreferedMgrsSquareHelper();
|
||||
}
|
||||
|
||||
|
||||
bool lonLatAltToGeocentric(double lon, double lat, double altitude, double &xOut, double &yOut, double &zOut)
|
||||
{
|
||||
|
||||
lon *= rad2deg();
|
||||
lat *= rad2deg();
|
||||
|
||||
try
|
||||
{
|
||||
GeographicLib::Geocentric::WGS84().Forward(lat, lon, altitude, xOut, yOut, zOut);
|
||||
incrementCounter(s_conversion_counter_WGS84_Forward);
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
xOut = 0;
|
||||
yOut = 0;
|
||||
zOut = 0;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool geocentric2LonLatAlt(double x, double y, double z,
|
||||
double &lon, double &lat, double &altitude)
|
||||
{
|
||||
|
||||
|
||||
try
|
||||
{
|
||||
GeographicLib::Geocentric::WGS84().Reverse(x, y, z, lat, lon, altitude);
|
||||
incrementCounter(s_conversion_counter_WGS84_Reverse);
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
lon = 0;
|
||||
lat = 0;
|
||||
altitude = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
lon *= deg2rad();
|
||||
lat *= deg2rad();
|
||||
return true;
|
||||
}
|
||||
|
||||
double distance(const QPointF &p1, const QPointF &p2)
|
||||
{
|
||||
return distance(p1.x(), p1.y(), p2.x(), p2.y());
|
||||
}
|
||||
|
||||
double squareDistance(const QPointF &p1, const QPointF &p2)
|
||||
{
|
||||
return squareDistance(p1.x(), p1.y(), p2.x(), p2.y());
|
||||
}
|
||||
|
||||
std::ostream &operator<< (std::ostream &stream,
|
||||
const UtmCoordinate &coord)
|
||||
{
|
||||
|
||||
stream << "UtmCoordinate(" << std::setprecision(8) << coord.x
|
||||
<< "#" << std::setprecision(8) << coord.y
|
||||
<< " zone=" << coord.zone << ")";
|
||||
return stream;
|
||||
}
|
||||
|
||||
void dumpConversionCounter()
|
||||
{
|
||||
fprintf(stderr, "LtsCoordTrans: s_conversion_counter_MGRS_Forward : - mgrs : %ld\n" , s_conversion_counter_MGRS_Forward);
|
||||
fprintf(stderr, "LtsCoordTrans: s_conversion_counter_MGRS_Reverse - fromMgrs : %ld\n" , s_conversion_counter_MGRS_Reverse);
|
||||
fprintf(stderr, "LtsCoordTrans: s_conversion_counter_WGS84_Forward - lonLatAltToGeocentric : %ld\n" , s_conversion_counter_WGS84_Forward);
|
||||
fprintf(stderr, "LtsCoordTrans: s_conversion_counter_WGS84_Reverse - geocentric2LonLatAlt : %ld\n" , s_conversion_counter_WGS84_Reverse);
|
||||
fprintf(stderr, "LtsCoordTrans: s_conversion_counter_UTMUPS_Forward - fromGeographic : %ld\n" , s_conversion_counter_UTMUPS_Forward);
|
||||
fprintf(stderr, "LtsCoordTrans: s_conversion_counter_UTMUPS_Reverse - geographic : %ld\n" , s_conversion_counter_UTMUPS_Reverse);
|
||||
}
|
||||
|
||||
long getConversionCounter_MGRS_Forward() { return s_conversion_counter_MGRS_Forward; }
|
||||
long getConversionCounter_MGRS_Reverse() { return s_conversion_counter_MGRS_Reverse; }
|
||||
long getConversionCounter_WGS84_Forward() { return s_conversion_counter_WGS84_Forward; }
|
||||
long getConversionCounter_WGS84_Reverse() { return s_conversion_counter_WGS84_Reverse; }
|
||||
long getConversionCounter_UTMUPS_Forward() { return s_conversion_counter_UTMUPS_Forward; }
|
||||
long getConversionCounter_UTMUPS_Reverse() { return s_conversion_counter_UTMUPS_Reverse; }
|
||||
|
||||
}
|
||||
Reference in new issue
Block a user