Port apt_surface and elevation to SGGeod
This commit is contained in:
parent
780e836e8b
commit
620f941609
6 changed files with 122 additions and 198 deletions
|
@ -172,7 +172,7 @@ static point_list calc_elevations( TGAptSurface &surf,
|
|||
{
|
||||
point_list result = geod_nodes;
|
||||
for ( unsigned int i = 0; i < result.size(); ++i ) {
|
||||
double elev = surf.query( result[i].lon(), result[i].lat() );
|
||||
double elev = surf.query( SGGeod::fromDeg( result[i].lon(), result[i].lat()) );
|
||||
result[i].setelev( elev + offset );
|
||||
}
|
||||
|
||||
|
@ -186,7 +186,7 @@ static tgContour calc_elevations( TGAptSurface &surf,
|
|||
tgContour result = geod_nodes;
|
||||
for ( unsigned int i = 0; i < result.GetSize(); ++i ) {
|
||||
SGGeod node = result.GetNode(i);
|
||||
double elev = surf.query( node.getLongitudeDeg(), node.getLatitudeDeg() );
|
||||
double elev = surf.query( node );
|
||||
node.setElevationM( elev + offset );
|
||||
result.SetNode( i, node );
|
||||
}
|
||||
|
@ -198,7 +198,7 @@ static double calc_elevation( TGAptSurface &surf,
|
|||
const SGGeod& node,
|
||||
double offset )
|
||||
{
|
||||
double elev = surf.query( node.getLongitudeDeg(), node.getLatitudeDeg() );
|
||||
double elev = surf.query( node );
|
||||
elev += offset;
|
||||
|
||||
return elev;
|
||||
|
@ -916,29 +916,30 @@ void Airport::BuildBtg(const string& root, const string_list& elev_src )
|
|||
// calculation min/max coordinates of airport area
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, " calculation min/max coordinates of airport area");
|
||||
|
||||
Point3D min_deg(9999.0, 9999.0, 0), max_deg(-9999.0, -9999.0, 0);
|
||||
SGGeod min_deg = SGGeod::fromDeg(9999.0, 9999.0);
|
||||
SGGeod max_deg = SGGeod::fromDeg(-9999.0, -9999.0);
|
||||
for ( unsigned int j = 0; j < nodes.get_node_list().size(); ++j )
|
||||
{
|
||||
Point3D p = nodes.get_node_list()[j];
|
||||
if ( p.lon() < min_deg.lon() )
|
||||
if ( p.lon() < min_deg.getLongitudeDeg() )
|
||||
{
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, "new min lon from node " << j << " is " << p.lon() );
|
||||
min_deg.setlon( p.lon() );
|
||||
min_deg.setLongitudeDeg( p.lon() );
|
||||
}
|
||||
if ( p.lon() > max_deg.lon() )
|
||||
if ( p.lon() > max_deg.getLongitudeDeg() )
|
||||
{
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, "new max lon from node " << j << " is " << p.lon() );
|
||||
max_deg.setlon( p.lon() );
|
||||
max_deg.setLongitudeDeg( p.lon() );
|
||||
}
|
||||
if ( p.lat() < min_deg.lat() )
|
||||
if ( p.lat() < min_deg.getLatitudeDeg() )
|
||||
{
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, "new min lat from node " << j << " is " << p.lat() );
|
||||
min_deg.setlat( p.lat() );
|
||||
min_deg.setLatitudeDeg( p.lat() );
|
||||
}
|
||||
if ( p.lat() > max_deg.lat() )
|
||||
if ( p.lat() > max_deg.getLatitudeDeg() )
|
||||
{
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, "new max lat from node " << j << " is " << p.lat() );
|
||||
max_deg.setlat( p.lat() );
|
||||
max_deg.setLatitudeDeg( p.lat() );
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -954,33 +955,33 @@ void Airport::BuildBtg(const string& root, const string_list& elev_src )
|
|||
|
||||
for ( unsigned int j = 0; j < rwy_lights[i].ContourSize(); ++j )
|
||||
{
|
||||
Point3D p = Point3D::fromSGGeod( rwy_lights[i].GetNode(j) );
|
||||
if ( p.lon() < min_deg.lon() )
|
||||
SGGeod p = rwy_lights[i].GetNode(j);
|
||||
if ( p.getLongitudeDeg() < min_deg.getLongitudeDeg() )
|
||||
{
|
||||
min_deg.setlon( p.lon() );
|
||||
min_deg.setLongitudeDeg( p.getLongitudeDeg() );
|
||||
}
|
||||
if ( p.lon() > max_deg.lon() )
|
||||
if ( p.getLongitudeDeg() > max_deg.getLongitudeDeg() )
|
||||
{
|
||||
max_deg.setlon( p.lon() );
|
||||
max_deg.setLongitudeDeg( p.getLongitudeDeg() );
|
||||
}
|
||||
if ( p.lat() < min_deg.lat() )
|
||||
if ( p.getLatitudeDeg() < min_deg.getLatitudeDeg() )
|
||||
{
|
||||
min_deg.setlat( p.lat() );
|
||||
min_deg.setLatitudeDeg( p.getLatitudeDeg() );
|
||||
}
|
||||
if ( p.lat() > max_deg.lat() )
|
||||
if ( p.getLatitudeDeg() > max_deg.getLatitudeDeg() )
|
||||
{
|
||||
max_deg.setlat( p.lat() );
|
||||
max_deg.setLatitudeDeg( p.getLatitudeDeg() );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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.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 );
|
||||
double dlon = max_deg.getLongitudeDeg() - min_deg.getLongitudeDeg();
|
||||
double dlat = max_deg.getLatitudeDeg() - min_deg.getLatitudeDeg();
|
||||
min_deg.setLongitudeDeg( min_deg.getLongitudeDeg() - 0.01 * dlon );
|
||||
max_deg.setLongitudeDeg( max_deg.getLongitudeDeg() + 0.01 * dlon );
|
||||
min_deg.setLatitudeDeg( min_deg.getLatitudeDeg() - 0.01 * dlat );
|
||||
max_deg.setLatitudeDeg( max_deg.getLatitudeDeg() + 0.01 * dlat );
|
||||
SG_LOG(SG_GENERAL, SG_INFO, "min = " << min_deg << " max = " << max_deg );
|
||||
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, "Create Apt surface:" );
|
||||
|
@ -990,7 +991,9 @@ void Airport::BuildBtg(const string& root, const string_list& elev_src )
|
|||
SG_LOG(SG_GENERAL, SG_DEBUG, " max: " << max_deg );
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, " average: " << average );
|
||||
|
||||
TGAptSurface apt_surf( root, elev_src, min_deg, max_deg, average );
|
||||
tg::Rectangle aptBounds(min_deg, max_deg);
|
||||
|
||||
TGAptSurface apt_surf( root, elev_src, aptBounds, average );
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, "Airport surface created");
|
||||
|
||||
// calculate node elevations
|
||||
|
|
|
@ -28,8 +28,7 @@
|
|||
|
||||
#include <simgear/compiler.h>
|
||||
#include <simgear/constants.h>
|
||||
#include <simgear/math/sg_geodesy.hxx>
|
||||
#include <simgear/math/sg_types.hxx>
|
||||
#include <simgear/math/SGMath.hxx>
|
||||
#include <simgear/debug/logstream.hxx>
|
||||
|
||||
#include "elevations.hxx"
|
||||
|
@ -42,16 +41,12 @@ static bool limit_slope( SimpleMatrix *Pts, int i1, int j1, int i2, int j2,
|
|||
{
|
||||
bool slope_error = false;
|
||||
|
||||
Point3D p1, p2;
|
||||
SGGeod p1, p2;
|
||||
p1 = Pts->element(i1,j1);
|
||||
p2 = Pts->element(i2,j2);
|
||||
|
||||
double az1, az2, dist;
|
||||
double slope;
|
||||
|
||||
geo_inverse_wgs_84( 0, p1.y(), p1.x(), p2.y(), p2.x(),
|
||||
&az1, &az2, &dist );
|
||||
slope = (p2.z() - p1.z()) / dist;
|
||||
double dist = SGGeodesy::distanceM(p1, p2);
|
||||
double slope = (p2.getElevationM() - p1.getElevationM()) / dist;
|
||||
|
||||
if ( fabs(slope) > (slope_max + slope_eps) ) {
|
||||
// need to throttle slope, let's move the point
|
||||
|
@ -59,32 +54,28 @@ static bool limit_slope( SimpleMatrix *Pts, int i1, int j1, int i2, int j2,
|
|||
|
||||
slope_error = true;
|
||||
|
||||
SG_LOG( SG_GENERAL, SG_DEBUG, " (a) detected slope of "
|
||||
<< slope << " dist = " << dist );
|
||||
SG_LOG( SG_GENERAL, SG_DEBUG, " (a) detected slope of " << slope << " dist = " << dist );
|
||||
|
||||
double e1 = fabs(average_elev_m - p1.z());
|
||||
double e2 = fabs(average_elev_m - p2.z());
|
||||
// cout << " z1 = " << p1.z() << " z2 = " << p2.z() << endl;
|
||||
// cout << " e1 = " << e1 << " e2 = " << e2 << endl;
|
||||
double e1 = fabs(average_elev_m - p1.getElevationM());
|
||||
double e2 = fabs(average_elev_m - p2.getElevationM());
|
||||
|
||||
if ( e1 > e2 ) {
|
||||
// cout << " p1 error larger" << endl;
|
||||
// p1 error larger
|
||||
if ( slope > 0 ) {
|
||||
p1.setz( p2.z() - (dist * slope_max) );
|
||||
p1.setElevationM( p2.getElevationM() - (dist * slope_max) );
|
||||
} else {
|
||||
p1.setz( p2.z() + (dist * slope_max) );
|
||||
p1.setElevationM( p2.getElevationM() + (dist * slope_max) );
|
||||
}
|
||||
Pts->set(i1, j1, p1);
|
||||
} else {
|
||||
// cout << " p2 error larger" << endl;
|
||||
// p2 error larger
|
||||
if ( slope > 0 ) {
|
||||
p2.setz( p1.z() + (dist * slope_max) );
|
||||
p2.setElevationM( p1.getElevationM() + (dist * slope_max) );
|
||||
} else {
|
||||
p2.setz( p1.z() - (dist * slope_max) );
|
||||
p2.setElevationM( p1.getElevationM() - (dist * slope_max) );
|
||||
}
|
||||
Pts->set(i2, j2, p2);
|
||||
}
|
||||
// cout << " z1 = " << p1.z() << " z2 = " << p2.z() << endl;
|
||||
}
|
||||
|
||||
return slope_error;
|
||||
|
@ -95,26 +86,25 @@ static bool limit_slope( SimpleMatrix *Pts, int i1, int j1, int i2, int j2,
|
|||
// lon/lat degrees
|
||||
TGAptSurface::TGAptSurface( const std::string& path,
|
||||
const string_list& elev_src,
|
||||
Point3D _min_deg, Point3D _max_deg,
|
||||
double _average_elev_m )
|
||||
tg::Rectangle aptBounds,
|
||||
double average_elev_m )
|
||||
{
|
||||
// Calculate desired size of grid
|
||||
min_deg = _min_deg;
|
||||
max_deg = _max_deg;
|
||||
average_elev_m = _average_elev_m;
|
||||
// cout << "min = " << min_deg << " max = " << max_deg
|
||||
// << " ave = " << average_elev_m << endl;
|
||||
_aptBounds = aptBounds;
|
||||
_min_deg = _aptBounds.getMin();
|
||||
_max_deg = _aptBounds.getMax();
|
||||
_average_elev_m = average_elev_m;
|
||||
|
||||
// The following size calculations are for the purpose of
|
||||
// determining grid divisions so it's not important that they be
|
||||
// *exact*, just ball park.
|
||||
double y_deg = max_deg.lat() - min_deg.lat();
|
||||
double y_deg = _max_deg.getLatitudeDeg() - _min_deg.getLatitudeDeg();
|
||||
double y_rad = y_deg * SG_DEGREES_TO_RADIANS;
|
||||
double y_nm = y_rad * SG_RAD_TO_NM;
|
||||
double y_m = y_nm * SG_NM_TO_METER;
|
||||
|
||||
double xfact = cos( min_deg.lat() * SG_DEGREES_TO_RADIANS );
|
||||
double x_deg = max_deg.lon() - min_deg.lon();
|
||||
double xfact = cos( _min_deg.getLatitudeRad() );
|
||||
double x_deg = _max_deg.getLongitudeDeg() - _min_deg.getLongitudeDeg();
|
||||
double x_rad = x_deg * SG_DEGREES_TO_RADIANS;
|
||||
double x_nm = x_rad * SG_RAD_TO_NM * xfact;
|
||||
double x_m = x_nm * SG_NM_TO_METER;
|
||||
|
@ -143,9 +133,9 @@ TGAptSurface::TGAptSurface( const std::string& path,
|
|||
SimpleMatrix dPts( (xdivs + 1) * mult + 1, (ydivs + 1) * mult + 1 );
|
||||
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
|
||||
dPts.set(i, j, SGGeod::fromDegM( _min_deg.getLongitudeDeg() - dlon_h
|
||||
+ i * (dlon / (double)mult),
|
||||
min_deg.lat() - dlat_h
|
||||
_min_deg.getLatitudeDeg() - dlat_h
|
||||
+ j * (dlat / (double)mult),
|
||||
-9999 )
|
||||
);
|
||||
|
@ -154,29 +144,10 @@ TGAptSurface::TGAptSurface( const std::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.rows(); ++j ) {
|
||||
for ( int i = 0; i < dPts.cols(); ++i ) {
|
||||
printf("hr %.5f %.5f %.1f\n",
|
||||
dPts.element(i,j).x(), dPts.element(i,j).y(),
|
||||
dPts.element(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.rows(); ++j ) {
|
||||
for ( int i = 0; i < dPts.cols(); ++i ) {
|
||||
printf("chr %.5f %.5f %.1f\n",
|
||||
dPts.element(i,j).x(), dPts.element(i,j).y(),
|
||||
dPts.element(i,j).z() );
|
||||
}
|
||||
}
|
||||
#endif
|
||||
tgClampElevations( dPts, _average_elev_m, max_clamp );
|
||||
|
||||
// Build the normal res input grid from the double res version
|
||||
Pts = new SimpleMatrix(xdivs + 1, ydivs + 1 );
|
||||
|
@ -189,40 +160,23 @@ TGAptSurface::TGAptSurface( const std::string& path,
|
|||
double lat_accum = 0.0;
|
||||
for ( int jj = 0; jj <= mult; ++jj ) {
|
||||
for ( int ii = 0; ii <= mult; ++ii ) {
|
||||
double value = dPts.element(mult*i + ii, mult*j + jj).z();
|
||||
double value = dPts.element(mult*i + ii, mult*j + jj).getElevationM();
|
||||
SG_LOG( SG_GENERAL, SG_DEBUG, "value = " << value );
|
||||
accum += value;
|
||||
lon_accum += dPts.element(mult*i + ii, mult*j + jj).x();
|
||||
lat_accum += dPts.element(mult*i + ii, mult*j + jj).y();
|
||||
lon_accum += dPts.element(mult*i + ii, mult*j + jj).getLongitudeDeg();
|
||||
lat_accum += dPts.element(mult*i + ii, mult*j + jj).getLatitudeDeg();
|
||||
}
|
||||
}
|
||||
double val_ave = accum / ave_divider;
|
||||
double lon_ave = lon_accum / ave_divider;
|
||||
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() + i * dlon,
|
||||
min_deg.lat() + j * dlat,
|
||||
Pts->set(i, j, SGGeod::fromDegM( _min_deg.getLongitudeDeg() + i * dlon,
|
||||
_min_deg.getLatitudeDeg() + j * 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() + j * dlon
|
||||
<< " lat = " << min_deg.lat() + i * dlat );
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef DEBUG
|
||||
for ( int j = 0; j < Pts->rows(); ++j ) {
|
||||
for ( int i = 0; i < Pts->cols(); ++i ) {
|
||||
printf("nr %.5f %.5f %.1f\n",
|
||||
Pts->element(i,j).x(),
|
||||
Pts->element(i,j).y(),
|
||||
Pts->element(i,j).z() );
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
bool slope_error = true;
|
||||
while ( slope_error ) {
|
||||
SG_LOG( SG_GENERAL, SG_DEBUG, "start of slope processing pass" );
|
||||
|
@ -230,58 +184,29 @@ TGAptSurface::TGAptSurface( const std::string& path,
|
|||
// Add some "slope" sanity to the resulting surface grid points
|
||||
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 ) ) {
|
||||
if ( limit_slope( Pts, i, j, i+1, j, _average_elev_m ) ) {
|
||||
slope_error = true;
|
||||
}
|
||||
if ( limit_slope( Pts, i, j, i, j+1, average_elev_m ) ) {
|
||||
if ( limit_slope( Pts, i, j, i, j+1, _average_elev_m ) ) {
|
||||
slope_error = true;
|
||||
}
|
||||
if ( limit_slope( Pts, i, j, i+1, j+1, average_elev_m ) ) {
|
||||
if ( limit_slope( Pts, i, j, i+1, j+1, _average_elev_m ) ) {
|
||||
slope_error = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef DEBUG
|
||||
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
|
||||
|
||||
// compute an central offset point.
|
||||
double clon = (min_deg.lon() + max_deg.lon()) / 2.0;
|
||||
double clat = (min_deg.lat() + max_deg.lat()) / 2.0;
|
||||
offset = Point3D( clon, clat, average_elev_m );
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, "Central offset point = " << offset);
|
||||
double clon = (_min_deg.getLongitudeDeg() + _max_deg.getLongitudeDeg()) / 2.0;
|
||||
double clat = (_min_deg.getLatitudeDeg() + _max_deg.getLatitudeDeg()) / 2.0;
|
||||
area_center = SGGeod::fromDegM( clon, clat, _average_elev_m );
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, "Central offset point = " << area_center);
|
||||
|
||||
// Create the fitted surface
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, "ready to create fitted surface");
|
||||
fit();
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, " fit process successful.");
|
||||
|
||||
#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;
|
||||
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(lon, lat) );
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
@ -313,11 +238,11 @@ void TGAptSurface::fit() {
|
|||
// generate the required fit data
|
||||
for ( int j = 0; j < Pts->rows(); j++ ) {
|
||||
for ( int i = 0; i < Pts->cols(); i++ ) {
|
||||
Point3D p = Pts->element( i, j );
|
||||
SGGeod p = Pts->element( i, j );
|
||||
int index = ( j * Pts->cols() ) + i;
|
||||
double x = p.x() - offset.x();
|
||||
double y = p.y() - offset.y();
|
||||
double z = p.z() - offset.z();
|
||||
double x = p.getLongitudeDeg() - area_center.getLongitudeDeg();
|
||||
double y = p.getLatitudeDeg() - area_center.getLatitudeDeg();
|
||||
double z = p.getElevationM() - area_center.getElevationM();
|
||||
|
||||
zmat[index] = z;
|
||||
|
||||
|
@ -348,10 +273,10 @@ void TGAptSurface::fit() {
|
|||
|
||||
|
||||
// Query the elevation of a point, return -9999 if out of range
|
||||
double TGAptSurface::query( double lon_deg, double lat_deg ) {
|
||||
double TGAptSurface::query( SGGeod query ) {
|
||||
|
||||
// sanity check
|
||||
if ( lon_deg < min_deg.lon() || lon_deg > max_deg.lon() ||
|
||||
lat_deg < min_deg.lat() || lat_deg > max_deg.lat() )
|
||||
if ( !_aptBounds.isInside(query) )
|
||||
{
|
||||
SG_LOG(SG_GENERAL, SG_WARN,
|
||||
"Warning: query out of bounds for fitted surface!");
|
||||
|
@ -366,15 +291,15 @@ double TGAptSurface::query( double lon_deg, double lat_deg ) {
|
|||
// 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 - offset.x();
|
||||
double y = lat_deg - offset.y();
|
||||
double x = query.getLongitudeDeg() - area_center.getLongitudeDeg();
|
||||
double y = query.getLatitudeDeg() - area_center.getLatitudeDeg();
|
||||
TNT::Array1D<double> A = surface_coefficients;
|
||||
|
||||
double result = A[0] + A[1]*x + A[2]*x*y + A[3]*y + A[4]*x*x + A[5]*x*x*y
|
||||
+ A[6]*x*x*y*y + A[7]*y*y + A[8]*x*y*y + A[9]*x*x*x + A[10]*x*x*x*y
|
||||
+ A[11]*x*x*x*y*y + A[12]*x*x*x*y*y*y + A[13]*y*y*y + A[14]*x*y*y*y
|
||||
+ A[15]*x*x*y*y*y;
|
||||
result += offset.z();
|
||||
result += area_center.getElevationM();
|
||||
|
||||
return result;
|
||||
}
|
||||
|
|
|
@ -29,11 +29,12 @@
|
|||
#include <Lib/Geometry/TNT/tnt_array2d.h>
|
||||
|
||||
#include <simgear/debug/logstream.hxx>
|
||||
#include <Lib/Geometry/point3d.hxx>
|
||||
#include <Lib/Polygon/polygon.hxx>
|
||||
|
||||
|
||||
|
||||
/***
|
||||
* A dirt simple matrix class for our convenience based on top of Point3D
|
||||
* A dirt simple matrix class for our convenience based on top of SGGeod
|
||||
*/
|
||||
|
||||
class SimpleMatrix {
|
||||
|
@ -42,17 +43,17 @@ private:
|
|||
|
||||
int _rows;
|
||||
int _cols;
|
||||
point_list m;
|
||||
tgContour m;
|
||||
|
||||
public:
|
||||
|
||||
inline SimpleMatrix( int columns, int rows ) {
|
||||
_cols = columns;
|
||||
_rows = rows;
|
||||
m.resize( _cols * _rows );
|
||||
m.Resize( _cols * _rows );
|
||||
}
|
||||
|
||||
inline Point3D element( int col, int row ) {
|
||||
inline SGGeod element( int col, int row ) {
|
||||
int index = ( row * _cols ) + col;
|
||||
if ( col < 0 || col >= _cols ) {
|
||||
SG_LOG(SG_GENERAL, SG_WARN, "column out of bounds on read (" << col << " >= " << _cols << ")");
|
||||
|
@ -61,10 +62,10 @@ public:
|
|||
SG_LOG(SG_GENERAL, SG_WARN, "row out of bounds on read (" << row << " >= " << _rows << ")");
|
||||
int *p = 0; *p = 1; // force crash
|
||||
}
|
||||
return m[index];
|
||||
return m.GetNode(index);
|
||||
}
|
||||
|
||||
inline void set( int col, int row, Point3D p ) {
|
||||
inline void set( int col, int row, SGGeod p ) {
|
||||
int index = ( row * _cols ) + col;
|
||||
if ( col < 0 || col >= _cols ) {
|
||||
SG_LOG(SG_GENERAL, SG_WARN,"column out of bounds on set (" << col << " >= " << _cols << ")");
|
||||
|
@ -73,7 +74,7 @@ public:
|
|||
SG_LOG(SG_GENERAL, SG_WARN,"row out of bounds on set (" << row << " >= " << _rows << ")");
|
||||
int *p = 0; *p = 1; // force crash
|
||||
}
|
||||
m[index] = p;
|
||||
m.SetNode(index, p);
|
||||
}
|
||||
|
||||
inline int cols() const { return _cols; }
|
||||
|
@ -102,13 +103,15 @@ private:
|
|||
SimpleMatrix *Pts;
|
||||
TNT::Array1D<double> surface_coefficients;
|
||||
|
||||
Point3D min_deg, max_deg;
|
||||
tg::Rectangle _aptBounds;
|
||||
|
||||
SGGeod _min_deg, _max_deg;
|
||||
|
||||
// A central point in the airport area
|
||||
Point3D offset;
|
||||
SGGeod area_center;
|
||||
|
||||
// externally seeded average airport elevation
|
||||
double average_elev_m;
|
||||
double _average_elev_m;
|
||||
|
||||
|
||||
public:
|
||||
|
@ -117,7 +120,7 @@ public:
|
|||
// lon/lat degrees, also please specify an "average" airport
|
||||
// elevations in meters.
|
||||
TGAptSurface( const std::string &path, const string_list& elev_src,
|
||||
Point3D _min_deg, Point3D _max_deg, double _average_elev_m );
|
||||
tg::Rectangle aptBounds, double average_elev_m );
|
||||
|
||||
// Destructor
|
||||
~TGAptSurface();
|
||||
|
@ -129,7 +132,7 @@ public:
|
|||
// Query the elevation of a point, return -9999 if out of range.
|
||||
// This routine makes a simplistic assumption that X,Y space is
|
||||
// proportional to u,v space on the nurbs surface which it isn't.
|
||||
double query( double lon_deg, double lat_deg );
|
||||
double query( SGGeod query );
|
||||
|
||||
};
|
||||
|
||||
|
|
|
@ -25,8 +25,7 @@
|
|||
#endif
|
||||
|
||||
#include <simgear/constants.h>
|
||||
#include <simgear/math/sg_geodesy.hxx>
|
||||
#include <simgear/math/sg_types.hxx>
|
||||
#include <simgear/math/SGMath.hxx>
|
||||
#include <simgear/debug/logstream.hxx>
|
||||
|
||||
#include <Array/array.hxx>
|
||||
|
@ -60,11 +59,11 @@ double tgAverageElevation( const std::string &root, const string_list elev_src,
|
|||
|
||||
while ( !done ) {
|
||||
// find first node with -9999 elevation
|
||||
Point3D first(0.0);
|
||||
SGGeod first = SGGeod();
|
||||
bool found_one = false;
|
||||
for ( i = 0; i < points.size(); ++i ) {
|
||||
if ( points[i].z() < -9000.0 && !found_one ) {
|
||||
first = points[i];
|
||||
first = points[i].toSGGeod();
|
||||
SG_LOG( SG_GENERAL, SG_DEBUG, "found first = " << first );
|
||||
|
||||
found_one = true;
|
||||
|
@ -72,7 +71,7 @@ double tgAverageElevation( const std::string &root, const string_list elev_src,
|
|||
}
|
||||
|
||||
if ( found_one ) {
|
||||
SGBucket b( first.x(), first.y() );
|
||||
SGBucket b( first );
|
||||
std::string base = b.gen_base_path();
|
||||
|
||||
// try the various elevation sources
|
||||
|
@ -107,9 +106,6 @@ double tgAverageElevation( const std::string &root, const string_list elev_src,
|
|||
points[i].lat() * 3600.0 );
|
||||
if ( elev > -9000 ) {
|
||||
points[i].setz( elev );
|
||||
// cout << "interpolating for " << p << endl;
|
||||
// cout << p.x() << " " << p.y() << " " << p.z()
|
||||
// << endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -151,20 +147,20 @@ void tgCalcElevations( const std::string &root, const string_list elev_src,
|
|||
// set all elevations to -9999
|
||||
for ( j = 0; j < Pts.rows(); ++j ) {
|
||||
for ( i = 0; i < Pts.cols(); ++i ) {
|
||||
Point3D p = Pts.element(i, j);
|
||||
p.setz( -9999.0 );
|
||||
SGGeod p = Pts.element(i, j);
|
||||
p.setElevationM(-9999.0);
|
||||
Pts.set(i, j, p);
|
||||
}
|
||||
}
|
||||
|
||||
while ( !done ) {
|
||||
// find first node with -9999 elevation
|
||||
Point3D first(0.0);
|
||||
SGGeod first = SGGeod();
|
||||
bool found_one = false;
|
||||
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 ) {
|
||||
SGGeod p = Pts.element(i,j);
|
||||
if ( p.getElevationM() < -9000.0 && !found_one ) {
|
||||
first = p;
|
||||
found_one = true;
|
||||
}
|
||||
|
@ -172,7 +168,7 @@ void tgCalcElevations( const std::string &root, const string_list elev_src,
|
|||
}
|
||||
|
||||
if ( found_one ) {
|
||||
SGBucket b( first.x(), first.y() );
|
||||
SGBucket b( first );
|
||||
std::string base = b.gen_base_path();
|
||||
|
||||
// try the various elevation sources
|
||||
|
@ -202,13 +198,13 @@ void tgCalcElevations( const std::string &root, const string_list elev_src,
|
|||
done = true;
|
||||
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 ) {
|
||||
SGGeod p = Pts.element(i,j);
|
||||
if ( p.getElevationM() < -9000.0 ) {
|
||||
done = false;
|
||||
elev = array.altitude_from_grid( p.x() * 3600.0,
|
||||
p.y() * 3600.0 );
|
||||
elev = array.altitude_from_grid( p.getLongitudeDeg() * 3600.0,
|
||||
p.getLatitudeDeg() * 3600.0 );
|
||||
if ( elev > -9000 ) {
|
||||
p.setz( elev );
|
||||
p.setElevationM( elev );
|
||||
Pts.set(i, j, p);
|
||||
}
|
||||
}
|
||||
|
@ -229,8 +225,8 @@ void tgCalcElevations( const std::string &root, const string_list elev_src,
|
|||
int count = 0;
|
||||
for ( j = 0; j < Pts.rows(); ++j ) {
|
||||
for ( i = 0; i < Pts.cols(); ++i ) {
|
||||
Point3D p = Pts.element(i,j);
|
||||
total += p.z();
|
||||
SGGeod p = Pts.element(i,j);
|
||||
total += p.getElevationM();
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
@ -249,17 +245,17 @@ void tgClampElevations( SimpleMatrix &Pts,
|
|||
// +/-max_m of the center_m elevation.
|
||||
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()
|
||||
SGGeod p = Pts.element(i,j);
|
||||
if ( p.getElevationM() < center_m - max_clamp_m ) {
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, " clamping " << p.getElevationM()
|
||||
<< " to " << center_m - max_clamp_m );
|
||||
p.setz( center_m - max_clamp_m );
|
||||
p.setElevationM( center_m - max_clamp_m );
|
||||
Pts.set(i, j, p);
|
||||
}
|
||||
if ( p.z() > center_m + max_clamp_m ) {
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, " clamping " << p.z()
|
||||
if ( p.getElevationM() > center_m + max_clamp_m ) {
|
||||
SG_LOG(SG_GENERAL, SG_DEBUG, " clamping " << p.getElevationM()
|
||||
<< " to " << center_m + max_clamp_m );
|
||||
p.setz( center_m + max_clamp_m );
|
||||
p.setElevationM( center_m + max_clamp_m );
|
||||
Pts.set(i, j, p);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -20,14 +20,7 @@
|
|||
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
|
||||
//
|
||||
|
||||
#include <simgear/constants.h>
|
||||
#include <simgear/math/sg_geodesy.hxx>
|
||||
#include <simgear/math/sg_types.hxx>
|
||||
#include <simgear/debug/logstream.hxx>
|
||||
|
||||
#include <Array/array.hxx>
|
||||
|
||||
#include "global.hxx"
|
||||
#include "apt_surface.hxx"
|
||||
|
||||
|
||||
|
|
|
@ -340,6 +340,10 @@ public:
|
|||
return node_list.size();
|
||||
}
|
||||
|
||||
void Resize( int size ) {
|
||||
node_list.resize( size );
|
||||
}
|
||||
|
||||
void AddNode( SGGeod n ) {
|
||||
node_list.push_back( n );
|
||||
}
|
||||
|
|
Loading…
Add table
Reference in a new issue