src/host/gsmmap/geo.c

   1 #include <math.h>
   2 #include "geo.h"
   3
   4 void geo2space(double *x, double *y, double *z, double lon, double lat)
   5 {
   6         *z = sin(lat / 180.0 * PI) * POLE_RADIUS;
   7         *x = sin(lon / 180.0 * PI) * cos(lat / 180.0 * PI) * EQUATOR_RADIUS;
   8         *y = -cos(lon / 180.0 * PI) * cos(lat / 180.0 * PI) * EQUATOR_RADIUS;
   9 }
  10
  11 void space2geo(double *lon, double *lat, double x, double y, double z)
  12 {
  13         double r;
  14
  15         /* bring geoid to 1m radius */
  16         z = z / POLE_RADIUS;
  17         x = x / EQUATOR_RADIUS;
  18         y = y / EQUATOR_RADIUS;
  19
  20         /* normalize */
  21         r = sqrt(x * x + y * y + z * z);
  22         z = z / r;
  23         x = x / r;
  24         y = y / r;
  25
  26         *lat = asin(z) / PI * 180;
  27         *lon = atan2(x, -y) / PI * 180;
  28 }
  29
  30 double distinspace(double x1, double y1, double z1, double x2, double y2,
  31         double z2)
  32 {
  33         double x = x1 - x2;
  34         double y = y1 - y2;
  35         double z = z1 - z2;
  36
  37         return sqrt(x * x + y * y + z * z);
  38 }
  39
  40 double distonplane(double x1, double y1, double x2, double y2)
  41 {
  42         double x = x1 - x2;
  43         double y = y1 - y2;
  44
  45         return sqrt(x * x + y * y);
  46 }
  47