src/map/geocentric.cpp

   1 /*
   2  * Based on libgeotrans with the following Source Code Disclaimer:
   3
   4 1. The GEOTRANS source code ("the software") is provided free of charge by
   5 the National Imagery and Mapping Agency (NIMA) of the United States
   6 Department of Defense. Although NIMA makes no copyright claim under Title 17
   7 U.S.C., NIMA claims copyrights in the source code under other legal regimes.
   8 NIMA hereby grants to each user of the software a license to use and
   9 distribute the software, and develop derivative works.
  10
  11 2. Warranty Disclaimer: The software was developed to meet only the internal
  12 requirements of the U.S. National Imagery and Mapping Agency. The software
  13 is provided "as is," and no warranty, express or implied, including but not
  14 limited to the implied warranties of merchantability and fitness for
  15 particular purpose or arising by statute or otherwise in law or from a
  16 course of dealing or usage in trade, is made by NIMA as to the accuracy and
  17 functioning of the software.
  18
  19 3. NIMA and its personnel are not required to provide technical support or
  20 general assistance with respect to the software.
  21
  22 4. Neither NIMA nor its personnel will be liable for any claims, losses, or
  23 damages arising from or connected with the use of the software. The user
  24 agrees to hold harmless the United States National Imagery and Mapping
  25 Agency. The user's sole and exclusive remedy is to stop using the software.
  26
  27 5. NIMA requests that products developed using the software credit the
  28 source of the software with the following statement, "The product was
  29 developed using GEOTRANS, a product of the National Imagery and Mapping
  30 Agency and U.S. Army Engineering Research and Development Center."
  31
  32 6. For any products developed using the software, NIMA requires a disclaimer
  33 that use of the software does not indicate endorsement or approval of the
  34 product by the Secretary of Defense or the National Imagery and Mapping
  35 Agency. Pursuant to the United States Code, 10 U.S.C. Sec. 2797, the name of
  36 the National Imagery and Mapping Agency, the initials "NIMA", the seal of
  37 the National Imagery and Mapping Agency, or any colorable imitation thereof
  38 shall not be used to imply approval, endorsement, or authorization of a
  39 product without prior written permission from United States Secretary of
  40 Defense.
  41
  42 */
  43
  44 #include "ellipsoid.h"
  45 #include "geocentric.h"
  46
  47
  48 #define AD_C 1.0026000
  49
  50 Point3D Geocentric::fromGeodetic(const Coordinates &c, const Ellipsoid &e)
  51 {
  52         double lat = deg2rad(c.lat());
  53         double lon = deg2rad(c.lon());
  54         double slat = sin(lat);
  55         double slat2 = slat * slat;
  56         double clat = cos(lat);
  57         double Rn = e.radius() / (sqrt(1.0 - e.es() * slat2));
  58
  59         if (lon > M_PI)
  60                 lon -= 2 * M_PI;
  61
  62         return Point3D(Rn * clat * cos(lon), Rn * clat * sin(lon),
  63           (Rn * (1 - e.es())) * slat);
  64 }
  65
  66 Coordinates Geocentric::toGeodetic(const Point3D &p, const Ellipsoid &e)
  67 {
  68         bool pole = false;
  69         double lat, lon;
  70
  71         if (p.x() == 0.0) {
  72                 if (p.y() > 0)
  73                         lon = M_PI_2;
  74                 else if (p.y() < 0)
  75                         lon = -M_PI_2;
  76                 else {
  77                         pole = true;
  78                         lon = 0.0;
  79                         if (p.z() > 0.0)
  80                                 lat = M_PI_2;
  81                         else if (p.z() < 0.0)
  82                                 lat = -M_PI_2;
  83                         else
  84                                 return Coordinates(rad2deg(lon), rad2deg(M_PI_2));
  85                 }
  86         } else
  87                 lon = atan2(p.y(), p.x());
  88
  89         double W2 = p.x()*p.x() + p.y()*p.y();
  90         double W = sqrt(W2);
  91         double T0 = p.z() * AD_C;
  92         double S0 = sqrt(T0 * T0 + W2);
  93         double Sin_B0 = T0 / S0;
  94         double Cos_B0 = W / S0;
  95         double Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
  96         double T1 = p.z() + e.b() * e.e2s() * Sin3_B0;
  97         double Sum = W - e.radius() * e.es() * Cos_B0 * Cos_B0 * Cos_B0;
  98         double S1 = sqrt(T1*T1 + Sum * Sum);
  99         double Sin_p1 = T1 / S1;
 100         double Cos_p1 = Sum / S1;
 101
 102         if (!pole)
 103                 lat = atan(Sin_p1 / Cos_p1);
 104
 105         return Coordinates(rad2deg(lon), rad2deg(lat));
 106 }