Continued progress in implementing the convex hull algorithm.
This commit is contained in:
curt
parent
aa77cd6079
commit
5b1b93bf87
4 changed files with 275 additions and 32 deletions
|
|
@ -83,7 +83,7 @@ batch_cart_to_polar_2d( list < point2d > in_list)
|
|||
while ( current != last ) {
|
||||
p = cart_to_polar_2d( *current );
|
||||
out_list.push_back(p);
|
||||
current++;
|
||||
++current;
|
||||
}
|
||||
|
||||
return out_list;
|
||||
|
|
@ -116,7 +116,7 @@ gen_area(point2d origin, double angle, list < point2d > cart_list)
|
|||
last = rad_list.end();
|
||||
while ( current != last ) {
|
||||
printf("(%.2f, %.2f)\n", current->theta, current->dist);
|
||||
current++;
|
||||
++current;
|
||||
}
|
||||
printf("\n");
|
||||
*/
|
||||
|
|
@ -131,7 +131,7 @@ gen_area(point2d origin, double angle, list < point2d > cart_list)
|
|||
current->theta -= FG_2PI;
|
||||
}
|
||||
// printf("(%.2f, %.2f)\n", current->theta, current->dist);
|
||||
current++;
|
||||
++current;
|
||||
}
|
||||
// printf("\n");
|
||||
|
||||
|
|
@ -147,7 +147,7 @@ gen_area(point2d origin, double angle, list < point2d > cart_list)
|
|||
p.lat *= RAD_TO_DEG;
|
||||
// printf("(%.8f, %.8f)\n", p.lon, p.lat);
|
||||
result_list.push_back(p);
|
||||
current++;
|
||||
++current;
|
||||
}
|
||||
// printf("\n");
|
||||
|
||||
|
|
@ -193,7 +193,7 @@ gen_runway_area( double lon, double lat, double heading,
|
|||
last = tmp_list.end();
|
||||
while ( current != last ) {
|
||||
printf("(%.2f, %.2f)\n", current->x, current->y);
|
||||
current++;
|
||||
++current;
|
||||
}
|
||||
printf("\n");
|
||||
*/
|
||||
|
|
@ -208,7 +208,7 @@ gen_runway_area( double lon, double lat, double heading,
|
|||
last = result_list.end();
|
||||
while ( current != last ) {
|
||||
printf("(%.8f, %.8f)\n", current->lon, current->lat);
|
||||
current++;
|
||||
++current;
|
||||
}
|
||||
printf("\n");
|
||||
*/
|
||||
|
|
@ -218,6 +218,9 @@ gen_runway_area( double lon, double lat, double heading,
|
|||
|
||||
|
||||
// $Log$
|
||||
// Revision 1.3 1998/09/09 16:26:31 curt
|
||||
// Continued progress in implementing the convex hull algorithm.
|
||||
//
|
||||
// Revision 1.2 1998/09/04 23:04:48 curt
|
||||
// Beginning of convex hull genereration routine.
|
||||
//
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@
|
|||
//
|
||||
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <list>
|
||||
|
|
@ -32,15 +33,80 @@
|
|||
using namespace std;
|
||||
#endif
|
||||
|
||||
#include <Include/fg_constants.h>
|
||||
|
||||
#include "convex_hull.hxx"
|
||||
#include "point2d.hxx"
|
||||
|
||||
|
||||
// stl map typedefs
|
||||
typedef map < double, double, less<double> > map_container;
|
||||
typedef map_container::iterator map_iterator;
|
||||
|
||||
|
||||
// Calculate theta of angle (a, b, c)
|
||||
double calc_angle(point2d a, point2d b, point2d c) {
|
||||
point2d u, v;
|
||||
double udist, vdist, uv_dot, tmp;
|
||||
|
||||
// u . v = ||u|| * ||v|| * cos(theta)
|
||||
|
||||
u.x = b.x - a.x;
|
||||
u.y = b.y - a.y;
|
||||
udist = sqrt( u.x * u.x + u.y * u.y );
|
||||
// printf("udist = %.6f\n", udist);
|
||||
|
||||
v.x = b.x - c.x;
|
||||
v.y = b.y - c.y;
|
||||
vdist = sqrt( v.x * v.x + v.y * v.y );
|
||||
// printf("vdist = %.6f\n", vdist);
|
||||
|
||||
uv_dot = u.x * v.x + u.y * v.y;
|
||||
// printf("uv_dot = %.6f\n", uv_dot);
|
||||
|
||||
tmp = uv_dot / (udist * vdist);
|
||||
// printf("tmp = %.6f\n", tmp);
|
||||
|
||||
return acos(tmp);
|
||||
}
|
||||
|
||||
|
||||
// Test to see if angle(Pa, Pb, Pc) < 180 degrees
|
||||
bool test_point(point2d Pa, point2d Pb, point2d Pc) {
|
||||
point2d origin, a, b, c;
|
||||
double a1, a2;
|
||||
|
||||
origin.x = origin.y = 0.0;
|
||||
|
||||
a.x = cos(Pa.theta) * Pa.dist;
|
||||
a.y = sin(Pa.theta) * Pa.dist;
|
||||
|
||||
b.x = cos(Pb.theta) * Pb.dist;
|
||||
b.y = sin(Pb.theta) * Pb.dist;
|
||||
|
||||
c.x = cos(Pc.theta) * Pc.dist;
|
||||
c.y = sin(Pc.theta) * Pc.dist;
|
||||
|
||||
// printf("a is %.6f %.6f\n", a.x, a.y);
|
||||
// printf("b is %.6f %.6f\n", b.x, b.y);
|
||||
// printf("c is %.6f %.6f\n", c.x, c.y);
|
||||
|
||||
a1 = calc_angle(a, b, origin);
|
||||
a2 = calc_angle(origin, b, c);
|
||||
|
||||
printf("a1 = %.2f a2 = %.2f\n", a1 * RAD_TO_DEG, a2 * RAD_TO_DEG);
|
||||
|
||||
return ( (a1 + a2) < FG_PI );
|
||||
}
|
||||
|
||||
|
||||
// calculate the convex hull of a set of points, return as a list of
|
||||
// point2d
|
||||
// point2d. The algorithm description can be found at:
|
||||
// http://riot.ieor.berkeley.edu/riot/Applications/ConvexHull/CHDetails.html
|
||||
list_container convex_hull( list_container input_list )
|
||||
{
|
||||
list_iterator current, last;
|
||||
map_iterator map_current, map_last;
|
||||
map_iterator map_current, map_next, map_next_next, map_last;
|
||||
|
||||
// list of translated points
|
||||
list_container trans_list;
|
||||
|
|
@ -51,9 +117,9 @@ list_container convex_hull( list_container input_list )
|
|||
// will contain the convex hull
|
||||
list_container con_hull;
|
||||
|
||||
point2d p, average;
|
||||
point2d p, average, Pa, Pb, Pc, result;
|
||||
double sum_x, sum_y;
|
||||
int in_count;
|
||||
int in_count, last_size;
|
||||
|
||||
// STEP ONE: Find an average midpoint of the input set of points
|
||||
current = input_list.begin();
|
||||
|
|
@ -65,7 +131,7 @@ list_container convex_hull( list_container input_list )
|
|||
sum_x += (*current).x;
|
||||
sum_y += (*current).y;
|
||||
|
||||
current++;
|
||||
++current;
|
||||
}
|
||||
|
||||
average.x = sum_x / in_count;
|
||||
|
|
@ -81,9 +147,9 @@ list_container convex_hull( list_container input_list )
|
|||
while ( current != last ) {
|
||||
p.x = (*current).x - average.x;
|
||||
p.y = (*current).y - average.y;
|
||||
printf("p is %.6f %.6f\n", p.x, p.y);
|
||||
printf("%.6f %.6f\n", p.x, p.y);
|
||||
trans_list.push_back(p);
|
||||
current++;
|
||||
++current;
|
||||
}
|
||||
|
||||
// STEP THREE: convert to radians and sort by theta
|
||||
|
|
@ -93,8 +159,10 @@ list_container convex_hull( list_container input_list )
|
|||
|
||||
while ( current != last ) {
|
||||
p = cart_to_polar_2d(*current);
|
||||
radians_map[p.theta] = p.dist;
|
||||
current++;
|
||||
if ( p.dist > radians_map[p.theta] ) {
|
||||
radians_map[p.theta] = p.dist;
|
||||
}
|
||||
++current;
|
||||
}
|
||||
|
||||
printf("Sorted list\n");
|
||||
|
|
@ -106,7 +174,89 @@ list_container convex_hull( list_container input_list )
|
|||
|
||||
printf("p is %.6f %.6f\n", p.x, p.y);
|
||||
|
||||
map_current++;
|
||||
++map_current;
|
||||
}
|
||||
|
||||
// STEP FOUR: traverse the sorted list and eliminate everything
|
||||
// not on the perimeter.
|
||||
printf("Traversing list\n");
|
||||
|
||||
// double check list size ... this should never fail because a
|
||||
// single runway will always generate four points.
|
||||
if ( radians_map.size() < 3 ) {
|
||||
printf("convex hull not possible with < 3 points\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
// ensure that we run the while loop at least once
|
||||
last_size = radians_map.size() + 1;
|
||||
|
||||
while ( last_size > radians_map.size() ) {
|
||||
printf("Running an iteration of the graham scan algorithm\n");
|
||||
last_size = radians_map.size();
|
||||
|
||||
map_current = radians_map.begin();
|
||||
while ( map_current != radians_map.end() ) {
|
||||
// get first element
|
||||
Pa.theta = (*map_current).first;
|
||||
Pa.dist = (*map_current).second;
|
||||
|
||||
// get second element
|
||||
map_next = map_current;
|
||||
++map_next;
|
||||
if ( map_next == radians_map.end() ) {
|
||||
map_next = radians_map.begin();
|
||||
}
|
||||
Pb.theta = (*map_next).first;
|
||||
Pb.dist = (*map_next).second;
|
||||
|
||||
// get third element
|
||||
map_next_next = map_next;
|
||||
++map_next_next;
|
||||
if ( map_next_next == radians_map.end() ) {
|
||||
map_next_next = radians_map.begin();
|
||||
}
|
||||
Pc.theta = (*map_next_next).first;
|
||||
Pc.dist = (*map_next_next).second;
|
||||
|
||||
// printf("Pa is %.6f %.6f\n", Pa.theta, Pa.dist);
|
||||
// printf("Pb is %.6f %.6f\n", Pb.theta, Pb.dist);
|
||||
// printf("Pc is %.6f %.6f\n", Pc.theta, Pc.dist);
|
||||
|
||||
if ( test_point(Pa, Pb, Pc) ) {
|
||||
printf("Accepted a point\n");
|
||||
// accept point, advance Pa, Pb, and Pc.
|
||||
++map_current;
|
||||
} else {
|
||||
printf("REJECTED A POINT\n");
|
||||
// reject point, delete it and advance only Pb and Pc
|
||||
map_next = map_current;
|
||||
++map_next;
|
||||
if ( map_next == radians_map.end() ) {
|
||||
map_next = radians_map.begin();
|
||||
}
|
||||
radians_map.erase( map_next );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// translate back to correct lon/lat
|
||||
printf("Final sorted convex hull\n");
|
||||
con_hull.erase( con_hull.begin(), con_hull.end() );
|
||||
map_current = radians_map.begin();
|
||||
map_last = radians_map.end();
|
||||
while ( map_current != map_last ) {
|
||||
p.theta = (*map_current).first;
|
||||
p.dist = (*map_current).second;
|
||||
|
||||
result.x = cos(p.theta) * p.dist + average.x;
|
||||
result.y = sin(p.theta) * p.dist + average.x;
|
||||
|
||||
printf("%.6f %.6f\n", result.x, result.y);
|
||||
|
||||
con_hull.push_back(result);
|
||||
|
||||
++map_current;
|
||||
}
|
||||
|
||||
return con_hull;
|
||||
|
|
@ -114,6 +264,9 @@ list_container convex_hull( list_container input_list )
|
|||
|
||||
|
||||
// $Log$
|
||||
// Revision 1.2 1998/09/09 16:26:32 curt
|
||||
// Continued progress in implementing the convex hull algorithm.
|
||||
//
|
||||
// Revision 1.1 1998/09/04 23:04:51 curt
|
||||
// Beginning of convex hull genereration routine.
|
||||
//
|
||||
|
|
|
|||
|
|
@ -28,7 +28,6 @@
|
|||
|
||||
|
||||
#include <list>
|
||||
#include <map>
|
||||
|
||||
#ifdef NEEDNAMESPACESTD
|
||||
using namespace std;
|
||||
|
|
@ -41,13 +40,10 @@ using namespace std;
|
|||
typedef list < point2d > list_container;
|
||||
typedef list_container::iterator list_iterator;
|
||||
|
||||
// stl mapp typedefs
|
||||
typedef map < double, double, less<double> > map_container;
|
||||
typedef map_container::iterator map_iterator;
|
||||
|
||||
|
||||
// calculate the convex hull of a set of points, return as a list of
|
||||
// point2d
|
||||
// point2d. The algorithm description can be found at:
|
||||
// http://riot.ieor.berkeley.edu/riot/Applications/ConvexHull/CHDetails.html
|
||||
list_container convex_hull( list_container input_list );
|
||||
|
||||
|
||||
|
|
@ -55,6 +51,9 @@ list_container convex_hull( list_container input_list );
|
|||
|
||||
|
||||
// $Log$
|
||||
// Revision 1.2 1998/09/09 16:26:33 curt
|
||||
// Continued progress in implementing the convex hull algorithm.
|
||||
//
|
||||
// Revision 1.1 1998/09/04 23:04:51 curt
|
||||
// Beginning of convex hull genereration routine.
|
||||
//
|
||||
|
|
|
|||
|
|
@ -45,16 +45,25 @@
|
|||
|
||||
|
||||
// process and airport + runway list
|
||||
void process_airport( string last_airport, list < string > & runway_list ) {
|
||||
list < point2d > rwy_list, apt_list;
|
||||
list < point2d > :: iterator current;
|
||||
list < point2d > :: iterator last;
|
||||
void process_airport( string last_airport, list < string > & runway_list,
|
||||
const string& root ) {
|
||||
list_container rwy_list, apt_list, hull_list;
|
||||
list_iterator current, last;
|
||||
|
||||
string line_str;
|
||||
double lon, lat;
|
||||
int len, width, hdg, label_hdg, elev;
|
||||
char codes[10];
|
||||
char side;
|
||||
point2d average;
|
||||
double sum_x, sum_y;
|
||||
|
||||
FILE *fd;
|
||||
fgBUCKET b;
|
||||
long int index;
|
||||
char base[256], tmp[256];
|
||||
string path, command, exfile, file;
|
||||
int i, count;
|
||||
|
||||
printf( "(apt) %s", last_airport.c_str() );
|
||||
|
||||
|
|
@ -76,21 +85,100 @@ void process_airport( string last_airport, list < string > & runway_list ) {
|
|||
last = rwy_list.end();
|
||||
while ( current != last ) {
|
||||
apt_list.push_back(*current);
|
||||
current++;
|
||||
++current;
|
||||
}
|
||||
}
|
||||
|
||||
printf("Final results in degrees\n");
|
||||
printf("Runway points in degrees\n");
|
||||
current = apt_list.begin();
|
||||
last = apt_list.end();
|
||||
while ( current != last ) {
|
||||
// printf( "(%.4f, %.4f)\n",
|
||||
printf( "%.5f %.5f\n", current->lon, current->lat );
|
||||
current++;
|
||||
++current;
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
convex_hull(apt_list);
|
||||
// generate convex hull
|
||||
hull_list = convex_hull(apt_list);
|
||||
|
||||
// find average center point of convex hull
|
||||
count = hull_list.size();
|
||||
|
||||
current = hull_list.begin();
|
||||
last = hull_list.end();
|
||||
sum_x = sum_y = 0.0;
|
||||
while ( current != last ) {
|
||||
sum_x += (*current).x;
|
||||
sum_y += (*current).y;
|
||||
|
||||
++current;
|
||||
}
|
||||
|
||||
average.x = sum_x / count;
|
||||
average.y = sum_y / count;
|
||||
|
||||
// find bucket based on first point in hull list. Eventually
|
||||
// we'll need to handle cases where the area crosses bucket
|
||||
// boundaries.
|
||||
fgBucketFind( (*current).lon, (*current).lat, &b);
|
||||
printf( "Bucket = lon,lat = %d,%d x,y index = %d,%d\n",
|
||||
b.lon, b.lat, b.x, b.y);
|
||||
|
||||
index = fgBucketGenIndex(&b);
|
||||
fgBucketGenBasePath(&b, base);
|
||||
path = root + "/Scenery/" + base;
|
||||
command = "mkdir -p " + path;
|
||||
system( command.c_str() );
|
||||
|
||||
sprintf(tmp, "%ld", index);
|
||||
exfile = path + "/" + tmp + ".node.ex";
|
||||
file = path + "/" + tmp + ".poly";
|
||||
printf( "extra node file = %s\n", exfile.c_str() );
|
||||
printf( "poly file = %s\n", file.c_str() );
|
||||
|
||||
// output exclude nodes
|
||||
printf("Output exclude nodes\n");
|
||||
if ( (fd = fopen(exfile.c_str(), "w")) == NULL ) {
|
||||
printf("Cannot open file: %s\n", exfile.c_str());
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
fprintf( fd, "%d 2 0 0\n", count );
|
||||
|
||||
current = hull_list.begin();
|
||||
last = hull_list.end();
|
||||
i = 1;
|
||||
while ( current != last ) {
|
||||
// printf( "(%.4f, %.4f)\n",
|
||||
fprintf( fd, "%d %.2f %.2f %.2f\n", i,
|
||||
(*current).lon * 3600.0, (*current).lat * 3600.0, elev);
|
||||
++current;
|
||||
++i;
|
||||
}
|
||||
fclose(fd);
|
||||
|
||||
// output poly
|
||||
if ( (fd = fopen(file.c_str(), "w")) == NULL ) {
|
||||
printf("Cannot open file: %s\n", file.c_str());
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
// output empty node list
|
||||
fprintf(fd, "0 2 0 0\n");
|
||||
|
||||
// output segments
|
||||
fprintf( fd, "%d 0\n", count );
|
||||
for ( i = 1; i < count; i++ ) {
|
||||
fprintf( fd, "%d %d %d\n", i, i, i + 1 );
|
||||
}
|
||||
fprintf( fd, "%d %d %d\n", count, count, 1 );
|
||||
|
||||
// output hole center
|
||||
fprintf( fd, "1\n");
|
||||
fprintf( fd, "1 %.2f %.2f\n", average.x * 3600.0, average.y * 3600);
|
||||
|
||||
fclose(fd);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -142,7 +230,7 @@ int main( int argc, char **argv ) {
|
|||
|
||||
if ( last_airport.length() ) {
|
||||
// process previous record
|
||||
process_airport(last_airport, runway_list);
|
||||
process_airport(last_airport, runway_list, argv[2]);
|
||||
}
|
||||
|
||||
last_airport = airport;
|
||||
|
|
@ -151,7 +239,7 @@ int main( int argc, char **argv ) {
|
|||
|
||||
if ( last_airport.length() ) {
|
||||
// process previous record
|
||||
process_airport(last_airport, runway_list);
|
||||
process_airport(last_airport, runway_list, argv[2]);
|
||||
}
|
||||
|
||||
fgclose(f);
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue