location/manager/location-position.c

   1 /*
   2  * libslp-location
   3  *
   4  * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd. All rights reserved.
   5  *
   6  * Contact: Youngae Kang <youngae.kang@samsung.com>, Yunhan Kim <yhan.kim@samsung.com>,
   7  *          Genie Kim <daejins.kim@samsung.com>, Minjune Kim <sena06.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
  29 #include "location-position.h"
  30 #include "location-setting.h"
  31 #include "location-log.h"
  32
  33 #define         DEG2RAD(x)      ((x) * M_PI / 180)
  34
  35 GType
  36 location_position_get_type (void)
  37 {
  38         static volatile gsize type_volatile = 0;
  39         if(g_once_init_enter(&type_volatile)) {
  40                 GType type = g_boxed_type_register_static (
  41                         g_intern_static_string ("LocationPosition"),
  42                         (GBoxedCopyFunc) location_position_copy,
  43                         (GBoxedFreeFunc) location_position_free);
  44                 g_once_init_leave(&type_volatile, type);
  45         }
  46         return type_volatile;
  47 }
  48
  49 EXPORT_API LocationPosition *
  50 location_position_new (guint timestamp,
  51         gdouble latitude,
  52         gdouble longitude,
  53         gdouble altitude,
  54         LocationStatus status)
  55 {
  56         if (latitude < -90 || latitude > 90) return NULL;
  57         if (longitude < -180 || longitude > 180) return NULL;
  58
  59         LocationPosition* position = g_slice_new0(LocationPosition);
  60         position->timestamp = timestamp;
  61         position->latitude = latitude;
  62         position->longitude = longitude;
  63         position->altitude = altitude;
  64         position->status = status;
  65         return position;
  66 }
  67
  68 EXPORT_API void
  69 location_position_free (LocationPosition* position)
  70 {
  71         g_return_if_fail(position);
  72         g_slice_free(LocationPosition, position);
  73 }
  74
  75 EXPORT_API gboolean
  76 location_position_equal (const LocationPosition *position1, const LocationPosition *position2)
  77 {
  78         g_return_val_if_fail(position1, FALSE);
  79         g_return_val_if_fail(position2, FALSE);
  80
  81         if (position1->latitude == position2->latitude &&
  82                 position1->longitude == position2->longitude &&
  83                 position1->altitude == position2->altitude)
  84                 return TRUE;
  85         return FALSE;
  86 }
  87
  88 EXPORT_API LocationPosition*
  89 location_position_copy (const LocationPosition *position)
  90 {
  91         g_return_val_if_fail(position, NULL);
  92
  93         LocationPosition *new_position = NULL;
  94
  95         new_position = location_position_new (position->timestamp,
  96                                                                 position->latitude,
  97                                                                 position->longitude,
  98                                                                 position->altitude,
  99                                                                 position->status);
 100
 101         if(new_position) new_position->updated_timestamp = position->updated_timestamp;
 102
 103         return new_position;
 104
 105 }
 106
 107 /* Vincenty formula. WGS-84 */
 108 EXPORT_API      int
 109 location_get_distance(const LocationPosition *pos1, const LocationPosition *pos2, gulong *distance)
 110 {
 111         g_return_val_if_fail(pos1, LOCATION_ERROR_PARAMETER);
 112         g_return_val_if_fail(pos2, LOCATION_ERROR_PARAMETER);
 113         g_return_val_if_fail(distance, LOCATION_ERROR_PARAMETER);
 114
 115         *distance = 0;
 116
 117         const double a = 6378137.0, b = 6356752.314245,  f = 1/298.257223563;
 118         double delta_lon = DEG2RAD(pos2->longitude-pos1->longitude);
 119         double u_1 = atan((1-f) * tan(DEG2RAD(pos1->latitude)));
 120         double u_2 = atan((1-f) * tan(DEG2RAD(pos2->latitude)));
 121
 122         double lambdaP, iter_limit = 100.0;
 123         double lambda = delta_lon;
 124
 125         double sin_sigma, sin_alpha, cos_sigma, sigma,  sq_cos_alpha, cos_2sigma, C;
 126         double sq_u, cal1, cal2, delta_sigma, cal_dist;
 127         double sin_lambda, cos_lambda;
 128
 129         double sin_u1 = sin(u_1);
 130         double cos_u1 = cos(u_1);
 131         double sin_u2 = sin(u_2);
 132         double cos_u2 = cos(u_2);
 133
 134         do {
 135                 sin_lambda = sin(lambda);
 136                 cos_lambda = cos(lambda);
 137
 138                 sin_sigma = sqrt((cos_u2*sin_lambda)*(cos_u2*sin_lambda) + \
 139                         (cos_u1*sin_u2-sin_u1*cos_u2*cos_lambda) * \
 140                         (cos_u1*sin_u2-sin_u1*cos_u2*cos_lambda));
 141
 142                 if (sin_sigma ==0)
 143                         return LOCATION_ERROR_NONE;  // co-incident points
 144
 145                 cos_sigma = sin_u1*sin_u2 + cos_u1*cos_u2*cos_lambda;
 146                 sigma = atan2(sin_sigma, cos_sigma);
 147
 148                 sin_alpha = cos_u1 * cos_u2 * sin_lambda / sin_sigma;
 149                 sq_cos_alpha = 1.0 - sin_alpha*sin_alpha;
 150                 cos_2sigma = cos_sigma - 2.0*sin_u1*sin_u2/sq_cos_alpha;
 151
 152                 if (isnan(cos_2sigma))
 153                         cos_2sigma = 0;
 154
 155                 C = f/16.0*sq_cos_alpha*(4.0+f*(4.0-3.0*sq_cos_alpha));
 156
 157                 lambdaP = lambda;
 158                 lambda = delta_lon + (1.0-C) * f * sin_alpha * \
 159                          (sigma + C*sin_sigma*(cos_2sigma+C*cos_sigma*(-1.0+2.0*cos_2sigma*cos_2sigma)));
 160
 161         } while (abs(lambda-lambdaP) > 1e-12 && --iter_limit>0);
 162
 163         if (iter_limit==0) return LOCATION_ERROR_UNKNOWN;
 164
 165         sq_u = sq_cos_alpha * (a*a - b*b) / (b*b);
 166
 167         cal1 = 1.0 + sq_u/16384.0*(4096.0+sq_u*(-768.0+sq_u*(320.0-175.0*sq_u)));
 168         cal2 = sq_u/1024.0 * (256.0+sq_u*(-128.0+sq_u*(74.0-47.0*sq_u)));
 169
 170         delta_sigma = cal2*sin_sigma*(cos_2sigma+cal2/4.0*(cos_sigma*(-1.0+2.0*cos_2sigma*cos_2sigma)- \
 171                                 cal2/6.0*cos_2sigma*(-3.0+4.0*sin_sigma*sin_sigma)*(-3.0+4.0*cos_2sigma*cos_2sigma)));
 172         cal_dist = b*cal1*(sigma-delta_sigma);
 173
 174         *distance = (gulong) cal_dist;
 175
 176         return LOCATION_ERROR_NONE;
 177
 178 }
 179
 180 EXPORT_API void
 181 location_last_position_a2i(char *position, int *lat, int *lon)
 182 {
 183         char *d_lat, *d_lon;
 184         char latitude[HALF_KEY_LENGTH];
 185         char longitude[HALF_KEY_LENGTH];
 186
 187         memcpy(latitude, position + 1, HALF_KEY_LENGTH - 1);
 188         memcpy(longitude, position + HALF_KEY_LENGTH + 1, HALF_KEY_LENGTH - 1);
 189         latitude[HALF_KEY_LENGTH - 1] = '\0';
 190         longitude[HALF_KEY_LENGTH - 1] = '\0';
 191         d_lat = position;
 192         d_lon = position + HALF_KEY_LENGTH;
 193
 194         *lat = atoi(latitude);
 195         *lon = atoi(longitude);
 196
 197         if (*d_lat == 'S') {
 198                 *lat = *lat * -1;
 199         }
 200         if (*d_lon == 'W') {
 201                 *lon = *lon * -1;
 202         }
 203 }