git.sesse.net Git - wardrive/blob - main.cpp

   1 #include <stdio.h>
   2 #include <math.h>
   3 #include <vector>
   4 #include <algorithm>
   5
   6 #define EPS 1e-12
   7 #define WIDTH 1280
   8 #define HEIGHT 1024
   9
  10 float nom[WIDTH * HEIGHT], denom[WIDTH * HEIGHT];
  11 unsigned char pix[WIDTH * HEIGHT * 3];
  12
  13 struct point {
  14         double lat, lon;
  15         double strength;
  16
  17         int x, y;
  18         double radius;
  19
  20         bool operator< (const point &other) const
  21         {
  22                 if (fabs(lat - other.lat) > EPS)
  23                         return (lat < other.lat);
  24                 if (fabs(lon - other.lon) > EPS)
  25                         return (lon < other.lon);
  26                 return (strength < other.strength);
  27         }
  28
  29         bool same_place_as(const point &other) const
  30         {
  31                 return !((fabs(lat - other.lat) > EPS) || (fabs(lon - other.lon) > EPS));
  32         }
  33 };
  34
  35 void collapse_points(std::vector<point> &points, std::vector<point> &upoints)
  36 {
  37         // average non-unique points
  38         point cp = { 0.0, 0.0, 0 };
  39         unsigned np = 0;
  40
  41         for (std::vector<point>::const_iterator i = points.begin(); i != points.end(); ++i) {
  42                 if (np == 0) {
  43                         // first point
  44                         cp = *i;
  45                         np = 1;
  46                         continue;
  47                 }
  48                 if (i->same_place_as(cp)) {
  49                         cp.strength += i->strength;
  50                         ++np;
  51                 } else {
  52                         cp.strength /= np;
  53                         upoints.push_back(cp);
  54
  55                         cp = *i;
  56                         np = 1;
  57                 }
  58         }
  59
  60         cp.strength /= np;
  61         upoints.push_back(cp);
  62 }
  63
  64 int main()
  65 {
  66         std::vector<point> points, upoints;
  67         double max_lat = -HUGE_VAL, min_lat = HUGE_VAL;
  68         double max_lon = -HUGE_VAL, min_lon = HUGE_VAL;
  69
  70         for ( ;; ) {
  71                 struct point p;
  72                 if (scanf("%lf %lf %lf", &p.lat, &p.lon, &p.strength) != 3)
  73                         break;
  74
  75                 max_lon = std::max(max_lon, p.lon);
  76                 min_lon = std::min(min_lon, p.lon);
  77
  78                 max_lat = std::max(max_lat, p.lat);
  79                 min_lat = std::min(min_lat, p.lat);
  80
  81                 points.push_back(p);
  82         }
  83
  84         double x_res = WIDTH / (max_lon - min_lon);
  85         double y_res = HEIGHT / (max_lat - min_lat);
  86
  87         if (y_res > x_res) {
  88                 double lat_dist = max_lat - min_lat;
  89                 double lat_wanted = HEIGHT / x_res;
  90                 double extra_lat = lat_wanted - lat_dist;
  91
  92                 min_lat -= extra_lat * 0.5;
  93                 max_lat += extra_lat * 0.5;
  94
  95                 y_res = HEIGHT / (max_lat - min_lat);
  96         } else {
  97                 fprintf(stderr, "FIXME\n");
  98         }
  99
 100         std::sort(points.begin(), points.end());
 101         collapse_points(points, upoints);
 102
 103         // map to x/y
 104         for (std::vector<point>::iterator i = upoints.begin(); i != upoints.end(); ++i) {
 105                 i->x = (int)((i->lon - min_lon) * WIDTH  / (max_lon - min_lon));
 106                 i->y = (int)((i->lat - min_lat) * HEIGHT / (max_lat - min_lat));
 107                 i->strength = 0.02 * (i->strength + 80);
 108                 if (i->strength < 0.0)
 109                         i->strength = 0.0;
 110         }
 111
 112         // find the radius
 113         for (std::vector<point>::iterator i = upoints.begin(); i != upoints.end(); ++i) {
 114                 int min_rad = 65536;
 115                 for (std::vector<point>::iterator j = upoints.begin(); j != upoints.end(); ++j) {
 116                         int rad =
 117                                 (i->x - j->x) * (i->x - j->x) +
 118                                 (i->y - j->y) * (i->y - j->y);
 119                         if (rad == 0)
 120                                 continue;
 121                         min_rad = std::min(min_rad, rad);
 122                 }
 123
 124                 i->radius = std::min(sqrt(min_rad), 15.0);
 125         }
 126
 127         // clear the drawing
 128         std::fill(nom, nom + WIDTH*HEIGHT, 0.0);
 129         std::fill(denom, denom + WIDTH*HEIGHT, 0.001);
 130
 131         for (std::vector<point>::iterator i = upoints.begin(); i != upoints.end(); ++i) {
 132                 double r = i->radius * 3.0;
 133                 double sigma = i->radius;
 134
 135                 for (int y = -r; y <= r; ++y) {
 136                         for (int x = -r; x <= r; ++x) {
 137                                 int rx = i->x + x;
 138                                 int ry = i->y + y;
 139
 140                                 if (rx < 0 || ry < 0 || rx >= WIDTH || ry >= HEIGHT)
 141                                         continue;
 142
 143                                 double z = - (x*x + y*y) / (2.0 * sigma * sigma);
 144                                 double fac = exp(z) / sigma;
 145
 146                                 nom  [ry * WIDTH + rx] += i->strength * fac;
 147                                 denom[ry * WIDTH + rx] += fac;
 148                         }
 149                 }
 150         }
 151
 152         // draw!
 153         for (unsigned y = 0; y < HEIGHT; ++y) {
 154                 for (unsigned x = 0; x < WIDTH; ++x) {
 155                         unsigned ch;
 156                         if (denom[y * WIDTH + x] < EPS)
 157                                 ch = 0;
 158                         else {
 159                                 ch = (unsigned)(255.0 * nom[y * WIDTH + x] / denom[y * WIDTH + x]);
 160                                 if (ch > 255)
 161                                         ch = 255;
 162                         }
 163
 164                         pix[(y * WIDTH + x) * 3]     = ch;
 165                         pix[(y * WIDTH + x) * 3 + 1] = ch;
 166                         pix[(y * WIDTH + x) * 3 + 2] = ch;
 167                 }
 168         }
 169
 170         // add the track
 171         FILE *track = fopen("track.txt", "r");
 172         for ( ;; ) {
 173                 double lat, lon;
 174                 if (fscanf(track, "%lf %lf", &lat, &lon) != 2)
 175                         break;
 176
 177                 int x = (int)((lon - min_lon) * WIDTH  / (max_lon - min_lon));
 178                 int y = (int)((lat - min_lat) * HEIGHT / (max_lat - min_lat));
 179
 180                 if (x < 0 || y < 0 || x >= WIDTH || y >= HEIGHT)
 181                         continue;
 182                 pix[(y * WIDTH + x) * 3] = 255;
 183         }
 184
 185         // and the sampling points
 186         for (std::vector<point>::iterator i = upoints.begin(); i != upoints.end(); ++i) {
 187                 int x = (int)((i->lon - min_lon) * WIDTH  / (max_lon - min_lon));
 188                 int y = (int)((i->lat - min_lat) * HEIGHT / (max_lat - min_lat));
 189
 190                 if (x < 0 || y < 0 || x >= WIDTH || y >= HEIGHT)
 191                         continue;
 192                 pix[(y * WIDTH + x) * 3 + 2] = 255;
 193         }
 194
 195         // and write PPM
 196         printf("P6\n%u %u\n255\n", WIDTH, HEIGHT);
 197         fwrite(pix, 3, WIDTH*HEIGHT, stdout);
 198 }