location/manager/location-position.c

   1 /*
   2  * libslp-location
   3  *
   4  * Copyright (c) 2010-2013 Samsung Electronics Co., Ltd. All rights reserved.
   5  *
   6  * Contact: Youngae Kang <youngae.kang@samsung.com>, Minjune Kim <sena06.kim@samsung.com>
   7  *          Genie Kim <daejins.kim@samsung.com>
   8  *
   9  * Licensed under the Apache License, Version 2.0 (the "License");
  10  * you may not use this file except in compliance with the License.
  11  * You may obtain a copy of the License at
  12  *
  13  * http://www.apache.org/licenses/LICENSE-2.0
  14  *
  15  * Unless required by applicable law or agreed to in writing, software
  16  * distributed under the License is distributed on an "AS IS" BASIS,
  17  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  18  * See the License for the specific language governing permissions and
  19  * limitations under the License.
  20  */
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include "config.h"
  24 #endif
  25
  26 #include <math.h>
  27 #include <stdlib.h>
  28 #include <string.h>
  29
  30 #include "location-position.h"
  31 #include "location-setting.h"
  32 #include "location-log.h"
  33
  34 #define         DEG2RAD(x)      ((x) * M_PI / 180)
  35
  36 GType
  37 location_position_get_type(void)
  38 {
  39         static volatile gsize type_volatile = 0;
  40         if (g_once_init_enter(&type_volatile)) {
  41                 GType type = g_boxed_type_register_static(
  42                                                 g_intern_static_string("LocationPosition"),
  43                                                 (GBoxedCopyFunc) location_position_copy,
  44                                                 (GBoxedFreeFunc) location_position_free);
  45                 g_once_init_leave(&type_volatile, type);
  46         }
  47         return type_volatile;
  48 }
  49
  50 EXPORT_API LocationPosition *
  51 location_position_new(guint timestamp, gdouble latitude, gdouble longitude, gdouble altitude, LocationStatus status)
  52 {
  53         if (latitude < -90 || latitude > 90) return NULL;
  54         if (longitude < -180 || longitude > 180) return NULL;
  55
  56         LocationPosition *position = g_slice_new0(LocationPosition);
  57         g_return_val_if_fail(position, NULL);
  58
  59         position->timestamp = timestamp;
  60         position->latitude = latitude;
  61         position->longitude = longitude;
  62         position->altitude = altitude;
  63         position->status = status;
  64         return position;
  65 }
  66
  67 EXPORT_API void
  68 location_position_free(LocationPosition *position)
  69 {
  70         g_return_if_fail(position);
  71         g_slice_free(LocationPosition, position);
  72 }
  73
  74 EXPORT_API gboolean
  75 location_position_equal(const LocationPosition *position1, const LocationPosition *position2)
  76 {
  77         g_return_val_if_fail(position1, FALSE);
  78         g_return_val_if_fail(position2, FALSE);
  79
  80         if (position1->latitude == position2->latitude &&
  81                 position1->longitude == position2->longitude &&
  82                 position1->altitude == position2->altitude)
  83                 return TRUE;
  84         return FALSE;
  85 }
  86
  87 EXPORT_API LocationPosition *
  88 location_position_copy(const LocationPosition *position)
  89 {
  90         g_return_val_if_fail(position, NULL);
  91
  92         LocationPosition *new_position = NULL;
  93
  94         new_position = location_position_new(position->timestamp, position->latitude, position->longitude, position->altitude, position->status);
  95
  96         return new_position;
  97
  98 }
  99
 100 /* Vincenty formula. WGS-84 */
 101 EXPORT_API int
 102 location_get_distance(const LocationPosition *pos1, const LocationPosition *pos2, gulong *distance)
 103 {
 104         g_return_val_if_fail(pos1, LOCATION_ERROR_PARAMETER);
 105         g_return_val_if_fail(pos2, LOCATION_ERROR_PARAMETER);
 106         g_return_val_if_fail(distance, LOCATION_ERROR_PARAMETER);
 107
 108         *distance = 0;
 109
 110         const double a = 6378137.0, b = 6356752.314245, f = 1 / 298.257223563;
 111         double delta_lon = DEG2RAD(pos2->longitude - pos1->longitude);
 112         double u_1 = atan((1 - f) * tan(DEG2RAD(pos1->latitude)));
 113         double u_2 = atan((1 - f) * tan(DEG2RAD(pos2->latitude)));
 114
 115         double lambdaP, iter_limit = 100.0;
 116         double lambda = delta_lon;
 117
 118         double sin_sigma, sin_alpha, cos_sigma, sigma, sq_cos_alpha, cos_2sigma, C;
 119         double sq_u, cal1, cal2, delta_sigma, cal_dist;
 120         double sin_lambda, cos_lambda;
 121
 122         double sin_u1 = sin(u_1);
 123         double cos_u1 = cos(u_1);
 124         double sin_u2 = sin(u_2);
 125         double cos_u2 = cos(u_2);
 126
 127         do {
 128                 sin_lambda = sin(lambda);
 129                 cos_lambda = cos(lambda);
 130
 131                 sin_sigma = sqrt((cos_u2 * sin_lambda) * (cos_u2 * sin_lambda) + \
 132                                                  (cos_u1 * sin_u2 - sin_u1 * cos_u2 * cos_lambda) * \
 133                                                  (cos_u1 * sin_u2 - sin_u1 * cos_u2 * cos_lambda));
 134
 135                 if (sin_sigma == 0)
 136                         return LOCATION_ERROR_NONE;     /* co-incident points */
 137
 138                 cos_sigma = sin_u1 * sin_u2 + cos_u1 * cos_u2 * cos_lambda;
 139                 sigma = atan2(sin_sigma, cos_sigma);
 140
 141                 sin_alpha = cos_u1 * cos_u2 * sin_lambda / sin_sigma;
 142                 sq_cos_alpha = 1.0 - sin_alpha * sin_alpha;
 143                 cos_2sigma = cos_sigma - 2.0 * sin_u1 * sin_u2 / sq_cos_alpha;
 144
 145                 if (isnan(cos_2sigma))
 146                         cos_2sigma = 0;
 147
 148                 C = f / 16.0 * sq_cos_alpha * (4.0 + f * (4.0 - 3.0 * sq_cos_alpha));
 149
 150                 lambdaP = lambda;
 151                 lambda = delta_lon + (1.0 - C) * f * sin_alpha * \
 152                                  (sigma + C * sin_sigma * (cos_2sigma + C * cos_sigma * (-1.0 + 2.0 * cos_2sigma * cos_2sigma)));
 153
 154         } while (abs(lambda - lambdaP) > 1e-12 && --iter_limit > 0);
 155
 156         if (iter_limit == 0) return LOCATION_ERROR_UNKNOWN;
 157
 158         sq_u = sq_cos_alpha * (a * a - b * b) / (b * b);
 159
 160         cal1 = 1.0 + sq_u / 16384.0 * (4096.0 + sq_u * (-768.0 + sq_u * (320.0 - 175.0 * sq_u)));
 161         cal2 = sq_u / 1024.0 * (256.0 + sq_u * (-128.0 + sq_u * (74.0 - 47.0 * sq_u)));
 162
 163         delta_sigma = cal2 * sin_sigma * (cos_2sigma + cal2 / 4.0 * (cos_sigma * (-1.0 + 2.0 * cos_2sigma * cos_2sigma) - \
 164                                                                                                                                  cal2 / 6.0 * cos_2sigma * (-3.0 + 4.0 * sin_sigma * sin_sigma) * (-3.0 + 4.0 * cos_2sigma * cos_2sigma)));
 165         cal_dist = b * cal1 * (sigma - delta_sigma);
 166
 167         *distance = (gulong) cal_dist;
 168
 169         return LOCATION_ERROR_NONE;
 170
 171 }
 172
 173 EXPORT_API void
 174 location_last_position_a2i(char *position, int *lat, int *lon)
 175 {
 176         char *d_lat, *d_lon;
 177         char latitude[HALF_KEY_LENGTH];
 178         char longitude[HALF_KEY_LENGTH];
 179
 180         memcpy(latitude, position + 1, HALF_KEY_LENGTH - 1);
 181         memcpy(longitude, position + HALF_KEY_LENGTH + 1, HALF_KEY_LENGTH - 1);
 182         latitude[HALF_KEY_LENGTH - 1] = '\0';
 183         longitude[HALF_KEY_LENGTH - 1] = '\0';
 184         d_lat = position;
 185         d_lon = position + HALF_KEY_LENGTH;
 186
 187         *lat = atoi(latitude);
 188         *lon = atoi(longitude);
 189
 190         if (*d_lat == 'S')
 191                 *lat = *lat * -1;
 192
 193         if (*d_lon == 'W')
 194                 *lon = *lon * -1;
 195 }