vcs.maemo.org Git - ptas/blob - getmehome/coordinate-system.c

   1 #include "coordinate-system.h"
   2
   3 #include <math.h>
   4
   5
   6 // Degrees to radians
   7 double radians(double deg) {
   8     return deg * M_PI / 180.0;
   9 }
  10
  11 // Radians to degrees
  12 double degrees(double rad) {
  13     return rad * 180.0 / M_PI;
  14 }
  15
  16
  17 // Constants
  18 // Longitude0 and Center meridian of KKJ bands
  19 const double KKJ_ZONE_INFO[6][2] = { {18.0,  500000.0},
  20                                      {21.0, 1500000.0},
  21                                      {24.0, 2500000.0},
  22                                      {27.0, 3500000.0},
  23                                      {30.0, 4500000.0},
  24                                      {33.0, 5500000.0} };
  25
  26
  27 // Function:  KKJ_Zone_I
  28 int KKJ_Zone_I(KKJ easting) {
  29     int zoneNumber = floor(easting / 1000000.0);
  30     if (zoneNumber < 0 || zoneNumber > 5) {
  31         zoneNumber = -1;
  32     }
  33
  34     return zoneNumber;
  35 }
  36
  37
  38 // Function:  KKJ_Zone_Lo
  39 int KKJ_Zone_Lo(double kkjlo) {
  40     // determine the zonenumber from KKJ easting
  41     // takes KKJ zone which has center meridian
  42     // longitude nearest (in math value) to
  43     // the given KKJ longitude
  44     int zoneNumber = 5;
  45     while (zoneNumber >= 0) {
  46         if (fabs(kkjlo - KKJ_ZONE_INFO[zoneNumber][0]) <= 1.5) {
  47             break;
  48         }
  49         zoneNumber--;
  50     }
  51
  52     return zoneNumber;
  53 }
  54
  55
  56 // Function:  KKJlalo_to_WGS84lalo
  57 void KKJlola_to_WGS84lola(double kkjlo, double kkjla, double *outLongitude, double *outLatitude) {
  58     double dLa = radians(0.124867E+01 + -0.269982E+00 * kkjla + 0.191330E+00 * kkjlo + 0.356119E-02 * kkjla * kkjla + -0.122312E-02 * kkjla * kkjlo + -0.335514E-03 * kkjlo * kkjlo) / 3600.0;
  59     double dLo = radians(-0.286111E+02 + 0.114183E+01 * kkjla + -0.581428E+00 * kkjlo + -0.152421E-01 * kkjla * kkjla + 0.118177E-01 * kkjla * kkjlo + 0.826646E-03 * kkjlo * kkjlo) / 3600.0;
  60
  61     *outLatitude = degrees(radians(kkjla) + dLa);
  62     *outLongitude = degrees(radians(kkjlo) + dLo);
  63 }
  64
  65
  66 // Function:  WGS84lalo_to_KKJlalo
  67 void WGS84lola_to_KKJlola(double longitude, double latitude, double *outLongitude, double *outLatitude) {
  68     double dLa = radians(-0.124766E+01 + 0.269941E+00 * latitude + -0.191342E+00 * longitude + -0.356086E-02 * latitude * latitude + 0.122353E-02 * latitude * longitude + 0.335456E-03 * longitude * longitude) / 3600.0;
  69     double dLo = radians(0.286008E+02 + -0.114139E+01 * latitude + 0.581329E+00 * longitude + 0.152376E-01 * latitude * latitude + -0.118166E-01 * latitude * longitude + -0.826201E-03 * longitude * longitude) / 3600.0;
  70
  71     *outLatitude = degrees(radians(latitude) + dLa);
  72     *outLongitude = degrees(radians(longitude) + dLo);
  73 }
  74
  75
  76 // Function:  KKJlalo_to_KKJxy
  77 void KKJlola_to_KKJxy(double lon, double lat, int zoneNumber, KKJ *outX, KKJ *outY) {
  78     // Hayford ellipsoid
  79     double a = 6378388.0;
  80     double f  = 1.0 / 297.0;
  81     double b  = (1.0 - f) * a;
  82     double bb = b * b;
  83     double c  = (a / b) * a;
  84     double ee = (a * a - bb) / bb;
  85     double n = (a - b) / (a + b);
  86     double nn = n * n;
  87
  88     double Lo = radians(lon) - radians(KKJ_ZONE_INFO[zoneNumber][0]);
  89     double cosLa = cos(radians(lat));
  90     double NN = ee * cosLa * cosLa;
  91     double LaF = atan(tan(radians(lat)) / cos(Lo * sqrt(1.0 + NN)));
  92     double cosLaF = cos(LaF);
  93     double t = (tan(Lo) * cosLaF) / sqrt(1.0 + ee * cosLaF * cosLaF);
  94     double A = a / (1.0 + n);
  95     double A1 = A * (1.0 + nn / 4.0 + nn * nn / 64.0);
  96     double A2 = A * 1.5 * n * (1.0 - nn / 8.0);
  97     double A3 = A * 0.9375 * nn * (1.0 - nn / 4.0);
  98     double A4 = A * 35.0 / 48.0 * nn * n;
  99
 100     *outY = A1 * LaF - A2 * sin(2.0 * LaF) + A3 * sin(4.0 * LaF) - A4 * sin(6.0 * LaF);
 101     *outX = c * log(t + sqrt(1.0 + t * t)) + 500000.0 + zoneNumber * 1000000.0;
 102 }
 103
 104 // Function:  KKJxy_to_KKJlalo
 105 void KKJxy_to_KKJlola(KKJ x, KKJ y, double *outLongitude, double *outLatitude) {
 106     // Scan iteratively the target area, until find matching
 107     // KKJ coordinate value.  Area is defined with Hayford Ellipsoid.
 108     int zoneNumber = KKJ_Zone_I(x);
 109     double minLo = radians(18.5);
 110     double maxLo = radians(32.0);
 111     double minLa = radians(59.0);
 112     double maxLa = radians(70.5);
 113
 114     int i = 1;
 115     KKJ tmpX, tmpY;
 116
 117     while (i < 35) {
 118         double deltaLo = maxLo - minLo;
 119         double deltaLa = maxLa - minLa;
 120         *outLongitude = degrees(minLo + 0.5 * deltaLo);
 121         *outLatitude = degrees(minLa + 0.5 * deltaLa);
 122         KKJlola_to_KKJxy(*outLongitude, *outLatitude, zoneNumber, &tmpX, &tmpY);
 123         if (tmpY < y) {
 124             minLa = minLa + 0.45 * deltaLa;
 125         } else {
 126             maxLa = minLa + 0.55 * deltaLa;
 127         }
 128
 129         if (tmpX < x) {
 130             minLo = minLo + 0.45 * deltaLo;
 131         } else {
 132             maxLo = minLo + 0.55 * deltaLo;
 133         }
 134
 135         i++;
 136     }
 137 }
 138
 139   ////////////////////
 140  // PUBLIC METHODS //
 141 ////////////////////
 142
 143 void WGS84lola_to_KKJxy(double longitude, double latitude, KKJ *outX, KKJ *outY) {
 144     double kkjlo, kkjla;
 145
 146     WGS84lola_to_KKJlola(longitude, latitude, &kkjlo, &kkjla);
 147     int zoneNumber = KKJ_Zone_Lo(kkjlo);
 148     KKJlola_to_KKJxy(kkjlo, kkjla, zoneNumber, outX, outY);
 149  }
 150
 151 void KKJxy_to_WGS84lola(KKJ x, KKJ y, double *outLongitude, double *outLatitude) {
 152     double kkjlo, kkjla;
 153
 154     KKJxy_to_KKJlola(x, y, &kkjlo, &kkjla);
 155     KKJlola_to_WGS84lola(kkjlo, kkjla, outLongitude, outLatitude);
 156 }