src/host/gsmmap/locate.c

   1 /* Algorithm to locate a destination by distance measurement:
   2  */
   3
   4 #include <stdio.h>
   5 #include <errno.h>
   6 #include <math.h>
   7
   8 #include "geo.h"
   9 #include "locate.h"
  10
  11 #define CIRCLE_PROBE    30.0
  12 #define FINETUNE_RADIUS 5.0
  13
  14 extern double debug_long, debug_lat, debug_x_scale;
  15 extern FILE *debug_fp;
  16 extern int log_debug;
  17
  18 static double finetune_x[6], finetune_y[6], finetune_dist[6];
  19
  20 int locate_cell(struct probe *probe_first, double *min_x, double *min_y)
  21 {
  22         struct probe *probe, *min_probe;
  23         int i, test_steps, optimized;
  24         double min_dist, dist, x, y, rad, temp;
  25         double circle_probe, finetune_radius;
  26
  27         /* convert meters into degrees */
  28         circle_probe = CIRCLE_PROBE / (EQUATOR_RADIUS * PI / 180.0);
  29         finetune_radius = FINETUNE_RADIUS / (EQUATOR_RADIUS * PI / 180.0);
  30
  31         if (log_debug) {
  32                 fprintf(debug_fp, "<Folder>\n");
  33                 fprintf(debug_fp, "\t<name>Debug Locator</name>\n");
  34                 fprintf(debug_fp, "\t<open>0</open>\n");
  35                 fprintf(debug_fp, "\t<visibility>0</visibility>\n");
  36         }
  37
  38         /* get probe of minimum distance */
  39         min_probe = NULL;
  40         probe = probe_first;
  41         min_dist = 42;
  42         i = 0;
  43         while (probe) {
  44                 if (log_debug) {
  45                         fprintf(debug_fp, "\t<Placemark>\n");
  46                         fprintf(debug_fp, "\t\t<name>MEAS</name>\n");
  47                         fprintf(debug_fp, "\t\t<visibility>0</visibility>\n");
  48                         fprintf(debug_fp, "\t\t<LineString>\n");
  49                         fprintf(debug_fp, "\t\t\t<tessellate>1</tessellate>\n");
  50                         fprintf(debug_fp, "\t\t\t<coordinates>\n");
  51                         rad = 2.0 * 3.1415927 / 35;
  52                         for (i = 0; i < 35; i++) {
  53                                 x = probe->x + probe->dist * sin(rad * i);
  54                                 y = probe->y + probe->dist * cos(rad * i);
  55                                 fprintf(debug_fp, "%.8f,%.8f\n", debug_long +
  56                                         x * debug_x_scale, debug_lat + y);
  57                         }
  58                         fprintf(debug_fp, "\t\t\t</coordinates>\n");
  59                         fprintf(debug_fp, "\t\t</LineString>\n");
  60                         fprintf(debug_fp, "\t</Placemark>\n");
  61                 }
  62
  63                 if (!min_probe || probe->dist < min_dist) {
  64                         min_probe = probe;
  65                         min_dist = probe->dist;
  66                 }
  67                 probe = probe->next;
  68                 i++;
  69         }
  70
  71         if (i < 3) {
  72                 fprintf(stderr, "Need at least 3 points\n");
  73                 return -EINVAL;
  74         }
  75
  76         /* calculate the number of steps to search for destination point */
  77         test_steps = 2.0 * 3.1415927 * min_probe->dist / circle_probe;
  78         rad = 2.0 * 3.1415927 / test_steps;
  79
  80         if (log_debug) {
  81                 fprintf(debug_fp, "\t<Placemark>\n");
  82                 fprintf(debug_fp, "\t\t<name>Smallest MEAS</name>\n");
  83                 fprintf(debug_fp, "\t\t<visibility>0</visibility>\n");
  84                 fprintf(debug_fp, "\t\t<LineString>\n");
  85                 fprintf(debug_fp, "\t\t\t<tessellate>1</tessellate>\n");
  86                 fprintf(debug_fp, "\t\t\t<coordinates>\n");
  87         }
  88
  89         /* search on a circle for the location of the lowest distance
  90          * to the radius with the greatest distance */
  91         min_dist = 42;
  92         *min_x = *min_y = 42;
  93         for (i = 0; i < test_steps; i++) {
  94                 x = min_probe->x + min_probe->dist * sin(rad * i);
  95                 y = min_probe->y + min_probe->dist * cos(rad * i);
  96                 if (log_debug)
  97                         fprintf(debug_fp, "%.8f,%.8f\n", debug_long +
  98                                 x * debug_x_scale, debug_lat + y);
  99                 /* look for greatest distance */
 100                 dist = 0;
 101                 probe = probe_first;
 102                 while (probe) {
 103                         if (probe != min_probe) {
 104                                 /* distance to the radius */
 105                                 temp = distonplane(probe->x, probe->y, x, y);
 106                                 temp -= probe->dist;
 107                                 if (temp < 0)
 108                                         temp = -temp;
 109                                 if (temp > dist)
 110                                         dist = temp;
 111                         }
 112                         probe = probe->next;
 113                 }
 114                 if (i == 0 || dist < min_dist) {
 115                         min_dist = dist;
 116                         *min_x = x;
 117                         *min_y = y;
 118                 }
 119         }
 120
 121         if (log_debug) {
 122                 fprintf(debug_fp, "\t\t\t</coordinates>\n");
 123                 fprintf(debug_fp, "\t\t</LineString>\n");
 124                 fprintf(debug_fp, "\t</Placemark>\n");
 125
 126                 fprintf(debug_fp, "\t<Placemark>\n");
 127                 fprintf(debug_fp, "\t\t<name>Finetune</name>\n");
 128                 fprintf(debug_fp, "\t\t<visibility>0</visibility>\n");
 129                 fprintf(debug_fp, "\t\t<LineString>\n");
 130                 fprintf(debug_fp, "\t\t\t<tessellate>1</tessellate>\n");
 131                 fprintf(debug_fp, "\t\t\t<coordinates>\n");
 132         }
 133
 134         min_dist = 9999999999.0;
 135 tune_again:
 136         if (log_debug)
 137                 fprintf(debug_fp, "%.8f,%.8f\n", debug_long +
 138                         *min_x * debug_x_scale, debug_lat + *min_y);
 139
 140         /* finetune the point */
 141         rad = 2.0 * 3.1415927 / 6;
 142         for (i = 0; i < 6; i++) {
 143                 x = *min_x + finetune_radius * sin(rad * i);
 144                 y = *min_y + finetune_radius * cos(rad * i);
 145                 /* search for the point with the lowest sum of distances */
 146                 dist = 0;
 147                 probe = probe_first;
 148                 while (probe) {
 149                         /* distance to the radius */
 150                         temp = distonplane(probe->x, probe->y, x, y);
 151                         temp -= probe->dist;
 152                         if (temp < 0)
 153                                 temp = -temp;
 154                         dist += temp;
 155                         probe = probe->next;
 156                 }
 157                 finetune_dist[i] = dist;
 158                 finetune_x[i] = x;
 159                 finetune_y[i] = y;
 160         }
 161
 162         optimized = 0;
 163         for (i = 0; i < 6; i++) {
 164                 if (finetune_dist[i] < min_dist) {
 165                         min_dist = finetune_dist[i];
 166                         *min_x = finetune_x[i];
 167                         *min_y = finetune_y[i];
 168                         optimized = 1;
 169                 }
 170         }
 171         if (optimized)
 172                 goto tune_again;
 173
 174         if (log_debug) {
 175                 fprintf(debug_fp, "\t\t\t</coordinates>\n");
 176                 fprintf(debug_fp, "\t\t</LineString>\n");
 177                 fprintf(debug_fp, "\t</Placemark>\n");
 178                 fprintf(debug_fp, "</Folder>\n");
 179         }
 180
 181         return 0;
 182 }