N-sim Emulation and simulation of Wireless Sensor Networks    Home    Project Page    Download    CVS    Installation    Configuration    Plug-ins  Hosted by

/home/brennan/n-sim/OrbisQuartus/shared/coordinates.cpp Go to the documentation of this file. /* * Copyright 2007. Los Alamos National Security, LLC. This material * was produced under U.S. Government contract DE-AC52-06NA25396 for * Los Alamos National Laboratory (LANL), which is operated by Los * Alamos National Security, LLC, for the Department of Energy. The * U.S. Government has rights to use, reproduce, and distribute this * software. NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, * LLC, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LEGAL * LIABILITY FOR THE USE OF THIS SOFTWARE. If software is modified to * produce derivative works, such modified software should be clearly * marked, so as not to confuse it with the version available from LANL. * * Additionally, this program is free software; you can redistribute * it and/or modify it under the terms of the GNU General Public * License as published by the Free Software Foundation; version 2 of * the License. Accordingly, this program is distributed in the hope * it will be useful, but WITHOUT ANY WARRANTY; without even the * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR * PURPOSE. See the GNU General Public License for more details. */ #include <math.h> #include <string.h> #include "coordinates.h" int Coordinates::list_reference_ellipsoids(char *names[], unsigned int *len) { int i, size = (*len > _GLOBAL_ELLIPSOIDS) ? _GLOBAL_ELLIPSOIDS : *len; for (i = 0; i < size; i++) { if (names != 0) { strcpy(names[i], reference_ellipsoids[i].name); } } names[i] = 0; *len = _GLOBAL_ELLIPSOIDS; return i; } int Coordinates::_reference_ellipsoid_index(const char *name) { unsigned int i; for (i = 0; i < _GLOBAL_ELLIPSOIDS; i++) { int size = (strlen(name) > strlen(reference_ellipsoids[i].name)) ? strlen(reference_ellipsoids[i].name) : strlen(name); if (strncmp(name, reference_ellipsoids[i].name, size) == 0) return i; } return -1; } static inline double __radians2degrees(double radians) { return radians * 180 / M_PI; } static inline double __degrees2radians(double degrees) { return degrees * M_PI / 180; } void Coordinates::set_ellipsoid(char *type) { if (_reference_ellipsoid_index(type) < 0) strcpy(_ellipsoid, "WGS 84"); else strcpy(_ellipsoid, type); } /* Using spherical trigonometry (< 1m accuracy with long double precision) */ double Coordinates::_rad_distance(double lat1, double lon1, double lat2, double lon2) { /* assumes called from origin point instance */ double delta_lon = __degrees2radians(lon2) - __degrees2radians(lon1); double rad_lat1 = __degrees2radians(lat1); double rad_lat2 = __degrees2radians(lat2); double num = sqrt(pow((cos(rad_lat2) * sin(delta_lon)), 2) + pow(((cos(rad_lat1) * sin(rad_lat2)) - (sin(rad_lat1) * cos(rad_lat2) * cos(delta_lon))), 2)); double denom = (sin(rad_lat1) * sin(rad_lat2)) + (cos(rad_lat1) * cos(rad_lat2) * cos(delta_lon)); return atan2(num, denom); } double Coordinates::coord_distance(double lat1, double lon1, double lat2, double lon2) { /* assumes called from origin point instance */ int idx = _reference_ellipsoid_index(_ellipsoid); double a = reference_ellipsoids[idx].equatorial_radius; return a * _rad_distance(lat1, lon1, lat2, lon2); } void Coordinates::coords2meters(double latitude, double longitude, double *x, double *y) { /* assumes called from origin point instance */ int idx = _reference_ellipsoid_index(_ellipsoid); double a = reference_ellipsoids[idx].equatorial_radius; double rad_lat1 = __degrees2radians(lat); double rad_lat2 = __degrees2radians(latitude); double rad_lon1 = __degrees2radians(lon); double rad_lon2 = __degrees2radians(longitude); double delta_lat = rad_lat2 - rad_lat1; double delta_lon = rad_lon2 - rad_lon1; double hypotenuse = _rad_distance(lat, lon, latitude, longitude); double bearing = atan2(sin(delta_lon) * cos(rad_lat2), cos(rad_lat1) * sin(rad_lat2) - sin(rad_lat1) * cos(rad_lat2) * cos(delta_lon)); double A = pow(sin((hypotenuse - delta_lat) / 2), 2) + sin(hypotenuse) * sin(delta_lat) * pow(sin(bearing / 2), 2); if (rad_lon2 >= rad_lon1) *x = a * 2 * atan2(sqrt(A), sqrt(1 - A)); else *x = -a * 2 * atan2(sqrt(A), sqrt(1 - A)); *y = a * delta_lat; } void Coordinates::meters2coords(double x, double y, double *latitude, double *longitude) { /* assumes called from origin point instance */ int idx = _reference_ellipsoid_index(_ellipsoid); double a = reference_ellipsoids[idx].equatorial_radius; double rad_lat1 = __degrees2radians(lat); double rad_lon1 = __degrees2radians(lon); double rad_y = y / a; double rad_x = x / a; double rad_dist = sqrt(pow(x, 2) + pow(y, 2)) / a; // double rad_bearing = atan2(rad_x, rad_y); equivalent?? double rad_bearing = atan2(sin(rad_x) * cos(rad_y), sin(rad_y)); double rad_lat2 = asin(sin(rad_lat1) * cos(rad_dist) + cos(rad_lat1) * sin(rad_dist) * cos(rad_bearing)); double rad_lon2 = rad_lon1 + atan2(sin(rad_bearing) * sin(rad_dist) * cos(rad_lat1), cos(rad_dist) - sin(rad_lat1) * sin(rad_lat2)); *latitude = __radians2degrees(rad_lat2); *longitude = __radians2degrees(rad_lon2); }

© 2007, Los Alamos National Security, LLC.