diff options
Diffstat (limited to 'src/host/layer23/src/misc/geo.c')
| -rw-r--r-- | src/host/layer23/src/misc/geo.c | 47 |
1 files changed, 47 insertions, 0 deletions
diff --git a/src/host/layer23/src/misc/geo.c b/src/host/layer23/src/misc/geo.c new file mode 100644 index 00000000..65633d2c --- /dev/null +++ b/src/host/layer23/src/misc/geo.c @@ -0,0 +1,47 @@ +#include <math.h> +#include "geo.h" + +void geo2space(double *x, double *y, double *z, double lon, double lat) +{ + *z = sin(lat / 180.0 * PI) * POLE_RADIUS; + *x = sin(lon / 180.0 * PI) * cos(lat / 180.0 * PI) * EQUATOR_RADIUS; + *y = -cos(lon / 180.0 * PI) * cos(lat / 180.0 * PI) * EQUATOR_RADIUS; +} + +void space2geo(double *lon, double *lat, double x, double y, double z) +{ + double r; + + /* bring geoid to 1m radius */ + z = z / POLE_RADIUS; + x = x / EQUATOR_RADIUS; + y = y / EQUATOR_RADIUS; + + /* normalize */ + r = sqrt(x * x + y * y + z * z); + z = z / r; + x = x / r; + y = y / r; + + *lat = asin(z) / PI * 180; + *lon = atan2(x, -y) / PI * 180; +} + +double distinspace(double x1, double y1, double z1, double x2, double y2, + double z2) +{ + double x = x1 - x2; + double y = y1 - y2; + double z = z1 - z2; + + return sqrt(x * x + y * y + z * z); +} + +double distonplane(double x1, double y1, double x2, double y2) +{ + double x = x1 - x2; + double y = y1 - y2; + + return sqrt(x * x + y * y); +} + |