void
AirspaceDatabase::UpdateAGL(const RasterTerrain &terrain)
{
const RasterMap *map = terrain.GetMap();
if (map == NULL)
/* XXX: complain */
return;
// want most accurate rounding here
const RasterRounding rounding(*map, 0, 0);
for (unsigned i = 0; i < NumberOfAirspaceAreas; i++) {
AIRSPACE_AREA &area = AirspaceArea[i];
GEOPOINT center;
center.Latitude = (area.maxBound.Latitude + area.minBound.Latitude) / 2;
center.Longitude = (area.maxBound.Longitude + area.minBound.Longitude) / 2;
::UpdateAGL(AirspaceArea[i], center, terrain, rounding);
}
for (unsigned i = 0; i < NumberOfAirspaceCircles; i++) {
AIRSPACE_CIRCLE &circle = AirspaceCircle[i];
::UpdateAGL(circle, circle.Location, terrain, rounding);
}
}
void
WaypointAltitudeFromTerrain(WAYPOINT* Temp, RasterTerrain &terrain)
{
double myalt = -1;
if (terrain.GetMap()) {
RasterRounding rounding(*terrain.GetMap(),0,0);
myalt =
terrain.GetTerrainHeight(Temp->Location, rounding);
}
if (myalt>0) {
Temp->Altitude = myalt;
} else {
// error, can't find altitude for waypoint!
}
}
double
FinalGlideThroughTerrain(const double this_bearing,
const NMEA_INFO &basic,
const DERIVED_INFO &calculated,
const SETTINGS_COMPUTER &settings,
const RasterTerrain &terrain,
GEOPOINT *retloc,
const double max_range,
bool *out_of_range,
double *TerrainBase)
{
double mc = GlidePolar::GetMacCready();
double irange = GlidePolar::MacCreadyAltitude(mc,
1.0, this_bearing,
calculated.WindSpeed,
calculated.WindBearing,
0, 0, true, 0);
const GEOPOINT start_loc = basic.Location;
if (retloc) {
*retloc = start_loc;
}
*out_of_range = false;
if (irange <= 0.0 || calculated.NavAltitude <= 0)
// can't make progress in this direction at the current windspeed/mc
return 0;
const RasterMap *map = terrain.GetMap();
if (map == NULL)
return 0;
const double glide_max_range = calculated.NavAltitude/irange;
// returns distance one would arrive at altitude in straight glide
// first estimate max range at this altitude
GEOPOINT loc, last_loc;
double h=0.0, dh=0.0;
// int imax=0;
double last_dh=0;
double altitude;
double retval = 0;
int i=0;
bool start_under = false;
// calculate terrain rounding factor
FindLatitudeLongitude(start_loc, 0,
glide_max_range/NUMFINALGLIDETERRAIN, &loc);
double Xrounding = fabs(loc.Longitude-start_loc.Longitude)/2;
double Yrounding = fabs(loc.Latitude-start_loc.Latitude)/2;
const RasterRounding rounding(*map, Xrounding, Yrounding);
loc = last_loc = start_loc;
altitude = calculated.NavAltitude;
h = max(0, terrain.GetTerrainHeight(loc,rounding));
dh = altitude - h - settings.SafetyAltitudeTerrain;
last_dh = dh;
if (dh<0) {
start_under = true;
// already below safety terrain height
// retval = 0;
// goto OnExit;
}
// find grid
GEOPOINT dloc;
FindLatitudeLongitude(loc, this_bearing, glide_max_range, &dloc);
dloc.Latitude -= start_loc.Latitude;
dloc.Longitude -= start_loc.Longitude;
double f_scale = 1.0/NUMFINALGLIDETERRAIN;
if ((max_range>0) && (max_range<glide_max_range)) {
f_scale *= max_range/glide_max_range;
}
double delta_alt = -f_scale * calculated.NavAltitude;
dloc.Latitude *= f_scale;
dloc.Longitude *= f_scale;
for (i=1; i<=NUMFINALGLIDETERRAIN; i++) {
double f;
bool solution_found = false;
double fi = i*f_scale;
// fraction of glide_max_range
if ((max_range>0)&&(fi>=1.0)) {
// early exit
*out_of_range = true;
return max_range;
}
if (start_under) {
altitude += 2.0*delta_alt;
} else {
altitude += delta_alt;
}
// find lat, lon of point of interest
loc.Latitude += dloc.Latitude;
loc.Longitude += dloc.Longitude;
// find height over terrain
h = max(0,terrain.GetTerrainHeight(loc, rounding));
dh = altitude - h - settings.SafetyAltitudeTerrain;
if (TerrainBase && (dh>0) && (h>0)) {
*TerrainBase = min(*TerrainBase, h);
}
if (start_under) {
if (dh>last_dh) {
// better solution found, ok to continue...
if (dh>0) {
// we've now found a terrain point above safety altitude,
// so consider rest of track to search for safety altitude
start_under = false;
}
} else {
f= 0.0;
solution_found = true;
}
} else if (dh<=0) {
if ((dh<last_dh) && (last_dh>0)) {
f = max(0,min(1,(-last_dh)/(dh-last_dh)));
} else {
f = 0.0;
}
solution_found = true;
}
if (solution_found) {
loc.Latitude = last_loc.Latitude*(1.0-f)+loc.Latitude*f;
loc.Longitude = last_loc.Longitude*(1.0-f)+loc.Longitude*f;
if (retloc) {
*retloc = loc;
}
return Distance(start_loc, loc);
}
last_dh = dh;
last_loc = loc;
}
*out_of_range = true;
retval = glide_max_range;
return retval;
}
