X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNavaids%2Fnavlist.cxx;h=a145bac0d9d5d7090e4c237597062da2234d6366;hb=d8f5529d217c3710bcece9244f1f41cfb8550669;hp=4e1bea4c817a71b12ff3e699fd38fcafbb32df3b;hpb=81797885ce38edf58d786b3e54555aca3070a08e;p=flightgear.git diff --git a/src/Navaids/navlist.cxx b/src/Navaids/navlist.cxx index 4e1bea4c8..a145bac0d 100644 --- a/src/Navaids/navlist.cxx +++ b/src/Navaids/navlist.cxx @@ -16,7 +16,7 @@ // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software -// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. // // $Id$ @@ -35,34 +35,37 @@ // Return true if the nav record matches the type static bool isTypeMatch(const FGNavRecord* n, fg_nav_types type) { switch(type) { - case FG_NAV_ANY: return(true); - case FG_NAV_VOR: return(n->get_type() == 3); - case FG_NAV_NDB: return(n->get_type() == 2); - case FG_NAV_ILS: return(n->get_type() == 4); // Note - very simplified, only matches loc as part of full ILS. - default: return false; + case FG_NAV_ANY: return(true); + case FG_NAV_VOR: return(n->get_type() == 3); + case FG_NAV_NDB: return(n->get_type() == 2); + case FG_NAV_ILS: return(n->get_type() == 4); // Note - very simplified, only matches loc as part of full ILS. + default: return false; } } -// Constructor -FGNavList::FGNavList( void ) { -} -FGTACANList::FGTACANList( void ){ +// FGNavList ------------------------------------------------------------------ + +FGNavList::FGNavList( void ) +{ } -// Destructor -FGNavList::~FGNavList( void ) { +FGNavList::~FGNavList( void ) +{ + navaids_by_tile.erase( navaids_by_tile.begin(), navaids_by_tile.end() ); + nav_list_type navlist = navaids.begin()->second; + navaids.erase( navaids.begin(), navaids.end() ); } -FGTACANList::~FGTACANList( void ){ -} // load the navaids and build the map -bool FGNavList::init() { - - // FIXME: leaves all the individual navaid entries leaked +bool FGNavList::init() +{ + // No need to delete the original navaid structures + // since we're using an SGSharedPointer + nav_list_type navlist = navaids.begin()->second; navaids.erase( navaids.begin(), navaids.end() ); navaids_by_tile.erase( navaids_by_tile.begin(), navaids_by_tile.end() ); ident_navaids.erase( ident_navaids.begin(), ident_navaids.end() ); @@ -70,10 +73,6 @@ bool FGNavList::init() { return true; } -bool FGTACANList::init() { - - return true; -} // real add a marker beacon static void real_add( nav_map_type &navmap, const int master_index, @@ -84,8 +83,9 @@ static void real_add( nav_map_type &navmap, const int master_index, // front end for add a marker beacon -static void tile_add( nav_map_type &navmap, FGNavRecord *n ) { - double diff; +static void tile_add( nav_map_type &navmap, FGNavRecord *n ) +{ + double diff = 0; double lon = n->get_lon(); double lat = n->get_lat(); @@ -93,7 +93,7 @@ static void tile_add( nav_map_type &navmap, FGNavRecord *n ) { int lonidx = (int)lon; diff = lon - (double)lonidx; if ( (lon < 0.0) && (fabs(diff) > SG_EPSILON) ) { - lonidx -= 1; + lonidx -= 1; } double lonfrac = lon - (double)lonidx; lonidx += 180; @@ -101,7 +101,7 @@ static void tile_add( nav_map_type &navmap, FGNavRecord *n ) { int latidx = (int)lat; diff = lat - (double)latidx; if ( (lat < 0.0) && (fabs(diff) > SG_EPSILON) ) { - latidx -= 1; + latidx -= 1; } double latfrac = lat - (double)latidx; latidx += 90; @@ -119,46 +119,43 @@ static void tile_add( nav_map_type &navmap, FGNavRecord *n ) { // there are 8 cases since there are 8 adjacent tiles if ( lonfrac < 0.2 ) { - real_add( navmap, master_index - 1000, n ); - if ( latfrac < 0.2 ) { - real_add( navmap, master_index - 1000 - 1, n ); - } else if ( latfrac > 0.8 ) { - real_add( navmap, master_index - 1000 + 1, n ); - } + real_add( navmap, master_index - 1000, n ); + if ( latfrac < 0.2 ) { + real_add( navmap, master_index - 1000 - 1, n ); + } else if ( latfrac > 0.8 ) { + real_add( navmap, master_index - 1000 + 1, n ); + } } else if ( lonfrac > 0.8 ) { - real_add( navmap, master_index + 1000, n ); - if ( latfrac < 0.2 ) { - real_add( navmap, master_index + 1000 - 1, n ); - } else if ( latfrac > 0.8 ) { - real_add( navmap, master_index + 1000 + 1, n ); - } + real_add( navmap, master_index + 1000, n ); + if ( latfrac < 0.2 ) { + real_add( navmap, master_index + 1000 - 1, n ); + } else if ( latfrac > 0.8 ) { + real_add( navmap, master_index + 1000 + 1, n ); + } } else if ( latfrac < 0.2 ) { - real_add( navmap, master_index - 1, n ); + real_add( navmap, master_index - 1, n ); } else if ( latfrac > 0.8 ) { - real_add( navmap, master_index + 1, n ); + real_add( navmap, master_index + 1, n ); } } - // add an entry to the lists -bool FGNavList::add( FGNavRecord *n ) { +bool FGNavList::add( FGNavRecord *n ) +{ navaids[n->get_freq()].push_back(n); ident_navaids[n->get_ident()].push_back(n); tile_add( navaids_by_tile, n ); return true; } -// add an entry to the lists -bool FGTACANList::add( FGTACANRecord *c ) { - ident_channels[c->get_channel()].push_back(c); - return true; -} FGNavRecord *FGNavList::findByFreq( double freq, double lon, double lat, double elev ) { - nav_list_type stations = navaids[(int)(freq*100.0 + 0.5)]; - Point3D aircraft = sgGeodToCart( Point3D(lon, lat, elev) ); + const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)]; + + SGGeod geod = SGGeod::fromRadM(lon, lat, elev); + SGVec3d aircraft = SGVec3d::fromGeod(geod); SG_LOG( SG_INSTR, SG_DEBUG, "findbyFreq " << freq << " size " << stations.size() ); return findNavFromList( aircraft, stations ); @@ -168,75 +165,75 @@ FGNavRecord *FGNavList::findByFreq( double freq, double lon, double lat, double FGNavRecord *FGNavList::findByIdent( const char* ident, const double lon, const double lat ) { - nav_list_type stations = ident_navaids[ident]; - Point3D aircraft = sgGeodToCart( Point3D(lon, lat, 0.0) ); - + const nav_list_type& stations = ident_navaids[ident]; + SGGeod geod = SGGeod::fromRad(lon, lat); + SGVec3d aircraft = SGVec3d::fromGeod(geod); return findNavFromList( aircraft, stations ); } -nav_list_type FGNavList::findFirstByIdent( string ident, fg_nav_types type, bool exact) +nav_list_type FGNavList::findFirstByIdent( const string& ident, fg_nav_types type, bool exact) { nav_list_type n2; n2.clear(); - + int iType; if(type == FG_NAV_VOR) iType = 3; else if(type == FG_NAV_NDB) iType = 2; else return(n2); - + nav_ident_map_iterator it; if(exact) { - it = ident_navaids.find(ident); + it = ident_navaids.find(ident); } else { - bool typeMatch = false; - int safety_count = 0; - it = ident_navaids.lower_bound(ident); - while(!typeMatch) { - nav_list_type n0 = it->second; - // local copy, so we should be able to do anything with n0. - // Remove the types that don't match request. - for(nav_list_iterator it0 = n0.begin(); it0 != n0.end();) { - FGNavRecord* nv = *it0; - if(nv->get_type() == iType) { - typeMatch = true; - ++it0; - } else { - it0 = n0.erase(it0); - } - } - if(typeMatch) { - return(n0); - } - if(it == ident_navaids.begin()) { - // We didn't find a match before reaching the beginning of the map - n0.clear(); - return(n0); - } - safety_count++; - if(safety_count == 1000000) { - SG_LOG(SG_INSTR, SG_ALERT, - "safety_count triggered exit from while loop in findFirstByIdent!"); - break; - } - ++it; - if(it == ident_navaids.end()) { - n0.clear(); - return(n0); - } - } + bool typeMatch = false; + int safety_count = 0; + it = ident_navaids.lower_bound(ident); + while(!typeMatch) { + nav_list_type n0 = it->second; + // local copy, so we should be able to do anything with n0. + // Remove the types that don't match request. + for(nav_list_iterator it0 = n0.begin(); it0 != n0.end();) { + FGNavRecord* nv = *it0; + if(nv->get_type() == iType) { + typeMatch = true; + ++it0; + } else { + it0 = n0.erase(it0); + } + } + if(typeMatch) { + return(n0); + } + if(it == ident_navaids.begin()) { + // We didn't find a match before reaching the beginning of the map + n0.clear(); + return(n0); + } + safety_count++; + if(safety_count == 1000000) { + SG_LOG(SG_INSTR, SG_ALERT, + "safety_count triggered exit from while loop in findFirstByIdent!"); + break; + } + ++it; + if(it == ident_navaids.end()) { + n0.clear(); + return(n0); + } + } } if(it == ident_navaids.end()) { - n2.clear(); - return(n2); + n2.clear(); + return(n2); } else { - nav_list_type n1 = it->second; - n2.clear(); - for(nav_list_iterator it2 = n1.begin(); it2 != n1.end(); ++it2) { - FGNavRecord* nv = *it2; - if(nv->get_type() == iType) n2.push_back(nv); - } - return(n2); + nav_list_type n1 = it->second; + n2.clear(); + for(nav_list_iterator it2 = n1.begin(); it2 != n1.end(); ++it2) { + FGNavRecord* nv = *it2; + if(nv->get_type() == iType) n2.push_back(nv); + } + return(n2); } } @@ -251,12 +248,13 @@ FGNavRecord *FGNavList::findByIdentAndFreq( const char* ident, const double freq // sometimes there can be duplicated idents. If a freq is // specified, use it to refine the search. int f = (int)(freq*100.0 + 0.5); - for ( unsigned int i = 0; i < stations.size(); ++i ) { - if ( f == stations[i]->get_freq() ) { - return stations[i]; + nav_list_const_iterator it, end = stations.end(); + for ( it = stations.begin(); it != end; ++it ) { + if ( f == (*it)->get_freq() ) { + return (*it); } } - } else if (stations.size()) { + } else if (!stations.empty()) { return stations[0]; } @@ -266,26 +264,24 @@ FGNavRecord *FGNavList::findByIdentAndFreq( const char* ident, const double freq // Given a point and a list of stations, return the closest one to the // specified point. -FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft, +FGNavRecord *FGNavList::findNavFromList( const SGVec3d &aircraft, const nav_list_type &stations ) { FGNavRecord *nav = NULL; - Point3D station; double d2; // in meters squared double min_dist = FG_NAV_MAX_RANGE*SG_NM_TO_METER*FG_NAV_MAX_RANGE*SG_NM_TO_METER; + nav_list_const_iterator it; + nav_list_const_iterator end = stations.end(); // find the closest station within a sensible range (FG_NAV_MAX_RANGE) - for ( unsigned int i = 0; i < stations.size(); ++i ) { - // cout << "testing " << current->get_ident() << endl; - station = Point3D( stations[i]->get_x(), - stations[i]->get_y(), - stations[i]->get_z() ); - - d2 = aircraft.distance3Dsquared( station ); + for ( it = stations.begin(); it != end; ++it ) { + FGNavRecord *station = *it; + // cout << "testing " << current->get_ident() << endl; + d2 = distSqr(station->get_cart(), aircraft); - // cout << " dist = " << sqrt(d) - // << " range = " << current->get_range() * SG_NM_TO_METER + // cout << " dist = " << sqrt(d) + // << " range = " << current->get_range() * SG_NM_TO_METER // << endl; // LOC, ILS, GS, and DME antenna's could potentially be @@ -298,32 +294,28 @@ FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft, // placed from each other. (Do the expensive check only for // directional atennas and only when there is a chance it is // the closest station.) - if ( d2 < min_dist && - (stations[i]->get_type() == 4 || stations[i]->get_type() == 5 || - stations[i]->get_type() == 6 || stations[i]->get_type() == 12) ) + int type = station->get_type(); + if ( d2 < min_dist && + (type == 4 || type == 5 || type == 6 || type == 12 || type == 13) ) { double hdg_deg = 0.0; - if ( stations[i]->get_type() == 4 || stations[i]->get_type() == 5 ){ - hdg_deg = stations[i]->get_multiuse(); - } else if ( stations[i]->get_type() == 6 ) { - int tmp = (int)(stations[i]->get_multiuse() / 1000.0); - hdg_deg = stations[i]->get_multiuse() - (tmp * 1000); - } else if ( stations[i]->get_type() == 12 ) { + if ( type == 4 || type == 5 ){ + hdg_deg = station->get_multiuse(); + + } else if ( type == 6 ) { + int tmp = (int)(station->get_multiuse() / 1000.0); + hdg_deg = station->get_multiuse() - (tmp * 1000); + + } else if ( type == 12 || type == 13 ) { // oops, Robin's data format doesn't give us the // needed information to compute a heading for a DME // transmitter. FIXME Robin! } double az1 = 0.0, az2 = 0.0, s = 0.0; - double elev_m = 0.0, lat_rad = 0.0, lon_rad = 0.0; - double xyz[3] = { aircraft.x(), aircraft.y(), aircraft.z() }; - sgCartToGeod( xyz, &lat_rad, &lon_rad, &elev_m ); - geo_inverse_wgs_84( elev_m, - lat_rad * SG_RADIANS_TO_DEGREES, - lon_rad * SG_RADIANS_TO_DEGREES, - stations[i]->get_lat(), stations[i]->get_lon(), - &az1, &az2, &s); - az1 = az1 - stations[i]->get_multiuse(); + SGGeod geod = SGGeod::fromCart(aircraft); + geo_inverse_wgs_84( geod, station->get_pos(), &az1, &az2, &s); + az1 = az1 - station->get_multiuse(); if ( az1 > 180.0) az1 -= 360.0; if ( az1 < -180.0) az1 += 360.0; // penalize opposite facing stations by adding 5000 meters @@ -337,7 +329,7 @@ FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft, if ( d2 < min_dist ) { min_dist = d2; - nav = stations[i]; + nav = station; } } @@ -357,54 +349,48 @@ FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad, int lonidx = (int)lon_deg; diff = lon_deg - (double)lonidx; if ( (lon_deg < 0.0) && (fabs(diff) > SG_EPSILON) ) { - lonidx -= 1; + lonidx -= 1; } lonidx += 180; int latidx = (int)lat_deg; diff = lat_deg - (double)latidx; if ( (lat_deg < 0.0) && (fabs(diff) > SG_EPSILON) ) { - latidx -= 1; + latidx -= 1; } latidx += 90; int master_index = lonidx * 1000 + latidx; - nav_list_type navs = navaids_by_tile[ master_index ]; + const nav_list_type& navs = navaids_by_tile[ master_index ]; // cout << "Master index = " << master_index << endl; // cout << "beacon search length = " << beacons.size() << endl; nav_list_const_iterator current = navs.begin(); nav_list_const_iterator last = navs.end(); - Point3D aircraft = sgGeodToCart( Point3D(lon_rad, - lat_rad, - elev_m) ); + SGGeod geod = SGGeod::fromRadM(lon_rad, lat_rad, elev_m); + SGVec3d aircraft = SGVec3d::fromGeod(geod); double min_dist = 999999999.0; for ( ; current != last ; ++current ) { - if(isTypeMatch(*current, type)) { - // cout << " testing " << (*current)->get_ident() << endl; - Point3D station = Point3D( (*current)->get_x(), - (*current)->get_y(), - (*current)->get_z() ); - // cout << " aircraft = " << aircraft << " station = " << station - // << endl; - - double d = aircraft.distance3Dsquared( station ); // meters^2 - // cout << " distance = " << d << " (" - // << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER - // * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER - // << ")" << endl; - - // cout << " range = " << sqrt(d) << endl; - - if ( d < min_dist ) { - min_dist = d; - result = (*current); - } - } + if(isTypeMatch(*current, type)) { + // cout << " testing " << (*current)->get_ident() << endl; + + double d = distSqr((*current)->get_cart(), aircraft); + // cout << " distance = " << d << " (" + // << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER + // * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER + // << ")" << endl; + + // cout << " range = " << sqrt(d) << endl; + + if ( d < min_dist ) { + min_dist = d; + result = (*current); + } + } } // cout << "lon = " << lon << " lat = " << lat @@ -413,27 +399,57 @@ FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad, return result; } -// Given a TACAN Channel return the first matching frequency -FGTACANRecord *FGTACANList::findByChannel( const string& channel ) +// Given a frequency, return the first matching station. +FGNavRecord *FGNavList::findStationByFreq( double freq ) { - tacan_list_type stations = ident_channels[channel]; - SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() ); - - if (stations.size()) { + const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)]; + + SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size() ); + + if (!stations.empty()) { return stations[0]; - } + } return NULL; } -// Given a frequency, return the first matching station. -FGNavRecord *FGNavList::findStationByFreq( double freq ) + + +// FGTACANList ---------------------------------------------------------------- + +FGTACANList::FGTACANList( void ) { - nav_list_type stations = navaids[(int)(freq*100.0 + 0.5)]; - - SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size() ); - - if (stations.size()) { +} + + +FGTACANList::~FGTACANList( void ) +{ +} + + +bool FGTACANList::init() +{ + return true; +} + + +// add an entry to the lists +bool FGTACANList::add( FGTACANRecord *c ) +{ + ident_channels[c->get_channel()].push_back(c); + return true; +} + + +// Given a TACAN Channel return the first matching frequency +FGTACANRecord *FGTACANList::findByChannel( const string& channel ) +{ + const tacan_list_type& stations = ident_channels[channel]; + SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() ); + + if (!stations.empty()) { return stations[0]; - } + } return NULL; } + +