GenAirports/convex_hull.cxx

   1 // convex_hull.cxx -- calculate the convex hull of a set of points
   2 //
   3 // Written by Curtis Olson, started September 1998.
   4 //
   5 // Copyright (C) 1998  Curtis L. Olson  - curt@me.umn.edu
   6 //
   7 // This program is free software; you can redistribute it and/or modify
   8 // it under the terms of the GNU General Public License as published by
   9 // the Free Software Foundation; either version 2 of the License, or
  10 // (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful,
  13 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 // GNU General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22 // (Log is kept at end of this file)
  23 //
  24
  25
  26 #include <math.h>
  27 #include <stdio.h>
  28
  29 #include <list>
  30 #include <map>
  31
  32 #ifdef NEEDNAMESPACESTD
  33 using namespace std;
  34 #endif
  35
  36 #include <Include/fg_constants.h>
  37
  38 #include "convex_hull.hxx"
  39 #include "point2d.hxx"
  40
  41
  42 // stl map typedefs
  43 typedef map < double, double, less<double> > map_container;
  44 typedef map_container::iterator map_iterator;
  45
  46
  47 // Calculate theta of angle (a, b, c)
  48 double calc_angle(point2d a, point2d b, point2d c) {
  49     point2d u, v;
  50     double udist, vdist, uv_dot, tmp;
  51
  52     // u . v = ||u|| * ||v|| * cos(theta)
  53
  54     u.x = b.x - a.x;
  55     u.y = b.y - a.y;
  56     udist = sqrt( u.x * u.x + u.y * u.y );
  57     // printf("udist = %.6f\n", udist);
  58
  59     v.x = b.x - c.x;
  60     v.y = b.y - c.y;
  61     vdist = sqrt( v.x * v.x + v.y * v.y );
  62     // printf("vdist = %.6f\n", vdist);
  63
  64     uv_dot = u.x * v.x + u.y * v.y;
  65     // printf("uv_dot = %.6f\n", uv_dot);
  66
  67     tmp = uv_dot / (udist * vdist);
  68     // printf("tmp = %.6f\n", tmp);
  69
  70     return acos(tmp);
  71 }
  72
  73
  74 // Test to see if angle(Pa, Pb, Pc) < 180 degrees
  75 bool test_point(point2d Pa, point2d Pb, point2d Pc) {
  76     point2d origin, a, b, c;
  77     double a1, a2;
  78
  79     origin.x = origin.y = 0.0;
  80
  81     a.x = cos(Pa.theta) * Pa.dist;
  82     a.y = sin(Pa.theta) * Pa.dist;
  83
  84     b.x = cos(Pb.theta) * Pb.dist;
  85     b.y = sin(Pb.theta) * Pb.dist;
  86
  87     c.x = cos(Pc.theta) * Pc.dist;
  88     c.y = sin(Pc.theta) * Pc.dist;
  89
  90     // printf("a is %.6f %.6f\n", a.x, a.y);
  91     // printf("b is %.6f %.6f\n", b.x, b.y);
  92     // printf("c is %.6f %.6f\n", c.x, c.y);
  93
  94     a1 = calc_angle(a, b, origin);
  95     a2 = calc_angle(origin, b, c);
  96
  97     // printf("a1 = %.2f  a2 = %.2f\n", a1 * RAD_TO_DEG, a2 * RAD_TO_DEG);
  98
  99     return ( (a1 + a2) < FG_PI );
 100 }
 101
 102
 103 // calculate the convex hull of a set of points, return as a list of
 104 // point2d.  The algorithm description can be found at:
 105 // http://riot.ieor.berkeley.edu/riot/Applications/ConvexHull/CHDetails.html
 106 list_container convex_hull( list_container input_list )
 107 {
 108     list_iterator current, last;
 109     map_iterator map_current, map_next, map_next_next, map_last;
 110
 111     // list of translated points
 112     list_container trans_list;
 113
 114     // points sorted by radian degrees
 115     map_container radians_map;
 116
 117     // will contain the convex hull
 118     list_container con_hull;
 119
 120     point2d p, average, Pa, Pb, Pc, result;
 121     double sum_x, sum_y;
 122     int in_count, last_size;
 123
 124     // STEP ONE:  Find an average midpoint of the input set of points
 125     current = input_list.begin();
 126     last = input_list.end();
 127     in_count = input_list.size();
 128     sum_x = sum_y = 0.0;
 129
 130     for ( ; current != last ; ++current ) {
 131         sum_x += (*current).x;
 132         sum_y += (*current).y;
 133     }
 134
 135     average.x = sum_x / in_count;
 136     average.y = sum_y / in_count;
 137
 138     // printf("Average center point is %.4f %.4f\n", average.x, average.y);
 139
 140     // STEP TWO:  Translate input points so average is at origin
 141     current = input_list.begin();
 142     last = input_list.end();
 143     trans_list.erase( trans_list.begin(), trans_list.end() );
 144
 145     for ( ; current != last ; ++current ) {
 146         p.x = (*current).x - average.x;
 147         p.y = (*current).y - average.y;
 148         // printf("%.6f %.6f\n", p.x, p.y);
 149         trans_list.push_back(p);
 150     }
 151
 152     // STEP THREE:  convert to radians and sort by theta
 153     current = trans_list.begin();
 154     last = trans_list.end();
 155     radians_map.erase( radians_map.begin(), radians_map.end() );
 156
 157     for ( ; current != last ; ++current) {
 158         p = cart_to_polar_2d(*current);
 159         if ( p.dist > radians_map[p.theta] ) {
 160             radians_map[p.theta] = p.dist;
 161         }
 162     }
 163
 164     // printf("Sorted list\n");
 165     map_current = radians_map.begin();
 166     map_last = radians_map.end();
 167     for ( ; map_current != map_last ; ++map_current ) {
 168         p.x = (*map_current).first;
 169         p.y = (*map_current).second;
 170
 171         // printf("p is %.6f %.6f\n", p.x, p.y);
 172     }
 173
 174     // STEP FOUR: traverse the sorted list and eliminate everything
 175     // not on the perimeter.
 176     // printf("Traversing list\n");
 177
 178     // double check list size ... this should never fail because a
 179     // single runway will always generate four points.
 180     if ( radians_map.size() < 3 ) {
 181         // printf("convex hull not possible with < 3 points\n");
 182         exit(0);
 183     }
 184
 185     // ensure that we run the while loop at least once
 186     last_size = radians_map.size() + 1;
 187
 188     while ( last_size > radians_map.size() ) {
 189         // printf("Running an iteration of the graham scan algorithm\n");
 190         last_size = radians_map.size();
 191
 192         map_current = radians_map.begin();
 193         while ( map_current != radians_map.end() ) {
 194             // get first element
 195             Pa.theta = (*map_current).first;
 196             Pa.dist = (*map_current).second;
 197
 198             // get second element
 199             map_next = map_current;
 200             ++map_next;
 201             if ( map_next == radians_map.end() ) {
 202                 map_next = radians_map.begin();
 203             }
 204             Pb.theta = (*map_next).first;
 205             Pb.dist = (*map_next).second;
 206
 207             // get third element
 208             map_next_next = map_next;
 209             ++map_next_next;
 210             if ( map_next_next == radians_map.end() ) {
 211                 map_next_next = radians_map.begin();
 212             }
 213             Pc.theta = (*map_next_next).first;
 214             Pc.dist = (*map_next_next).second;
 215
 216             // printf("Pa is %.6f %.6f\n", Pa.theta, Pa.dist);
 217             // printf("Pb is %.6f %.6f\n", Pb.theta, Pb.dist);
 218             // printf("Pc is %.6f %.6f\n", Pc.theta, Pc.dist);
 219
 220             if ( test_point(Pa, Pb, Pc) ) {
 221                 // printf("Accepted a point\n");
 222                 // accept point, advance Pa, Pb, and Pc.
 223                 ++map_current;
 224             } else {
 225                 // printf("REJECTED A POINT\n");
 226                 // reject point, delete it and advance only Pb and Pc
 227                 map_next = map_current;
 228                 ++map_next;
 229                 if ( map_next == radians_map.end() ) {
 230                     map_next = radians_map.begin();
 231                 }
 232                 radians_map.erase( map_next );
 233             }
 234         }
 235     }
 236
 237     // translate back to correct lon/lat
 238     // printf("Final sorted convex hull\n");
 239     con_hull.erase( con_hull.begin(), con_hull.end() );
 240     map_current = radians_map.begin();
 241     map_last = radians_map.end();
 242     for ( ; map_current != map_last ; ++map_current ) {
 243         p.theta = (*map_current).first;
 244         p.dist = (*map_current).second;
 245
 246         result.x = cos(p.theta) * p.dist + average.x;
 247         result.y = sin(p.theta) * p.dist + average.y;
 248
 249         // printf("%.6f %.6f\n", result.x, result.y);
 250
 251         con_hull.push_back(result);
 252     }
 253
 254     return con_hull;
 255 }
 256
 257
 258 // $Log$
 259 // Revision 1.4  1998/09/17 18:40:42  curt
 260 // Debug message tweaks.
 261 //
 262 // Revision 1.3  1998/09/09 20:59:55  curt
 263 // Loop construct tweaks for STL usage.
 264 // Output airport file to be used to generate airport scenery on the fly
 265 //   by the run time sim.
 266 //
 267 // Revision 1.2  1998/09/09 16:26:32  curt
 268 // Continued progress in implementing the convex hull algorithm.
 269 //
 270 // Revision 1.1  1998/09/04 23:04:51  curt
 271 // Beginning of convex hull genereration routine.
 272 //
 273 //