qt6-migration-pipeline/Lts/Common/LtsCoordTrans/lts_coord_trans.cpp

#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;   }

}