src/Geo/UTM.cpp

   1 /*
   2 Copyright_License {
   3
   4   XCSoar Glide Computer - http://www.xcsoar.org/
   5   Copyright (C) 2000-2013 The XCSoar Project
   6   A detailed list of copyright holders can be found in the file "AUTHORS".
   7
   8   This program is free software; you can redistribute it and/or
   9   modify it under the terms of the GNU General Public License
  10   as published by the Free Software Foundation; either version 2
  11   of the License, or (at your option) any later version.
  12
  13   This program is distributed in the hope that it will be useful,
  14   but WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16   GNU General Public License for more details.
  17
  18   You should have received a copy of the GNU General Public License
  19   along with this program; if not, write to the Free Software
  20   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  21 }
  22 */
  23
  24 #include "Geo/UTM.hpp"
  25 #include "Geo/GeoPoint.hpp"
  26 #include "Util/Macros.hpp"
  27
  28 static constexpr double k0 = 0.9996;
  29
  30 static constexpr double e = 0.00669438;
  31 static constexpr double e2 = e * e;
  32 static constexpr double e3 = e * e;
  33 static constexpr double e_p2 = e / (1.0 - e);
  34
  35 static constexpr double r = 6378137;
  36
  37 char
  38 UTM::CalculateZoneLetter(const Angle latitude)
  39 {
  40   static constexpr char letters[] = "CDEFGHJKLMNPQRSTUVWXX";
  41   unsigned index = (unsigned)((latitude.Degrees() + fixed(80)) / 8);
  42   return (index < ARRAY_SIZE(letters)) ? letters[index] : '\0';
  43 }
  44
  45 unsigned char
  46 UTM::CalculateZoneNumber(const GeoPoint &p)
  47 {
  48   if (p.latitude.Degrees() <= fixed(64) &&
  49       p.latitude.Degrees() >= fixed(56) &&
  50       p.longitude.Degrees() <= fixed(12) &&
  51       p.longitude.Degrees() >= fixed(3))
  52     return 32;
  53
  54   if (p.latitude.Degrees() <= fixed(84) &&
  55       p.latitude.Degrees() >= fixed(72) &&
  56       p.longitude.Degrees() >= fixed(0)) {
  57     if (p.longitude.Degrees() <= fixed(9))
  58       return 31;
  59     if (p.longitude.Degrees() <= fixed(21))
  60       return 33;
  61     if (p.longitude.Degrees() <= fixed(33))
  62       return 35;
  63     if (p.longitude.Degrees() <= fixed(42))
  64       return 37;
  65   }
  66
  67   return (int)floor((p.longitude.Degrees() + fixed(180)) / 6) + 1;
  68 }
  69
  70 Angle
  71 UTM::GetCentralMeridian(unsigned char zone_number)
  72 {
  73   return Angle::Degrees(fixed((zone_number - 1) * 6 - 180 + 3));
  74 }
  75
  76 UTM
  77 UTM::FromGeoPoint(const GeoPoint &p) {
  78   double lat = (double)p.latitude.Radians();
  79   double _sin = (double)p.latitude.sin();
  80   double _cos = (double)p.latitude.cos();
  81   double _tan = _sin / _cos;
  82   double tan2 = _tan * _tan;
  83   double tan4 = tan2 * tan2;
  84
  85   UTM utm;
  86   utm.zone_number = CalculateZoneNumber(p);
  87   utm.zone_letter = CalculateZoneLetter(p.latitude);
  88
  89   double n = r / sqrt(1 - e * _sin * _sin);
  90   double c = e_p2 * _cos * _cos;
  91
  92   double a = _cos * (double)(p.longitude.Radians() -
  93                              GetCentralMeridian(utm.zone_number).Radians());
  94   double a2 = a * a;
  95   double a3 = a * a2;
  96   double a4 = a * a3;
  97   double a5 = a * a4;
  98   double a6 = a * a5;
  99
 100   double m = r * ((1 - e / 4 - 3 * e2 / 64 - 5 * e3 / 256) * lat -
 101                   (3 * e / 8 + 3 * e2 / 32 + 45 * e3 / 1024) * sin(2 * lat) +
 102                   (15 * e2 / 256 + 45 * e3 / 1024) * sin(4 * lat) -
 103                   (35 * e3 / 3072) * sin(6 * lat));
 104
 105   utm.easting = fixed(k0 * n * (a + (1 - tan2 + c) * a3 / 6 +
 106       (5 - 18 * tan2 + tan4 + 72 * c - 58 * e_p2) * a5 / 120) + 500000.0);
 107
 108   utm.northing = fixed(k0 * (m + n * _tan *
 109       (a2 / 2 +  a4 / 24 * (5 - tan2 + 9 * c + 4 * c * c) +
 110        a6 / 720 * (61 - 58 * tan2 + tan4 + 600 * c - 330 * e_p2))));
 111   if (negative(p.latitude.Native()))
 112     utm.northing += fixed(10000000);
 113
 114   return utm;
 115 }
 116
 117 GeoPoint
 118 UTM::ToGeoPoint() const
 119 {
 120   // remove longitude offset
 121   double x = (double)easting - 500000;
 122   double y = (double)northing;
 123
 124   // if southern hemisphere
 125   if (zone_letter < 'N')
 126     y -= 10000000.0;
 127
 128   double m  = y / k0;
 129   double mu = m / (r * (1 - e / 4 - 3 * e2 / 64 - 5 * e3 / 256));
 130   double _e_sqrt = sqrt(1 - e);
 131   double _e = (1 - _e_sqrt) / (1 + _e_sqrt);
 132   double _e2 = _e * _e;
 133   double _e3 = _e * _e2;
 134   double _e4 = _e * _e3;
 135
 136   Angle phi1rad = Angle::Radians(fixed(mu +
 137       (3 * _e / 2 - 27 * _e3 / 32) * sin(2 * mu) +
 138       (21 * _e3 / 16 - 55 * _e4 / 32) * sin(4 * mu) +
 139       (151 * _e3 / 96) * sin(6 * mu)));
 140
 141   double _sin = (double)phi1rad.sin();
 142   double sin2 = _sin * _sin;
 143   double _cos = (double)phi1rad.cos();
 144   double _tan = _sin / _cos;
 145   double tan2 = _tan * _tan;
 146   double tan4 = tan2 * tan2;
 147
 148   double _e_sin2_sqrt = sqrt(1 - e * sin2);
 149   double _e_sin2_sqrt3 = _e_sin2_sqrt * _e_sin2_sqrt * _e_sin2_sqrt;
 150   double n = r / _e_sin2_sqrt;
 151   double c = e * _cos * _cos;
 152   double c2 = c * c;
 153   double _r = r * (1 - e) / _e_sin2_sqrt3;
 154
 155   double d = x / (n * k0);
 156   double d2 = d * d;
 157   double d3 = d * d2;
 158   double d4 = d * d3;
 159   double d5 = d * d4;
 160   double d6 = d * d5;
 161
 162   double latitude = (double)phi1rad.Radians() -
 163       (n * _tan / _r) * (d2 / 2 -
 164        d4 / 24 * (5 + 3 * tan2 + 10 * c - 4 * c2 - 9 * e_p2) +
 165        d6 / 720 * (61 + 90 * tan2 + 298 * c + 45 * tan4 - 252 * e_p2 - 3 * c2));
 166
 167   double longitude = (d - d3 / 6 * (1 + 2 * tan2 + c) +
 168                       d5 / 120 * (5 - 2 * c + 28 * tan2 - 3 * c2 + 8 * e_p2 + 24 * tan4)) / _cos;
 169
 170   return GeoPoint(Angle::Radians(fixed(longitude)) + GetCentralMeridian(zone_number),
 171                   Angle::Radians(fixed(latitude)));
 172 }