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 while ( current != last ) {
131 sum_x += (*current).x;
132 sum_y += (*current).y;
137 average.x = sum_x / in_count;
138 average.y = sum_y / in_count;
140 printf("Average center point is %.4f %.4f\n", average.x, average.y);
142 // STEP TWO: Translate input points so average is at origin
143 current = input_list.begin();
144 last = input_list.end();
145 trans_list.erase( trans_list.begin(), trans_list.end() );
147 while ( current != last ) {
148 p.x = (*current).x - average.x;
149 p.y = (*current).y - average.y;
150 printf("%.6f %.6f\n", p.x, p.y);
151 trans_list.push_back(p);
155 // STEP THREE: convert to radians and sort by theta
156 current = trans_list.begin();
157 last = trans_list.end();
158 radians_map.erase( radians_map.begin(), radians_map.end() );
160 while ( current != last ) {
161 p = cart_to_polar_2d(*current);
162 if ( p.dist > radians_map[p.theta] ) {
163 radians_map[p.theta] = p.dist;
168 printf("Sorted list\n");
169 map_current = radians_map.begin();
170 map_last = radians_map.end();
171 while ( map_current != map_last ) {
172 p.x = (*map_current).first;
173 p.y = (*map_current).second;
175 printf("p is %.6f %.6f\n", p.x, p.y);
180 // STEP FOUR: traverse the sorted list and eliminate everything
181 // not on the perimeter.
182 printf("Traversing list\n");
184 // double check list size ... this should never fail because a
185 // single runway will always generate four points.
186 if ( radians_map.size() < 3 ) {
187 printf("convex hull not possible with < 3 points\n");
191 // ensure that we run the while loop at least once
192 last_size = radians_map.size() + 1;
194 while ( last_size > radians_map.size() ) {
195 printf("Running an iteration of the graham scan algorithm\n");
196 last_size = radians_map.size();
198 map_current = radians_map.begin();
199 while ( map_current != radians_map.end() ) {
201 Pa.theta = (*map_current).first;
202 Pa.dist = (*map_current).second;
204 // get second element
205 map_next = map_current;
207 if ( map_next == radians_map.end() ) {
208 map_next = radians_map.begin();
210 Pb.theta = (*map_next).first;
211 Pb.dist = (*map_next).second;
214 map_next_next = map_next;
216 if ( map_next_next == radians_map.end() ) {
217 map_next_next = radians_map.begin();
219 Pc.theta = (*map_next_next).first;
220 Pc.dist = (*map_next_next).second;
222 // printf("Pa is %.6f %.6f\n", Pa.theta, Pa.dist);
223 // printf("Pb is %.6f %.6f\n", Pb.theta, Pb.dist);
224 // printf("Pc is %.6f %.6f\n", Pc.theta, Pc.dist);
226 if ( test_point(Pa, Pb, Pc) ) {
227 printf("Accepted a point\n");
228 // accept point, advance Pa, Pb, and Pc.
231 printf("REJECTED A POINT\n");
232 // reject point, delete it and advance only Pb and Pc
233 map_next = map_current;
235 if ( map_next == radians_map.end() ) {
236 map_next = radians_map.begin();
238 radians_map.erase( map_next );
243 // translate back to correct lon/lat
244 printf("Final sorted convex hull\n");
245 con_hull.erase( con_hull.begin(), con_hull.end() );
246 map_current = radians_map.begin();
247 map_last = radians_map.end();
248 while ( map_current != map_last ) {
249 p.theta = (*map_current).first;
250 p.dist = (*map_current).second;
252 result.x = cos(p.theta) * p.dist + average.x;
253 result.y = sin(p.theta) * p.dist + average.x;
255 printf("%.6f %.6f\n", result.x, result.y);
257 con_hull.push_back(result);
267 // Revision 1.2 1998/09/09 16:26:32 curt
268 // Continued progress in implementing the convex hull algorithm.
270 // Revision 1.1 1998/09/04 23:04:51 curt
271 // Beginning of convex hull genereration routine.