GeodeticUtils.cpp

#include "GeodeticUtils.h"
 
#include <string>
#include <cmath>
#include <glm/glm.hpp>
 
namespace {
  struct Ellipsoid
  {
    int         id;
    std::string name;
    double      equatorialRadius;
    double      eccentricitySquared;
  };
 
  Ellipsoid ellipsoids[] = {
    {-1, "Placeholder", 0, 0},//placeholder only, To allow array indices to match id numbers
    {1, "Airy", 6377563, 0.00667054},
    {2, "Australian National", 6378160, 0.006694542},
    {3, "Bessel 1841", 6377397, 0.006674372},
    {4, "Bessel 1841 (Nambia) ", 6377484, 0.006674372},
    {5, "Clarke 1866", 6378206, 0.006768658},
    {6, "Clarke 1880", 6378249, 0.006803511},
    {7, "Everest", 6377276, 0.006637847},
    {8, "Fischer 1960 (Mercury) ", 6378166, 0.006693422},
    {9, "Fischer 1968", 6378150, 0.006693422},
    {10, "GRS 1967", 6378160, 0.006694605},
    {11, "GRS 1980", 6378137, 0.00669438},
    {12, "Helmert 1906", 6378200, 0.006693422},
    {13, "Hough", 6378270, 0.00672267},
    {14, "International", 6378388, 0.00672267},
    {15, "Krassovsky", 6378245, 0.006693422},
    {16, "Modified Airy", 6377340, 0.00667054},
    {17, "Modified Everest", 6377304, 0.006637847},
    {18, "Modified Fischer 1960", 6378155, 0.006693422},
    {19, "South American 1969", 6378160, 0.006694542},
    {20, "WGS 60", 6378165, 0.006693422},
    {21, "WGS 66", 6378145, 0.006694542},
    {22, "WGS-72", 6378135, 0.006694318},
    {23, "WGS-84", 6378137, 0.00669438}
  };
}
 
