1 // convex_hull.cxx -- calculate the convex hull of a set of points
3 // Written by Curtis Olson, started September 1998.
5 // Copyright (C) 1998 Curtis L. Olson - curt@me.umn.edu
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.
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.
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.
22 // (Log is kept at end of this file)
32 #ifdef NEEDNAMESPACESTD
36 #include <Include/fg_constants.h>
38 #include "convex_hull.hxx"
39 #include "point2d.hxx"
43 typedef map < double, double, less<double> > map_container;
44 typedef map_container::iterator map_iterator;
47 // Calculate theta of angle (a, b, c)
48 double calc_angle(point2d a, point2d b, point2d c) {
50 double udist, vdist, uv_dot, tmp;
52 // u . v = ||u|| * ||v|| * cos(theta)
56 udist = sqrt( u.x * u.x + u.y * u.y );
57 // printf("udist = %.6f\n", udist);
61 vdist = sqrt( v.x * v.x + v.y * v.y );
62 // printf("vdist = %.6f\n", vdist);
64 uv_dot = u.x * v.x + u.y * v.y;
65 // printf("uv_dot = %.6f\n", uv_dot);
67 tmp = uv_dot / (udist * vdist);
68 // printf("tmp = %.6f\n", tmp);
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;
79 origin.x = origin.y = 0.0;
81 a.x = cos(Pa.theta) * Pa.dist;
82 a.y = sin(Pa.theta) * Pa.dist;
84 b.x = cos(Pb.theta) * Pb.dist;
85 b.y = sin(Pb.theta) * Pb.dist;
87 c.x = cos(Pc.theta) * Pc.dist;
88 c.y = sin(Pc.theta) * Pc.dist;
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);
94 a1 = calc_angle(a, b, origin);
95 a2 = calc_angle(origin, b, c);
97 // printf("a1 = %.2f a2 = %.2f\n", a1 * RAD_TO_DEG, a2 * RAD_TO_DEG);
99 return ( (a1 + a2) < FG_PI );
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 )
108 list_iterator current, last;
109 map_iterator map_current, map_next, map_next_next, map_last;
111 // list of translated points
112 list_container trans_list;
114 // points sorted by radian degrees
115 map_container radians_map;
117 // will contain the convex hull
118 list_container con_hull;
120 point2d p, average, Pa, Pb, Pc, result;
122 int in_count, last_size;
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();
130 for ( ; current != last ; ++current ) {
131 sum_x += (*current).x;
132 sum_y += (*current).y;
135 average.x = sum_x / in_count;
136 average.y = sum_y / in_count;
138 // printf("Average center point is %.4f %.4f\n", average.x, average.y);
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() );
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);
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() );
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;
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;
171 // printf("p is %.6f %.6f\n", p.x, p.y);
174 // STEP FOUR: traverse the sorted list and eliminate everything
175 // not on the perimeter.
176 // printf("Traversing list\n");
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 cout << "convex hull not possible with < 3 points" << endl;
185 // ensure that we run the while loop at least once
186 last_size = radians_map.size() + 1;
188 while ( last_size > radians_map.size() ) {
189 // printf("Running an iteration of the graham scan algorithm\n");
190 last_size = radians_map.size();
192 map_current = radians_map.begin();
193 while ( map_current != radians_map.end() ) {
195 Pa.theta = (*map_current).first;
196 Pa.dist = (*map_current).second;
198 // get second element
199 map_next = map_current;
201 if ( map_next == radians_map.end() ) {
202 map_next = radians_map.begin();
204 Pb.theta = (*map_next).first;
205 Pb.dist = (*map_next).second;
208 map_next_next = map_next;
210 if ( map_next_next == radians_map.end() ) {
211 map_next_next = radians_map.begin();
213 Pc.theta = (*map_next_next).first;
214 Pc.dist = (*map_next_next).second;
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);
220 if ( test_point(Pa, Pb, Pc) ) {
221 // printf("Accepted a point\n");
222 // accept point, advance Pa, Pb, and Pc.
225 // printf("REJECTED A POINT\n");
226 // reject point, delete it and advance only Pb and Pc
227 map_next = map_current;
229 if ( map_next == radians_map.end() ) {
230 map_next = radians_map.begin();
232 radians_map.erase( map_next );
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;
246 result.x = cos(p.theta) * p.dist + average.x;
247 result.y = sin(p.theta) * p.dist + average.y;
249 // printf("%.6f %.6f\n", result.x, result.y);
251 con_hull.push_back(result);
259 // Revision 1.5 1999/02/25 21:32:48 curt
260 // Modified to adhere to new polygon naming convention, and also to read the
261 // new Robin Peel aiport format.
263 // Revision 1.4 1998/09/17 18:40:42 curt
264 // Debug message tweaks.
266 // Revision 1.3 1998/09/09 20:59:55 curt
267 // Loop construct tweaks for STL usage.
268 // Output airport file to be used to generate airport scenery on the fly
269 // by the run time sim.
271 // Revision 1.2 1998/09/09 16:26:32 curt
272 // Continued progress in implementing the convex hull algorithm.
274 // Revision 1.1 1998/09/04 23:04:51 curt
275 // Beginning of convex hull genereration routine.