From 3b879d29c23d1278b99e9843354dd747e9e21be2 Mon Sep 17 00:00:00 2001 From: curt Date: Fri, 9 Sep 2005 20:16:10 +0000 Subject: [PATCH] Fix some bugs in my first attempt at a new airport surface fitting scheme. The code is now workable but needs some fine tuning. --- src/Airports/GenAirports/apt_surface.cxx | 132 +++++++++++++++-------- src/Airports/GenAirports/apt_surface.hxx | 24 ++++- src/Airports/GenAirports/build.cxx | 8 +- src/Airports/GenAirports/elevations.cxx | 20 ++-- 4 files changed, 124 insertions(+), 60 deletions(-) diff --git a/src/Airports/GenAirports/apt_surface.cxx b/src/Airports/GenAirports/apt_surface.cxx index c20fbffd..231dd1c5 100644 --- a/src/Airports/GenAirports/apt_surface.cxx +++ b/src/Airports/GenAirports/apt_surface.cxx @@ -145,12 +145,12 @@ TGAptSurface::TGAptSurface( const string& path, // with an added major row column on the NE sides.) int mult = 10; SimpleMatrix dPts( (ydivs + 1) * mult + 1, (xdivs + 1) * mult + 1 ); - for ( int j = 0; j < dPts.cols(); ++j ) { - for ( int i = 0; i < dPts.rows(); ++i ) { + for ( int j = 0; j < dPts.rows(); ++j ) { + for ( int i = 0; i < dPts.cols(); ++i ) { dPts.set(i, j, Point3D( min_deg.lon() - dlon_h - + j * (dlon / (double)mult), + + i * (dlon / (double)mult), min_deg.lat() - dlat_h - + i * (dlat / (double)mult), + + j * (dlat / (double)mult), -9999 ) ); } @@ -159,8 +159,8 @@ TGAptSurface::TGAptSurface( const string& path, // 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 ) { + for ( int j = 0; j < dPts.rows(); ++j ) { + for ( int i = 0; i < dPts.cols(); ++i ) { printf("%.5f %.5f %.1f\n", dPts(i,j).x(), dPts(i,j).y(), dPts(i,j).z() ); } @@ -172,8 +172,8 @@ TGAptSurface::TGAptSurface( const string& path, 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 ) { + for ( int j = 0; j < dPts.rows(); ++j ) { + for ( int i = 0; i < dPts.cols(); ++i ) { printf("%.5f %.5f %.1f\n", dPts(i,j).x(), dPts(i,j).y(), dPts(i,j).z() ); } @@ -183,8 +183,8 @@ TGAptSurface::TGAptSurface( const string& path, // Build the normal res input grid from the double res version Pts = new SimpleMatrix(ydivs + 1, xdivs + 1); double ave_divider = (mult+1) * (mult+1); - for ( int j = 0; j < Pts->cols(); ++j ) { - for ( int i = 0; i < Pts->rows(); ++i ) { + for ( int j = 0; j < Pts->rows(); ++j ) { + for ( int i = 0; i < Pts->cols(); ++i ) { SG_LOG(SG_GENERAL, SG_DEBUG, i << "," << j); double accum = 0.0; double lon_accum = 0.0; @@ -203,8 +203,8 @@ TGAptSurface::TGAptSurface( const string& path, double lat_ave = lat_accum / ave_divider; SG_LOG( SG_GENERAL, SG_DEBUG, " val_ave = " << val_ave ); - Pts->set(i, j, Point3D( min_deg.lon() + j * dlon, - min_deg.lat() + i * dlat, + Pts->set(i, j, Point3D( min_deg.lon() + i * dlon, + min_deg.lat() + j * dlat, val_ave ) ); SG_LOG( SG_GENERAL, SG_DEBUG, "lon_ave = " << lon_ave @@ -215,10 +215,12 @@ 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() ); + for ( int j = 0; j < Pts->rows(); ++j ) { + for ( int i = 0; i < Pts->cols(); ++i ) { + printf("%.5f %.5f %.1f\n", + Pts->element(i,j).x(), + Pts->element(i,j).y(), + Pts->element(i,j).z() ); } } #endif @@ -228,8 +230,8 @@ TGAptSurface::TGAptSurface( const string& path, 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 j = 0; j < Pts->cols() - 1; ++j ) { - for ( int i = 0; i < Pts->rows() - 1; ++i ) { + for ( int j = 0; j < Pts->rows() - 1; ++j ) { + for ( int i = 0; i < Pts->cols() - 1; ++i ) { if ( limit_slope( Pts, i, j, i+1, j, average_elev_m ) ) { slope_error = true; } @@ -244,8 +246,8 @@ TGAptSurface::TGAptSurface( const string& path, } #ifdef DEBUG - for ( int j = 0; j < Pts->cols(); ++j ) { - for ( int i = 0; i < Pts->rows(); ++i ) { + for ( int j = 0; j < Pts->rows(); ++j ) { + for ( int i = 0; i < Pts->cols(); ++i ) { printf("%.5f %.5f %.1f\n", Pts(i,j).x(), Pts(i,j).y(), Pts(i,j).z() ); } @@ -305,6 +307,8 @@ TGAptSurface::~TGAptSurface() { static ColumnVector qr_method( Real* y, Real* t1, Real* t2, Real* t3, Real* t4, Real* t5, Real* t6, Real* t7, Real* t8, + /* Real* t9, Real* t10, Real* t11, Real* t12, + Real* t13, Real* t14, Real* t15, */ int nobs, int npred ) { cout << "QR triangularisation" << endl;; @@ -313,6 +317,8 @@ static ColumnVector qr_method( Real* y, // load data - 1s into col 1 of matrix int npred1 = npred+1; + cout << "nobs = " << nobs << " npred1 = " << npred1 << endl; + Matrix X(nobs,npred1); ColumnVector Y(nobs); X.column(1) = 1.0; X.column(2) << t1; @@ -323,6 +329,13 @@ static ColumnVector qr_method( Real* y, X.column(7) << t6; X.column(8) << t7; X.column(9) << t8; + /* X.column(10) << t9; + X.column(11) << t10; + X.column(12) << t11; + X.column(13) << t12; + X.column(14) << t13; + X.column(15) << t14; + X.column(16) << t15; */ Y << y; // do Householder triangularisation @@ -341,6 +354,9 @@ static ColumnVector qr_method( Real* y, // Get diagonals of Hat matrix DiagonalMatrix Hat; Hat << X1 * X1.t(); + cout << "A vector = " << A << endl; + cout << "A rows = " << A.nrows() << endl; + // print out answers cout << "\nEstimates and their standard errors\n\n"; ColumnVector SE(npred1); @@ -348,7 +364,7 @@ static ColumnVector qr_method( Real* y, cout << setw(11) << setprecision(5) << (A | SE) << endl; cout << "\nObservations, fitted value, residual value, hat value\n"; cout << setw(9) << setprecision(3) << - (X.columns(2,3) | Y | Fitted | Y1 | Hat.as_column()); + (X.columns(2,4) | Y | Fitted | Y1 | Hat.as_column()); cout << "\n\n"; return A; @@ -359,10 +375,14 @@ static ColumnVector qr_method( Real* y, // sampled surface data void TGAptSurface::fit() { - // the fit function is: f(x,y) = a*x + b*x^2 + c*y + d*y^2 + e*x*y + - // f*x*y^2 + g*x^2*y + h*x^2*y^2 + // the fit function is: + // f(x,y) = A1*x + A2*x*y + A3*y + + // A4*x*x + A5+x*x*y + A6*x*x*y*y + A7*y*y + A8*x*y*y + + // A9*x*x*x + A10*x*x*x*y + A11*x*x*x*y*y + A12*x*x*x*y*y*y + + // A13*y*y*y + A14*x*y*y*y + A15*x*x*y*y*y + int nobs = Pts->cols() * Pts->rows(); // number of observations - int npred = 8; // number of predictor values + int npred = 8; // number of predictor values A[n] Real z[nobs]; Real t1[nobs]; @@ -373,23 +393,38 @@ void TGAptSurface::fit() { Real t6[nobs]; Real t7[nobs]; Real t8[nobs]; + /* Real t9[nobs]; + Real t10[nobs]; + Real t11[nobs]; + Real t12[nobs]; + Real t13[nobs]; + Real t14[nobs]; + Real t15[nobs]; */ // generate the required fit data - for ( int j = 0; j < Pts->cols(); j++ ) { - for ( int i = 0; i <= Pts->rows(); i++ ) { + for ( int j = 0; j < Pts->rows(); j++ ) { + for ( int i = 0; i < Pts->cols(); i++ ) { Point3D p = Pts->element( i, j ); - int index = ( j * Pts->rows() ) + i; - Real xi = p.x(); - Real yi = p.y(); + int index = ( j * Pts->cols() ) + i; + Real x = p.x(); + Real y = p.y(); + cout << "pt = " << x << "," << y << "," << p.z() << endl; z[index] = p.z(); - t1[index] = xi; - t2[index] = xi*xi; - t3[index] = yi; - t4[index] = yi*yi; - t5[index] = xi*yi; - t6[index] = xi*yi*yi; - t7[index] = xi*xi*yi; - t8[index] = xi*xi*yi*yi; + t1[index] = x; + t2[index] = x*y; + t3[index] = y; + t4[index] = x*x; + t5[index] = x*x*y; + t6[index] = x*x*y*y; + t7[index] = y*y; + t8[index] = x*y*y; + /* t9[index] = x*x*x; + t10[index] = x*x*x*y; + t11[index] = x*x*x*y*y; + t12[index] = x*x*x*y*y*y; + t13[index] = y*y*y; + t14[index] = x*y*y*y; + t15[index] = x*x*y*y*y; */ } } @@ -398,7 +433,12 @@ void TGAptSurface::fit() { Try { surface_coefficients - = qr_method(z, t1, t2, t3, t4, t5, t6, t7, t8, nobs, npred); + = qr_method( z, + t1, t2, t3, t4, t5, t6, t7, t8, + /* t9, t10, t11, t12, t13, t14, t15, */ + nobs, npred + ); + cout << "surface_coefficients size = " << surface_coefficients.nrows() << endl; } CatchAll { cout << BaseException::what(); } @@ -418,14 +458,22 @@ double TGAptSurface::query( double lon_deg, double lat_deg ) { // compute the function with solved coefficients - // the fit function is: f(x,y) = a*x + b*x^2 + c*y + d*y^2 + e*x*y + - // f*x*y^2 + g*x^2*y + h*x^2*y^2 + // the fit function is: + // f(x,y) = A1*x + A2*x*y + A3*y + + // A4*x*x + A5+x*x*y + A6*x*x*y*y + A7*y*y + A8*x*y*y + + // A9*x*x*x + A10*x*x*x*y + A11*x*x*x*y*y + A12*x*x*x*y*y*y + + // A13*y*y*y + A14*x*y*y*y + A15*x*x*y*y*y double x = lon_deg; double y = lat_deg; ColumnVector A = surface_coefficients; - double result = A(0)*x + A(1)*x*x + A(2)*y + A(3)*y*y + A(4)*x*y - + A(5)*x*y*y + A(6)*x*x*y + A(7)*x*x*y*y; + + double result = A(1) + A(2)*x + A(3)*x*y + A(4)*y + A(5)*x*x + A(6)*x*x*y + + A(7)*x*x*y*y + A(8)*y*y + A(9)*x*y*y /* + A(10)*x*x*x + A(11)*x*x*x*y + + A(12)*x*x*x*y*y + A(13)*x*x*x*y*y*y + A(14)*y*y*y + A(15)*x*y*y*y + + A(16)*x*x*y*y*y */; + + printf("result = %.2f\n", result); return result; } diff --git a/src/Airports/GenAirports/apt_surface.hxx b/src/Airports/GenAirports/apt_surface.hxx index 0ccf1953..a25c46c5 100644 --- a/src/Airports/GenAirports/apt_surface.hxx +++ b/src/Airports/GenAirports/apt_surface.hxx @@ -43,8 +43,6 @@ * A dirt simple matrix class for our convenience based on top of Point3D */ -#define SIMPLE_MATRIX_MAX_SIZE 200 - class SimpleMatrix { private: @@ -62,12 +60,30 @@ public: } inline Point3D element( int col, int row ) { - int index = ( col * _rows ) + row; + int index = ( row * _cols ) + col; + if ( col < 0 || col >= _cols ) { + cout << "column out of bounds on read (" << col << " >= " << _cols << ")" + << endl; + int *p = 0; *p = 1; // force crash + } else if ( row < 0 || row >= _rows ) { + cout << "row out of bounds on read (" << row << " >= " << _rows << ")" + << endl; + int *p = 0; *p = 1; // force crash + } return m[index]; } inline void set( int col, int row, Point3D p ) { - int index = ( col * _rows ) + row; + int index = ( row * _cols ) + col; + if ( col < 0 || col >= _cols ) { + cout << "column out of bounds on set (" << col << " >= " << _cols << ")" + << endl; + int *p = 0; *p = 1; // force crash + } else if ( row < 0 || row >= _rows ) { + cout << "row out of bounds on set (" << row << " >= " << _rows << ")" + << endl; + int *p = 0; *p = 1; // force crash + } m[index] = p; } diff --git a/src/Airports/GenAirports/build.cxx b/src/Airports/GenAirports/build.cxx index 43f9c70c..223caad2 100644 --- a/src/Airports/GenAirports/build.cxx +++ b/src/Airports/GenAirports/build.cxx @@ -1029,10 +1029,10 @@ void build_airport( string airport_id, float alt_m, // Extend the area a bit so we don't have wierd things on the edges double dlon = max_deg.lon() - min_deg.lon(); double dlat = max_deg.lat() - min_deg.lat(); - min_deg.setlon( min_deg.lon() - 0.3 * dlon ); - max_deg.setlon( max_deg.lon() + 0.3 * dlon ); - min_deg.setlat( min_deg.lat() - 0.3 * dlat ); - max_deg.setlat( max_deg.lat() + 0.3 * dlat ); + min_deg.setlon( min_deg.lon() - 0.01 * dlon ); + max_deg.setlon( max_deg.lon() + 0.01 * dlon ); + min_deg.setlat( min_deg.lat() - 0.01 * dlat ); + max_deg.setlat( max_deg.lat() + 0.01 * dlat ); TGAptSurface apt_surf( root, elev_src, min_deg, max_deg, average ); SG_LOG(SG_GENERAL, SG_DEBUG, "Surface created"); diff --git a/src/Airports/GenAirports/elevations.cxx b/src/Airports/GenAirports/elevations.cxx index 325cb262..4588dd43 100644 --- a/src/Airports/GenAirports/elevations.cxx +++ b/src/Airports/GenAirports/elevations.cxx @@ -156,8 +156,8 @@ void tgCalcElevations( const string &root, const string_list elev_src, } // set all elevations to -9999 - for ( j = 0; j < Pts.cols(); ++j ) { - for ( i = 0; i < Pts.rows(); ++i ) { + for ( j = 0; j < Pts.rows(); ++j ) { + for ( i = 0; i < Pts.cols(); ++i ) { Point3D p = Pts.element(i, j); p.setz( -9999.0 ); Pts.set(i, j, p); @@ -168,8 +168,8 @@ void tgCalcElevations( const string &root, const string_list elev_src, // find first node with -9999 elevation Point3D first(0.0); bool found_one = false; - for ( j = 0; j < Pts.cols(); ++j ) { - for ( i = 0; i < Pts.rows(); ++i ) { + for ( j = 0; j < Pts.rows(); ++j ) { + for ( i = 0; i < Pts.cols(); ++i ) { Point3D p = Pts.element(i,j); if ( p.z() < -9000.0 && !found_one ) { first = p; @@ -208,8 +208,8 @@ void tgCalcElevations( const string &root, const string_list elev_src, // this array file double elev; done = true; - for ( j = 0; j < Pts.cols(); ++j ) { - for ( i = 0; i < Pts.rows(); ++i ) { + for ( j = 0; j < Pts.rows(); ++j ) { + for ( i = 0; i < Pts.cols(); ++i ) { Point3D p = Pts.element(i,j); if ( p.z() < -9000.0 ) { done = false; @@ -237,8 +237,8 @@ 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 ( j = 0; j < Pts.cols(); ++j ) { - for ( i = 0; i < Pts.rows(); ++i ) { + for ( j = 0; j < Pts.rows(); ++j ) { + for ( i = 0; i < Pts.cols(); ++i ) { Point3D p = Pts.element(i,j); total += p.z(); count++; @@ -259,8 +259,8 @@ void tgClampElevations( SimpleMatrix &Pts, // go through the elevations and clamp all elevations to within // +/-max_m of the center_m elevation. - for ( j = 0; j < Pts.cols(); ++j ) { - for ( i = 0; i < Pts.rows(); ++i ) { + for ( j = 0; j < Pts.rows(); ++j ) { + for ( i = 0; i < Pts.cols(); ++i ) { Point3D p = Pts.element(i,j); if ( p.z() < center_m - max_clamp_m ) { SG_LOG(SG_GENERAL, SG_DEBUG, " clamping " << p.z()