From 2df4726f33933ffa02344790c1c7dfd7572b266d Mon Sep 17 00:00:00 2001 From: curt <curt> Date: Mon, 8 Nov 2004 21:59:04 +0000 Subject: [PATCH] Do a least squares nurbs approximation to coarse grid when deriving airport surface. I think I have this working robustly. A few miscellaneous tweaks to handle latest X-Plane data (with a few new runway surface codes we hadn't seen before.) --- src/Airports/GenAirports/apt_surface.cxx | 178 +++++++++++++++++------ src/Airports/GenAirports/build.cxx | 14 +- src/Airports/GenAirports/elevations.cxx | 36 ++--- src/Airports/GenAirports/global.hxx | 11 +- src/Airports/GenAirports/main.cxx | 9 +- 5 files changed, 175 insertions(+), 73 deletions(-) diff --git a/src/Airports/GenAirports/apt_surface.cxx b/src/Airports/GenAirports/apt_surface.cxx index 63ac256f..417e4ab3 100644 --- a/src/Airports/GenAirports/apt_surface.cxx +++ b/src/Airports/GenAirports/apt_surface.cxx @@ -46,8 +46,8 @@ static bool limit_slope( Matrix_Point3Dd &Pts, int i1, int j1, int i2, int j2, bool slope_error = false; Point3Dd p1, p2; - p1 = Pts(j1,i1); - p2 = Pts(j2,i2); + p1 = Pts(i1,j1); + p2 = Pts(i2,j2); double az1, az2, dist; double slope; @@ -77,7 +77,7 @@ static bool limit_slope( Matrix_Point3Dd &Pts, int i1, int j1, int i2, int j2, } else { p1.z() = p2.z() + (dist * slope_max); } - Pts(j1,i1) = p1; + Pts(i1,j1) = p1; } else { // cout << " p2 error larger" << endl; if ( slope > 0 ) { @@ -85,7 +85,7 @@ static bool limit_slope( Matrix_Point3Dd &Pts, int i1, int j1, int i2, int j2, } else { p2.z() = p1.z() - (dist * slope_max); } - Pts(j2,i2) = p2; + Pts(i2,j2) = p2; } // cout << " z1 = " << p1.z() << " z2 = " << p2.z() << endl; } @@ -126,10 +126,18 @@ TGAptSurface::TGAptSurface( const string& path, int xdivs = (int)(x_m / coarse_grid) + 1; int ydivs = (int)(y_m / coarse_grid) + 1; +#if defined( _NURBS_GLOBAL_INTERP ) if ( xdivs < 3 ) { xdivs = 3; } if ( ydivs < 3 ) { ydivs = 3; } - - SG_LOG(SG_GENERAL, SG_DEBUG, " M(" << ydivs << "," << xdivs << ")"); +#elif defined( _NURBS_LEAST_SQUARES ) + // Minimum divs appears to need to be at least 5 before the + // leastsquares nurbs surface approximation stops crashing. + if ( xdivs < 5 ) { xdivs = 5; } + if ( ydivs < 5 ) { ydivs = 5; } +#else +# error "Need to define _NURBS_GLOBAL_INTER or _NURBS_LEAST_SQUARES" +#endif + SG_LOG(SG_GENERAL, SG_INFO, " M(" << ydivs << "," << xdivs << ")"); double dlon = x_deg / xdivs; double dlat = y_deg / ydivs; @@ -140,39 +148,56 @@ TGAptSurface::TGAptSurface( const string& path, // with an added major row column on the NE sides.) int mult = 10; Matrix_Point3Dd dPts( (ydivs + 1) * mult + 1, (xdivs + 1) * mult + 1 ); - for ( int i = 0; i < dPts.cols(); ++i ) { - for ( int j = 0; j < dPts.rows(); ++j ) { - dPts(j,i) = Point3Dd( min_deg.lon() - dlon_h - + i * (dlon / (double)mult), + for ( int j = 0; j < dPts.cols(); ++j ) { + for ( int i = 0; i < dPts.rows(); ++i ) { + dPts(i,j) = Point3Dd( min_deg.lon() - dlon_h + + j * (dlon / (double)mult), min_deg.lat() - dlat_h - + j * (dlat / (double)mult), + + i * (dlat / (double)mult), -9999 ); } } // Lookup the elevations of all the grid points tgCalcElevations( path, elev_src, dPts, 0.0 ); +#ifdef DEBUG + for ( int j = 0; j < dPts.cols(); ++j ) { + for ( int i = 0; i < dPts.rows(); ++i ) { + printf("%.5f %.5f %.1f\n", dPts(i,j).x(), dPts(i,j).y(), + dPts(i,j).z() ); + } + } +#endif // Clamp the elevations against the externally provided average // elevation. tgClampElevations( dPts, average_elev_m, max_clamp ); +#ifdef DEBUG + for ( int j = 0; j < dPts.cols(); ++j ) { + for ( int i = 0; i < dPts.rows(); ++i ) { + printf("%.5f %.5f %.1f\n", dPts(i,j).x(), dPts(i,j).y(), + dPts(i,j).z() ); + } + } +#endif + // Build the normal res input grid from the double res version Matrix_Point3Dd Pts(ydivs + 1, xdivs + 1); double ave_divider = (mult+1) * (mult+1); - for ( int i = 0; i < Pts.cols(); ++i ) { - for ( int j = 0; j < Pts.rows(); ++j ) { + for ( int j = 0; j < Pts.cols(); ++j ) { + for ( int i = 0; i < Pts.rows(); ++i ) { SG_LOG(SG_GENERAL, SG_DEBUG, i << "," << j); double accum = 0.0; double lon_accum = 0.0; double lat_accum = 0.0; - for ( int ii = 0; ii <= mult; ++ii ) { - for ( int jj = 0; jj <= mult; ++jj ) { - double value = dPts(mult*j + jj, mult*i + ii).z(); + for ( int jj = 0; jj <= mult; ++jj ) { + for ( int ii = 0; ii <= mult; ++ii ) { + double value = dPts(mult*i + ii, mult*j + jj).z(); SG_LOG( SG_GENERAL, SG_DEBUG, "value = " << value ); accum += value; - lon_accum += dPts(mult*j + jj, mult*i + ii).x(); - lat_accum += dPts(mult*j + jj, mult*i + ii).y(); + lon_accum += dPts(mult*i + ii, mult*j + jj).x(); + lat_accum += dPts(mult*i + ii, mult*j + jj).y(); } } double val_ave = accum / ave_divider; @@ -180,24 +205,32 @@ TGAptSurface::TGAptSurface( const string& path, double lat_ave = lat_accum / ave_divider; SG_LOG( SG_GENERAL, SG_DEBUG, " val_ave = " << val_ave ); - Pts(j,i) = Point3Dd( min_deg.lon() + i * dlon, - min_deg.lat() + j * dlat, + Pts(i,j) = Point3Dd( min_deg.lon() + j * dlon, + min_deg.lat() + i * dlat, val_ave ); - SG_LOG( SG_GENERAL, SG_DEBUG, "lon_ave = " << lon_ave << " lat_ave = " << lat_ave ); - SG_LOG( SG_GENERAL, SG_DEBUG, "lon = " << min_deg.lon() + i * dlon - << " lat = " << min_deg.lat() + j * dlat ); + SG_LOG( SG_GENERAL, SG_DEBUG, "lon = " << min_deg.lon() + j * dlon + << " lat = " << min_deg.lat() + i * dlat ); } } +#ifdef DEBUG + for ( int j = 0; j < Pts.cols(); ++j ) { + for ( int i = 0; i < Pts.rows(); ++i ) { + printf("%.5f %.5f %.1f\n", Pts(i,j).x(), Pts(i,j).y(), + Pts(i,j).z() ); + } + } +#endif + bool slope_error = true; while ( slope_error ) { SG_LOG( SG_GENERAL, SG_DEBUG, "start of slope processing pass" ); slope_error = false; // Add some "slope" sanity to the resulting surface grid points - for ( int i = 0; i < Pts.cols() - 1; ++i ) { - for ( int j = 0; j < Pts.rows() - 1; ++j ) { + for ( int j = 0; j < Pts.cols() - 1; ++j ) { + for ( int i = 0; i < Pts.rows() - 1; ++i ) { if ( limit_slope( Pts, i, j, i+1, j, average_elev_m ) ) { slope_error = true; } @@ -211,27 +244,67 @@ TGAptSurface::TGAptSurface( const string& path, } } +#ifdef DEBUG + for ( int j = 0; j < Pts.cols(); ++j ) { + for ( int i = 0; i < Pts.rows(); ++i ) { + printf("%.5f %.5f %.1f\n", Pts(i,j).x(), Pts(i,j).y(), + Pts(i,j).z() ); + } + } +#endif + // Create the nurbs surface SG_LOG(SG_GENERAL, SG_DEBUG, "ready to create nurbs surface"); apt_surf = new PlNurbsSurfaced; +#if defined( _NURBS_GLOBAL_INTERP ) apt_surf->globalInterp( Pts, 3, 3); +#elif defined( _NURBS_LEAST_SQUARES ) + cout << "Col = " << Pts.cols() << " Rows = " << Pts.rows() << endl; + int nU = Pts.rows() / 2; if ( nU < 4 ) { nU = 4; } + int nV = Pts.cols() / 2; if ( nV < 4 ) { nV = 4; } + cout << "nU = " << nU << " nV = " << nV << endl; + apt_surf->leastSquares( Pts, 3, 3, nU, nV ); + + // sanity check: I'm finding that leastSquares() can produce nan + // surfaces. We test for this and fall back to globalInterp() if + // the least squares fails. + Point3Dd p = apt_surf->pointAt( 0.5, 0.5 ); + if ( p.z() <= 0.0 || p.z() >= 0.0 ) { + // ok, a valid number + } else { + // no, sorry, a nan is not <= 0.0 or >= 0.0 + SG_LOG(SG_GENERAL, SG_WARN, + "leastSquares() failed, aborting()."); + exit(-1); + + // we could fall back to globalInterp() rather than aborting + // if we wanted to ... + apt_surf->globalInterp( Pts, 3, 3); + } +#else +# error "Need to define _NURBS_GLOBAL_INTER or _NURBS_LEAST_SQUARES" +#endif SG_LOG(SG_GENERAL, SG_DEBUG, " successful."); -#if 0 +#ifdef DEBUG // For debugging only: output an array of surface points suitable // for plotting with gnuplot. This is useful for comparing the // approximated and smoothed surface to the original rough // surface. cout << "DEBUGGING TEST" << endl; - for ( int i = 0; i < (xdivs+2) * mult + 1; ++i ) { - for ( int j = 0; j < (ydivs+2) * mult + 1; ++j ) { - double x = min_deg.lon() + (i-mult)*(dlon/(double)mult); - double y = min_deg.lat() + (j-mult)*(dlat/(double)mult); - cout << x << " " << y << " " << query(x,y) << endl; + const int divs = 100; + double dx = x_deg / divs; + double dy = y_deg / divs; + for ( int j = 0; j < divs; ++j ) { + for ( int i = 0; i < divs; ++i ) { + double lon = min_deg.lon() + j * dx; + double lat = min_deg.lat() + i * dy; + printf("%.5f %.5f %.1f\n", lon, lat, query_solver(lon, lat) ); } } #endif + } @@ -270,10 +343,9 @@ double TGAptSurface::query( double lon_deg, double lat_deg ) { return p.z(); } + // Query the elevation of a point, return -9999 if out of range double TGAptSurface::query_solver( double lon_deg, double lat_deg ) { - const double eps = 0.0000005; - // sanity check if ( lon_deg < min_deg.lon() || lon_deg > max_deg.lon() || lat_deg < min_deg.lat() || lat_deg > max_deg.lat() ) @@ -298,9 +370,10 @@ double TGAptSurface::query_solver( double lon_deg, double lat_deg ) { // cout << " solving for u,v simultaneously" << endl; count = 0; - while ( count < 0 ) { + while ( count < 0 /* disable fast solver for now */ ) { if ( u < 0.0 || u > 1.0 || v < 0.0 || v > 1.0 ) { - // oops, something goofed in the solver + cout << "oops, something goofed in the solver" << endl; + exit(-1); } p = apt_surf->pointAt( u, v ); @@ -312,7 +385,7 @@ double TGAptSurface::query_solver( double lon_deg, double lat_deg ) { dy = lat_deg - p.y(); // cout << " dx = " << dx << " dy = " << dy << endl; - if ( (fabs(dx) < eps) && (fabs(dy) < eps) ) { + if ( (fabs(dx) < nurbs_eps) && (fabs(dy) < nurbs_eps) ) { return p.z(); } else { u = u + (dy/lat_range); @@ -328,7 +401,7 @@ double TGAptSurface::query_solver( double lon_deg, double lat_deg ) { // cout << "quick query solver failed..." << endl; - // failed to converge with fast approach, try a binary paritition + // failed to converge with fast approach, try a binary partition // scheme, however we have to solve each axis independently // because when we move in one axis we may change our position in // the other axis. @@ -344,6 +417,8 @@ double TGAptSurface::query_solver( double lon_deg, double lat_deg ) { dx = 1000.0; dy = 1000.0; + int gcount = 0; + while ( true ) { // cout << "solving for u" << endl; @@ -356,21 +431,21 @@ double TGAptSurface::query_solver( double lon_deg, double lat_deg ) { // << p.z() << endl; // cout << " found " << p.x() << ", " << p.y() << " = " << p.z() // << endl; - dy = lat_deg - p.y(); dx = lon_deg - p.x(); - // cout << " dx = " << dx << " dy = " << dy << endl; + dy = lat_deg - p.y(); + // cout << " dx = " << dx << " dy = " << dy << " z = " << p.z() << endl; // double az1, az2, dist; // geo_inverse_wgs_84( 0, lat_deg, lon_deg, p.y(), p.x(), // &az1, &az2, &dist ); // cout << " query distance error = " << dist << endl; - if ( fabs(dy) < eps ) { + if ( fabs(dy) < nurbs_eps ) { break; } else { - if ( dy >= eps ) { + if ( dy >= nurbs_eps ) { min_u = u; - } else if ( dy <= eps ) { + } else if ( dy <= nurbs_eps ) { max_u = u; } } @@ -392,18 +467,18 @@ double TGAptSurface::query_solver( double lon_deg, double lat_deg ) { // << endl; dx = lon_deg - p.x(); dy = lat_deg - p.y(); - // cout << " dx = " << dx << " dy = " << dy << endl; + // cout << " dx = " << dx << " dy = " << dy << " z = " << p.z() << endl; // double az1, az2, dist; // geo_inverse_wgs_84( 0, lat_deg, lon_deg, p.y(), p.x(), // &az1, &az2, &dist ); // cout << " query distance error = " << dist << endl; - if ( fabs(dx) < eps ) { + if ( fabs(dx) < nurbs_eps ) { break; } else { - if ( dx >= eps ) { + if ( dx >= nurbs_eps ) { min_v = v; - } else if ( dx <= eps ) { + } else if ( dx <= nurbs_eps ) { max_v = v; } } @@ -412,13 +487,22 @@ double TGAptSurface::query_solver( double lon_deg, double lat_deg ) { ++count; } - if ( (fabs(dx) < eps) && (fabs(dy) < eps) ) { + // cout << "query count = " << gcount << " dist = " << dx << ", " + // << dy << endl; + + if ( (fabs(dx) < nurbs_eps) && (fabs(dy) < nurbs_eps) ) { // double az1, az2, dist; // geo_inverse_wgs_84( 0, lat_deg, lon_deg, p.y(), p.x(), // &az1, &az2, &dist ); // cout << " final query distance error = " << dist << endl; return p.z(); } + + gcount++; + if ( gcount % 100 == 0 ) { + cout << "query count = " << gcount << " dist = " << dx << ", " + << dy << endl; + } } cout << "binary solver failed..." << endl; diff --git a/src/Airports/GenAirports/build.cxx b/src/Airports/GenAirports/build.cxx index f4becd15..9ff354dd 100644 --- a/src/Airports/GenAirports/build.cxx +++ b/src/Airports/GenAirports/build.cxx @@ -287,6 +287,7 @@ static void build_runway( const TGRunway& rwy_info, } else if ( surface_code == 13 /* Water runway (buoy's?) */ ) { // water } else { + SG_LOG(SG_GENERAL, SG_WARN, "surface_code = " << surface_code); throw sg_exception("unknown runway type!"); } @@ -314,6 +315,10 @@ static void build_runway( const TGRunway& rwy_info, // visual runway markings gen_visual_rwy( rwy_info, alt_m, material, rwy_polys, texparams, accum ); + } else if ( rwy_info.marking_code == 0 /* No known markings, lets assume Visual */ ) { + // visual runway markings + gen_visual_rwy( rwy_info, alt_m, material, + rwy_polys, texparams, accum ); } else if ( surface_code == 13 /* Water buoys */ ) { // do nothing for now. } else { @@ -552,8 +557,13 @@ void build_airport( string airport_id, float alt_m, && runways[i].marking_code != 2 /* Non-precision */ && runways[i].marking_code != 1 /* Visual */ ) { - if ( runways[i].surface_code != 13 ) { - // only build non-water runways + if ( runways[i].surface_code != 6 /* Asphalt Helipad */ && + runways[i].surface_code != 7 /* Concrete Helipad */ && + runways[i].surface_code != 8 /* Turf Helipad */ && + runways[i].surface_code != 9 /* Dirt Helipad */ && + runways[i].surface_code != 13 /* Water/buoy runway */ ) + { + // only build non-water and non-heliport runways build_runway( runways[i], alt_m, &rwy_polys, &texparams, &accum, &apt_base, &apt_clearing ); diff --git a/src/Airports/GenAirports/elevations.cxx b/src/Airports/GenAirports/elevations.cxx index 7ecd497a..945482d5 100644 --- a/src/Airports/GenAirports/elevations.cxx +++ b/src/Airports/GenAirports/elevations.cxx @@ -153,9 +153,9 @@ void tgCalcElevations( const string &root, const string_list elev_src, } // set all elevations to -9999 - for ( i = 0; i < Pts.cols(); ++i ) { - for ( j = 0; j < Pts.rows(); ++j ) { - Point3Dd p = Pts(j,i); + for ( j = 0; j < Pts.cols(); ++j ) { + for ( i = 0; i < Pts.rows(); ++i ) { + Point3Dd p = Pts(i,j); p.z() = -9999.0; } } @@ -164,9 +164,9 @@ void tgCalcElevations( const string &root, const string_list elev_src, // find first node with -9999 elevation Point3Dd first; bool found_one = false; - for ( i = 0; i < Pts.cols(); ++i ) { - for ( j = 0; j < Pts.rows(); ++j ) { - Point3Dd p = Pts(j,i); + for ( j = 0; j < Pts.cols(); ++j ) { + for ( i = 0; i < Pts.rows(); ++i ) { + Point3Dd p = Pts(i,j); if ( p.z() < -9000.0 && !found_one ) { first = p; found_one = true; @@ -204,16 +204,16 @@ void tgCalcElevations( const string &root, const string_list elev_src, // this array file double elev; done = true; - for ( i = 0; i < Pts.cols(); ++i ) { - for ( j = 0; j < Pts.rows(); ++j ) { - Point3Dd p = Pts(j,i); + for ( j = 0; j < Pts.cols(); ++j ) { + for ( i = 0; i < Pts.rows(); ++i ) { + Point3Dd p = Pts(i,j); if ( p.z() < -9000.0 ) { done = false; elev = array.altitude_from_grid( p.x() * 3600.0, p.y() * 3600.0 ); if ( elev > -9000 ) { p.z() = elev; - Pts(j,i) = p; + Pts(i,j) = p; // cout << "interpolating for " << p << endl; // cout << p.x() << " " << p.y() << " " << p.z() // << endl; @@ -233,9 +233,9 @@ void tgCalcElevations( const string &root, const string_list elev_src, // find the average height of the queried points double total = 0.0; int count = 0; - for ( i = 0; i < Pts.cols(); ++i ) { - for ( j = 0; j < Pts.rows(); ++j ) { - Point3Dd p = Pts(j,i); + for ( j = 0; j < Pts.cols(); ++j ) { + for ( i = 0; i < Pts.rows(); ++i ) { + Point3Dd p = Pts(i,j); total += p.z(); count++; } @@ -255,20 +255,20 @@ void tgClampElevations( Matrix_Point3Dd &Pts, // go through the elevations and clamp all elevations to within // +/-max_m of the center_m elevation. - for ( i = 0; i < Pts.cols(); ++i ) { - for ( j = 0; j < Pts.rows(); ++j ) { - Point3Dd p = Pts(j,i); + for ( j = 0; j < Pts.cols(); ++j ) { + for ( i = 0; i < Pts.rows(); ++i ) { + Point3Dd p = Pts(i,j); if ( p.z() < center_m - max_clamp_m ) { SG_LOG(SG_GENERAL, SG_DEBUG, " clamping " << p.z() << " to " << center_m - max_clamp_m ); p.z() = center_m - max_clamp_m; - Pts(j,i) = p; + Pts(i,j) = p; } if ( p.z() > center_m + max_clamp_m ) { SG_LOG(SG_GENERAL, SG_DEBUG, " clamping " << p.z() << " to " << center_m + max_clamp_m ); p.z() = center_m + max_clamp_m; - Pts(j,i) = p; + Pts(i,j) = p; } } } diff --git a/src/Airports/GenAirports/global.hxx b/src/Airports/GenAirports/global.hxx index f431e13c..2fff0c1a 100644 --- a/src/Airports/GenAirports/global.hxx +++ b/src/Airports/GenAirports/global.hxx @@ -29,14 +29,21 @@ extern int nudge; // Final grid size for airport surface (in meters) -const double coarse_grid = 700.0; +const double coarse_grid = 500.0; // compared to the average surface elevation, clamp all values within // this many meters of the average const double max_clamp = 100.0; // maximum slope (rise/run) allowed on an airport surface -const double slope_max = 0.009; +const double slope_max = 0.02; const double slope_eps = 0.00001; +// nurbs query/search epsilon +const double nurbs_eps = 0.0000001; + +// Define only one of the following +// #define _NURBS_GLOBAL_APPROX 1 +#define _NURBS_LEAST_SQUARES 1 + #endif // _GEN_AIRPORT_GLOBAL_HXX diff --git a/src/Airports/GenAirports/main.cxx b/src/Airports/GenAirports/main.cxx index 95f22c55..c1ef9d26 100644 --- a/src/Airports/GenAirports/main.cxx +++ b/src/Airports/GenAirports/main.cxx @@ -271,10 +271,7 @@ int main( int argc, char **argv ) { in.getline(tmp, 2048); vector<string> vers_token = simgear::strutils::split( tmp ); SG_LOG( SG_GENERAL, SG_INFO, "Data version = " << vers_token[0] ); - } else if ( token[0] == "1" /* Airport */ - || token[0] == "16" /* Seaplane Base */ - || token[0] == "17" /* Heliport */ ) - { + } else if ( token[0] == "1" /* Airport */ ) { // extract some airport runway info string rwy; float lat, lon; @@ -357,6 +354,10 @@ int main( int argc, char **argv ) { windsock_list.push_back(line); } else if ( token[0] == "15" ) { // ignore custom startup locations + } else if ( token[0] == "16" ) { + // ignore seaplane bases for now + } else if ( token[0] == "17" ) { + // ignore heliports for now } else if ( token[0] == "50" || token[0] == "51" || token[0] == "52" || token[0] == "53" || token[0] == "54" || token[0] == "55" || token[0] == "56" )