void LLtoUTMZone(int& zoneNumber,
                 bool& northernHemisphere,
                 const double latitude,
                 const double longitude,
                 const int /*referenceEllipsoid*/)
{
  //converts lat/long to UTM coords.  Equations from USGS Bulletin 1532 
  //East Longitudes are positive, West longitudes are negative. 
  //North latitudes are positive, South latitudes are negative
  //Lat and Long are in decimal degrees
  //Original C++ version Written by Chuck Gantz- chuck.gantz@globalstar.com
 
  //Make sure the longitude is between -180.00 .. 179.9
  const double LongTemp = (longitude + 180) - (int)((longitude + 180) / 360) * 360 - 180; // -180.00 .. 179.9;
 
  zoneNumber = (int)((LongTemp + 180) / 6) + 1;
 
  if (latitude >= 56.0 && latitude < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0)
    zoneNumber = 32;
 
  // Special zones for Svalbard
  if (latitude >= 72.0 && latitude < 84.0)
  {
    if (LongTemp >= 0.0 && LongTemp < 9.0) zoneNumber = 31;
    else if (LongTemp >= 9.0 && LongTemp < 21.0) zoneNumber = 33;
    else if (LongTemp >= 21.0 && LongTemp < 33.0) zoneNumber = 35;
    else if (LongTemp >= 33.0 && LongTemp < 42.0) zoneNumber = 37;
  }
 
  northernHemisphere = latitude >= 0;
}
 
 
void LLtoUTM(double& easting,
             double& northing,
             const double latitude,
             const double longitude,
             int zone,
             bool north,
             const int referenceEllipsoid)
{
  //converts lat/long to UTM coords.  Equations from USGS Bulletin 1532 
  //East Longitudes are positive, West longitudes are negative. 
  //North latitudes are positive, South latitudes are negative
  //Lat and Long are in decimal degrees
  //Original C++ version Written by Chuck Gantz- chuck.gantz@globalstar.com
 
  double a = ellipsoids[referenceEllipsoid].equatorialRadius;
  double eccSquared = ellipsoids[referenceEllipsoid].eccentricitySquared;
  double k0 = 0.9996;
 
  double eccPrimeSquared;
  double N, T, C, A, M;
 
  //Make sure the longitude is between -180.00 .. 179.9
  double LongTemp = (longitude + 180) - (int)((longitude + 180) / 360) * 360 - 180; // -180.00 .. 179.9;
 
  double LatRad = glm::radians(latitude);
  double LongRad = glm::radians(LongTemp);
 
  if (zone == -1) {
    LLtoUTMZone(zone, north, latitude, longitude, referenceEllipsoid);
  }
 
  const double LongOrigin = (zone - 1) * 6 - 180 + 3;  //+3 puts origin in middle of zone
  const double LongOriginRad = glm::radians(LongOrigin);
 
  eccPrimeSquared = (eccSquared) / (1 - eccSquared);
 
  N = a / std::sqrt(1 - eccSquared * std::sin(LatRad) * std::sin(LatRad));
  T = std::tan(LatRad) * std::tan(LatRad);
  C = eccPrimeSquared * std::cos(LatRad) * std::cos(LatRad);
  A = std::cos(LatRad) * (LongRad - LongOriginRad);
 
  M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * LatRad
           - (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * std::sin(2 * LatRad)
           + (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * std::sin(4 * LatRad)
           - (35 * eccSquared * eccSquared * eccSquared / 3072) * std::sin(6 * LatRad));
 
  easting = (double)(k0 * N * (A + (1 - T + C) * A * A * A / 6
                               + (5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * A * A / 120)
                     + 500000.0);
 
  northing = (double)(k0 * (M + N * std::tan(LatRad) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24
                                                        + (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
  if (!north)
    northing += 10000000.0; //10000000 meter offset for southern hemisphere
}
 
glm::dvec3 LLtoECEF(double latitude,
                    double longitude,
                    double altitude) {
  constexpr double semimajorAxis = 6378137.0;
  constexpr double semiminorAxis = 6356752.314245;
 
  latitude = glm::radians(latitude);
  longitude = glm::radians(longitude);
 
  double sinLat = std::sin(latitude);
  double cosLat = std::cos(latitude);
  double sinLong = std::sin(longitude);
  double cosLong = std::cos(longitude);
 
  constexpr double fllipsoidFlatness = ((semimajorAxis - semiminorAxis) / semimajorAxis);
  constexpr double firstEccentricitySquared = (fllipsoidFlatness * (2.0 - fllipsoidFlatness));
 
  double n = semimajorAxis / std::sqrt(1.0 - firstEccentricitySquared * sinLat * sinLat);
  return glm::dvec3((altitude + n) * cosLat * cosLong,
                    (altitude + n) * cosLat * sinLong,
                    (altitude + (1.0 - firstEccentricitySquared) * n) * sinLat);
}
 
glm::dvec2 calcUTMVectorBetweenCoordinates(double lat1, double long1, double lat2, double long2, int zone1, int zone2) {
  bool  north1 = lat1 >= 0.0;
  bool  north2 = lat2 >= 0.0;
 
  if (zone1 == -1) {
    LLtoUTMZone(zone1, north1, lat1, long1);
  }
  if (zone2 == -1) {
    LLtoUTMZone(zone2, north2, lat2, long2);
  }
 
  if (zone1 == zone2) {
    double  easting1;
    double  northing1;
    double  easting2;
    double  northing2;
 
    LLtoUTM(easting1, northing1, lat1, long1, zone1, north1);
    LLtoUTM(easting2, northing2, lat2, long2, zone2, north2);
 
    if (north1 == north2) {
      return glm::dvec2(static_cast<float>(easting2 - easting1), static_cast<float>(northing2 - northing1));
    }
    else if (!north1) {
      return glm::dvec2(static_cast<float>(easting2 - easting1), static_cast<float>(northing2 - (northing1 - 10000000.0)));
    }
    else {
      return glm::dvec2(static_cast<float>(easting2 - easting1), static_cast<float>((northing2 - 10000000.0) - northing1));
    }
  }
  return glm::dvec2();
}
 
float calcBearingBetweenCoordinates(double lat1, double long1, double lat2, double long2, int zone) {
  bool  north = lat2 >= 0.0;
 
  if (zone == -1) {
    LLtoUTMZone(zone, north, lat2, long2);
  }
 
  lat1 = glm::radians(lat1);
  long1 = glm::radians(long1);
  lat2 = glm::radians(lat2);
  long2 = glm::radians(long2);
 
  double  y = std::sin(long2-long1) * std::cos(lat2);
  double  x = (std::cos(lat1) * std::sin(lat2)) - (std::sin(lat1) * std::cos(lat2) * std::cos(long2 - long1));
  double  meridian = glm::radians((zone * 6.0) - 183.0);
  double  convergence = glm::degrees(std::atan(std::tan(long2 - meridian) * std::sin(lat2)));
 
  return static_cast<float>(std::fmod(glm::degrees(std::atan2(y, x)) - convergence + 360.0, 360.0));
}
 
Cogs::LLToENUConverter::LLToENUConverter(double latitude, double longitude, double altitude) {
  ecefRef = LLtoECEF(latitude, longitude, altitude);
 
  latitude = glm::radians(latitude);
  longitude = glm::radians(longitude);
 
  double sinLat = std::sin(latitude);
  double cosLat = std::cos(latitude);
  double sinLong = std::sin(longitude);
  double cosLong = std::cos(longitude);
 
  ecefToENUMatrix = glm::dmat3x3(-sinLong, -sinLat * cosLong, cosLat * cosLong,
                                  cosLong, -sinLat * sinLong, cosLat * sinLong,
                                  0.0,      cosLat,           sinLat);
}
 
glm::dvec3 Cogs::LLToENUConverter::convert(double latitude, double longitude, double altitude) const {
  return ecefToENUMatrix * (LLtoECEF(latitude, longitude, altitude) - ecefRef);
}
 
glm::dvec2 Cogs::LLToENUConverter::calcVectorBetweenCoordinates(double lat1, double long1, double lat2, double long2) const {
  return convert(lat2, long2) - convert(lat1, long1);
}