diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index 6062f8dbe..cdc1fbf7e 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -909,12 +909,6 @@ void FGAIAircraft::getGroundElev(double dt) { else dt_elev_count = 0; - // It would be nice if we could set the correct tile center here in order to get a correct - // answer with one call to the function, but what I tried in the two commented-out lines - // below only intermittently worked, and I haven't quite groked why yet. - //SGBucket buck(pos.lon(), pos.lat()); - //aip.getSGLocation()->set_tile_center(Point3D(buck.get_center_lon(), buck.get_center_lat(), 0.0)); - // Only do the proper hitlist stuff if we are within visible range of the viewer. if (!invisible) { double visibility_meters = fgGetDouble("/environment/visibility-m"); @@ -922,8 +916,6 @@ void FGAIAircraft::getGroundElev(double dt) { FGViewer* vw = globals->get_current_view(); double course, distance; - //Point3D currView(vw->getLongitude_deg(), - // vw->getLatitude_deg(), 0.0); SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0); SGWayPoint view (vw->getLongitude_deg(), vw->getLatitude_deg(), 0); view.CourseAndDistance(current, &course, &distance); diff --git a/src/Instrumentation/adf.cxx b/src/Instrumentation/adf.cxx index b6de552c8..efae4aa34 100644 --- a/src/Instrumentation/adf.cxx +++ b/src/Instrumentation/adf.cxx @@ -61,7 +61,8 @@ ADF::ADF (SGPropertyNode *node ) _time_before_search_sec(0), _last_frequency_khz(-1), _transmitter_valid(false), - _transmitter_elevation_ft(0), + _transmitter_pos(SGGeod::fromDeg(0, 0)), + _transmitter_cart(0, 0, 0), _transmitter_range_nm(0), _ident_count(0), _last_ident_time(0), @@ -92,7 +93,8 @@ ADF::ADF () : _time_before_search_sec(0), _last_frequency_khz(-1), _transmitter_valid(false), - _transmitter_elevation_ft(0), + _transmitter_pos(SGGeod::fromDeg(0, 0)), + _transmitter_cart(0, 0, 0), _transmitter_range_nm(0), _ident_count(0), _last_ident_time(0), @@ -178,11 +180,11 @@ ADF::update (double delta_time_sec) } // Calculate the bearing to the transmitter - Point3D location = - sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m)); - - double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM; - double range_nm = adjust_range(_transmitter_elevation_ft, + SGGeod geod = SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m); + SGVec3d location = SGVec3d::fromGeod(geod); + + double distance_nm = dist(_transmitter_cart, location) * SG_METER_TO_NM; + double range_nm = adjust_range(_transmitter_pos.getElevationFt(), altitude_m * SG_METER_TO_FEET, _transmitter_range_nm); if (distance_nm <= range_nm) { @@ -190,11 +192,7 @@ ADF::update (double delta_time_sec) double bearing, az2, s; double heading = _heading_node->getDoubleValue(); - geo_inverse_wgs_84(altitude_m, - latitude_deg, - longitude_deg, - _transmitter_lat_deg, - _transmitter_lon_deg, + geo_inverse_wgs_84(geod, _transmitter_pos, &bearing, &az2, &s); _in_range_node->setBoolValue(true); @@ -258,10 +256,8 @@ ADF::search (double frequency_khz, double longitude_rad, if ( _transmitter_valid ) { ident = nav->get_trans_ident(); if ( ident != _last_ident ) { - _transmitter_lon_deg = nav->get_lon(); - _transmitter_lat_deg = nav->get_lat(); - _transmitter = Point3D(nav->get_x(), nav->get_y(), nav->get_z()); - _transmitter_elevation_ft = nav->get_elev_ft(); + _transmitter_pos = nav->get_pos(); + _transmitter_cart = nav->get_cart(); _transmitter_range_nm = nav->get_range(); } } diff --git a/src/Instrumentation/adf.hxx b/src/Instrumentation/adf.hxx index 6f584803c..d43bdce60 100644 --- a/src/Instrumentation/adf.hxx +++ b/src/Instrumentation/adf.hxx @@ -85,10 +85,8 @@ private: int _last_frequency_khz; bool _transmitter_valid; string _last_ident; - Point3D _transmitter; - double _transmitter_lon_deg; - double _transmitter_lat_deg; - double _transmitter_elevation_ft; + SGGeod _transmitter_pos; + SGVec3d _transmitter_cart; double _transmitter_range_nm; FGMorse morse; diff --git a/src/Instrumentation/dme.cxx b/src/Instrumentation/dme.cxx index 21f05386d..485412a12 100644 --- a/src/Instrumentation/dme.cxx +++ b/src/Instrumentation/dme.cxx @@ -152,9 +152,10 @@ DME::update (double delta_time_sec) } // Calculate the distance to the transmitter - Point3D location = - sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m)); - double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM; + SGGeod geod = SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m); + SGVec3d location = SGVec3d::fromGeod(geod); + + double distance_nm = dist(_transmitter, location) * SG_METER_TO_NM; double range_nm = adjust_range(_transmitter_elevation_ft, altitude_m * SG_METER_TO_FEET, @@ -198,7 +199,7 @@ DME::search (double frequency_mhz, double longitude_rad, _transmitter_valid = (dme != NULL); if ( _transmitter_valid ) { - _transmitter = Point3D(dme->get_x(), dme->get_y(), dme->get_z()); + _transmitter = dme->get_cart(); _transmitter_elevation_ft = dme->get_elev_ft(); _transmitter_range_nm = dme->get_range(); _transmitter_bias = dme->get_multiuse(); diff --git a/src/Instrumentation/dme.hxx b/src/Instrumentation/dme.hxx index 1fc5781c6..1d3e2e1cc 100644 --- a/src/Instrumentation/dme.hxx +++ b/src/Instrumentation/dme.hxx @@ -71,7 +71,7 @@ private: double _time_before_search_sec; bool _transmitter_valid; - Point3D _transmitter; + SGVec3d _transmitter; double _transmitter_elevation_ft; double _transmitter_range_nm; double _transmitter_bias; diff --git a/src/Instrumentation/kr_87.cxx b/src/Instrumentation/kr_87.cxx index aa2ffab90..c909b7cee 100644 --- a/src/Instrumentation/kr_87.cxx +++ b/src/Instrumentation/kr_87.cxx @@ -244,14 +244,12 @@ void FGKR_87::unbind () { // Update the various nav values based on position and valid tuned in navs void FGKR_87::update( double dt_sec ) { - double acft_lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; - double acft_lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; - double acft_elev = alt_node->getDoubleValue() * SG_FEET_TO_METER; + SGGeod acft = SGGeod::fromDegFt(lon_node->getDoubleValue(), + lat_node->getDoubleValue(), + alt_node->getDoubleValue()); need_update = false; - Point3D aircraft = sgGeodToCart( Point3D( acft_lon, acft_lat, acft_elev ) ); - Point3D station; double az1, az2, s; // On timeout, scan again @@ -361,20 +359,17 @@ void FGKR_87::update( double dt_sec ) { if ( valid ) { // cout << "adf is valid" << endl; // staightline distance - station = Point3D( x, y, z ); - dist = aircraft.distance3D( station ); + // What a hack, dist is a class local variable + dist = sqrt(distSqr(SGVec3d::fromGeod(acft), xyz)); // wgs84 heading - geo_inverse_wgs_84( acft_elev, - acft_lat * SGD_RADIANS_TO_DEGREES, - acft_lon * SGD_RADIANS_TO_DEGREES, - stn_lat, stn_lon, + geo_inverse_wgs_84( acft, SGGeod::fromDeg(stn_lon, stn_lat), &az1, &az2, &s ); heading = az1; // cout << " heading = " << heading // << " dist = " << dist << endl; - effective_range = kludgeRange(stn_elev, acft_elev, range); + effective_range = kludgeRange(stn_elev, acft.getElevationFt(), range); if ( dist < effective_range * SG_NM_TO_METER ) { inrange = true; } else if ( dist < 2 * effective_range * SG_NM_TO_METER ) { @@ -530,9 +525,7 @@ void FGKR_87::search() { stn_elev = adf->get_elev_ft(); range = adf->get_range(); effective_range = kludgeRange(stn_elev, acft_elev, range); - x = adf->get_x(); - y = adf->get_y(); - z = adf->get_z(); + xyz = adf->get_cart(); if ( globals->get_soundmgr()->exists( "adf-ident" ) ) { globals->get_soundmgr()->remove( "adf-ident" ); diff --git a/src/Instrumentation/kr_87.hxx b/src/Instrumentation/kr_87.hxx index 1664352ab..0b57a8e5f 100644 --- a/src/Instrumentation/kr_87.hxx +++ b/src/Instrumentation/kr_87.hxx @@ -59,9 +59,7 @@ class FGKR_87 : public SGSubsystem double effective_range; double dist; double heading; - double x; - double y; - double z; + SGVec3d xyz; double goal_needle_deg; double et_flash_time; diff --git a/src/Instrumentation/marker_beacon.cxx b/src/Instrumentation/marker_beacon.cxx index e9d54acff..854a4ebb9 100644 --- a/src/Instrumentation/marker_beacon.cxx +++ b/src/Instrumentation/marker_beacon.cxx @@ -201,15 +201,16 @@ FGMarkerBeacon::update(double dt) } -static bool check_beacon_range( double lon_rad, double lat_rad, double elev_m, +static bool check_beacon_range( const SGGeod& pos, FGNavRecord *b ) { - Point3D aircraft = sgGeodToCart( Point3D(lon_rad, lat_rad, elev_m) ); - Point3D station = Point3D( b->get_x(), b->get_y(), b->get_z() ); + SGVec3d aircraft = SGVec3d::fromGeod(pos); + SGVec3d station = b->get_cart(); // cout << " aircraft = " << aircraft << " station = " << station // << endl; - double d = aircraft.distance3Dsquared( station ); // meters^2 + SGVec3d tmp = station - aircraft; + double d = dot(tmp, tmp); // cout << " distance = " << d << " (" // << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER // * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER @@ -219,7 +220,7 @@ static bool check_beacon_range( double lon_rad, double lat_rad, double elev_m, // cout << "elev = " << elev * SG_METER_TO_FEET // << " current->get_elev() = " << current->get_elev() << endl; - double elev_ft = elev_m * SG_METER_TO_FEET; + double elev_ft = pos.getElevationFt(); double delev = elev_ft - b->get_elev_ft(); // max range is the area under r = 2.4 * alt or r^2 = 4000^2 - alt^2 @@ -253,9 +254,9 @@ void FGMarkerBeacon::search() static fgMkrBeacType last_beacon = NOBEACON; - double lon_rad = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; - double lat_rad = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; - double elev_m = alt_node->getDoubleValue() * SG_FEET_TO_METER; + SGGeod pos = SGGeod::fromDegFt(lon_node->getDoubleValue(), + lat_node->getDoubleValue(), + alt_node->getDoubleValue()); //////////////////////////////////////////////////////////////////////// // Beacons. @@ -263,7 +264,9 @@ void FGMarkerBeacon::search() // get closest marker beacon FGNavRecord *b - = globals->get_mkrlist()->findClosest( lon_rad, lat_rad, elev_m ); + = globals->get_mkrlist()->findClosest( pos.getLongitudeRad(), + pos.getLatitudeRad(), + pos.getElevationM() ); // cout << "marker beacon = " << b << " (" << b->get_type() << ")" << endl; @@ -277,7 +280,7 @@ void FGMarkerBeacon::search() } else if ( b->get_type() == 9 ) { beacon_type = INNER; } - inrange = check_beacon_range( lon_rad, lat_rad, elev_m, b ); + inrange = check_beacon_range( pos, b ); // cout << " inrange = " << inrange << endl; } diff --git a/src/Instrumentation/navradio.cxx b/src/Instrumentation/navradio.cxx index 697d3c8d7..511592e76 100644 --- a/src/Instrumentation/navradio.cxx +++ b/src/Instrumentation/navradio.cxx @@ -323,9 +323,9 @@ FGNavRadio::update(double dt) } // cache a few strategic values locally for speed - double lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; - double lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; - double elev = alt_node->getDoubleValue() * SG_FEET_TO_METER; + SGGeod pos = SGGeod::fromDegFt(lon_node->getDoubleValue(), + lat_node->getDoubleValue(), + alt_node->getDoubleValue()); bool power_btn = power_btn_node->getBoolValue(); bool nav_serviceable = nav_serviceable_node->getBoolValue(); bool cdi_serviceable = cdi_serviceable_node->getBoolValue(); @@ -335,8 +335,6 @@ FGNavRadio::update(double dt) bool is_loc = loc_node->getBoolValue(); double loc_dist = loc_dist_node->getDoubleValue(); - Point3D aircraft = sgGeodToCart( Point3D( lon, lat, elev ) ); - Point3D station; double az1, az2, s; // Create "formatted" versions of the nav frequencies for @@ -356,31 +354,22 @@ FGNavRadio::update(double dt) if ( is_valid && power_btn && (bus_power_node->getDoubleValue() > 1.0) && nav_serviceable ) { - station = Point3D( nav_x, nav_y, nav_z ); - loc_dist = aircraft.distance3D( station ); + SGVec3d aircraft = SGVec3d::fromGeod(pos); + loc_dist = dist(aircraft, nav_xyz); loc_dist_node->setDoubleValue( loc_dist ); // cout << "station = " << station << " dist = " << loc_dist << endl; if ( has_gs ) { // find closest distance to the gs base line - sgdVec3 p; - sgdSetVec3( p, aircraft.x(), aircraft.y(), aircraft.z() ); - sgdVec3 p0; - sgdSetVec3( p0, gs_x, gs_y, gs_z ); - double dist = sgdClosestPointToLineDistSquared( p, p0, - gs_base_vec ); + SGVec3d p = aircraft; + double dist = sgdClosestPointToLineDistSquared(p.sg(), gs_xyz.sg(), + gs_base_vec.sg()); gs_dist_node->setDoubleValue( sqrt( dist ) ); // cout << "gs_dist = " << gs_dist_node->getDoubleValue() // << endl; - Point3D tmp( gs_x, gs_y, gs_z ); - // cout << " (" << aircraft.distance3D( tmp ) << ")" << endl; - // wgs84 heading to glide slope (to determine sign of distance) - geo_inverse_wgs_84( elev, - lat * SGD_RADIANS_TO_DEGREES, - lon * SGD_RADIANS_TO_DEGREES, - gs_lat, gs_lon, + geo_inverse_wgs_84( pos, SGGeod::fromDeg(gs_lon, gs_lat), &az1, &az2, &s ); double r = az1 - target_radial; while ( r > 180.0 ) { r -= 360.0;} @@ -403,10 +392,7 @@ FGNavRadio::update(double dt) // compute forward and reverse wgs84 headings to localizer ////////////////////////////////////////////////////////// double hdg; - geo_inverse_wgs_84( elev, - lat * SGD_RADIANS_TO_DEGREES, - lon * SGD_RADIANS_TO_DEGREES, - loc_lat, loc_lon, + geo_inverse_wgs_84( pos, SGGeod::fromDeg(loc_lon, loc_lat), &hdg, &az2, &s ); // cout << "az1 = " << az1 << " magvar = " << nav_magvar << endl; heading_node->setDoubleValue( hdg ); @@ -444,10 +430,10 @@ FGNavRadio::update(double dt) while ( offset > 180.0 ) { offset -= 360.0; } // cout << "ils offset = " << offset << endl; effective_range - = adjustILSRange( nav_elev, elev, offset, - loc_dist * SG_METER_TO_NM ); + = adjustILSRange( nav_elev, pos.getElevationM(), offset, + loc_dist * SG_METER_TO_NM ); } else { - effective_range = adjustNavRange( nav_elev, elev, range ); + effective_range = adjustNavRange( nav_elev, pos.getElevationM(), range ); } // cout << "nav range = " << effective_range // << " (" << range << ")" << endl; @@ -809,9 +795,7 @@ void FGNavRadio::search() while ( target_radial > 360.0 ) { target_radial -= 360.0; } loc_lon = loc->get_lon(); loc_lat = loc->get_lat(); - nav_x = loc->get_x(); - nav_y = loc->get_y(); - nav_z = loc->get_z(); + nav_xyz = loc->get_cart(); last_nav_id = nav_id; last_nav_vor = false; loc_node->setBoolValue( true ); @@ -823,32 +807,26 @@ void FGNavRadio::search() nav_elev = gs->get_elev_ft(); int tmp = (int)(gs->get_multiuse() / 1000.0); target_gs = (double)tmp / 100.0; - gs_x = gs->get_x(); - gs_y = gs->get_y(); - gs_z = gs->get_z(); + gs_xyz = gs->get_cart(); // derive GS baseline (perpendicular to the runay // along the ground) double tlon, tlat, taz; geo_direct_wgs_84 ( 0.0, gs_lat, gs_lon, - target_radial + 90, + target_radial + 90, 100.0, &tlat, &tlon, &taz ); // cout << "target_radial = " << target_radial << endl; // cout << "nav_loc = " << loc_node->getBoolValue() << endl; // cout << gs_lon << "," << gs_lat << " " // << tlon << "," << tlat << " (" << nav_elev << ")" // << endl; - Point3D p1 = sgGeodToCart( Point3D(tlon*SGD_DEGREES_TO_RADIANS, - tlat*SGD_DEGREES_TO_RADIANS, - nav_elev*SG_FEET_TO_METER) - ); - // cout << gs_x << "," << gs_y << "," << gs_z - // << endl; + SGGeod tpos = SGGeod::fromDegFt(tlon, tlat, nav_elev); + SGVec3d p1 = SGVec3d::fromGeod(tpos); + + // cout << gs_xyz << endl; // cout << p1 << endl; - sgdSetVec3( gs_base_vec, - p1.x()-gs_x, p1.y()-gs_y, p1.z()-gs_z ); - // cout << gs_base_vec[0] << "," << gs_base_vec[1] << "," - // << gs_base_vec[2] << endl; + gs_base_vec = p1 - gs_xyz; + // cout << gs_base_vec << endl; } else { has_gs_node->setBoolValue( false ); nav_elev = loc->get_elev_ft(); @@ -905,9 +883,7 @@ void FGNavRadio::search() effective_range = adjustNavRange(nav_elev, elev, range); target_gs = 0.0; target_radial = sel_radial_node->getDoubleValue(); - nav_x = nav->get_x(); - nav_y = nav->get_y(); - nav_z = nav->get_z(); + nav_xyz = nav->get_cart(); if ( globals->get_soundmgr()->exists( nav_fx_name ) ) { globals->get_soundmgr()->remove( nav_fx_name ); diff --git a/src/Instrumentation/navradio.hxx b/src/Instrumentation/navradio.hxx index 6a07913ab..c6006a58d 100644 --- a/src/Instrumentation/navradio.hxx +++ b/src/Instrumentation/navradio.hxx @@ -123,16 +123,12 @@ class FGNavRadio : public SGSubsystem double target_radial; double loc_lon; double loc_lat; - double nav_x; - double nav_y; - double nav_z; + SGVec3d nav_xyz; double gs_lon; double gs_lat; double nav_elev; // use gs elev if available - double gs_x; - double gs_y; - double gs_z; - sgdVec3 gs_base_vec; + SGVec3d gs_xyz; + SGVec3d gs_base_vec; double gs_dist_signed; SGTimeStamp prev_time; SGTimeStamp curr_time; diff --git a/src/Instrumentation/tacan.cxx b/src/Instrumentation/tacan.cxx index 3c763e797..402b5a59d 100755 --- a/src/Instrumentation/tacan.cxx +++ b/src/Instrumentation/tacan.cxx @@ -51,10 +51,9 @@ TACAN::TACAN ( SGPropertyNode *node ) _time_before_search_sec(0), _mobile_valid(false), _transmitter_valid(false), - _transmitter_elevation_ft(0), + _transmitter_pos(SGGeod::fromDeg(0, 0)), _transmitter_range_nm(0), _transmitter_bias(0.0), - name("tacan"), num(0) { @@ -83,7 +82,7 @@ TACAN::TACAN () _time_before_search_sec(0), _mobile_valid(false), _transmitter_valid(false), - _transmitter_elevation_ft(0), + _transmitter_pos(SGGeod::fromDeg(0, 0)), _transmitter_range_nm(0), _transmitter_bearing_deg(0), _transmitter_bias(0.0), @@ -227,11 +226,8 @@ TACAN::update (double delta_time_sec) &mobile_bearing, &mobile_az2, &mobile_distance); //calculate the bearing and range of the station from the aircraft - geo_inverse_wgs_84(altitude_m, - latitude_deg, - longitude_deg, - _transmitter_lat, - _transmitter_lon, + SGGeod pos = SGGeod::fromDegM(longitude_deg, latitude_deg, altitude_m); + geo_inverse_wgs_84(pos, _transmitter_pos, &bearing, &az2, &distance); @@ -241,7 +237,7 @@ TACAN::update (double delta_time_sec) SG_LOG( SG_INSTR, SG_DEBUG, "distance_m " << distance); bearing = mobile_bearing; distance = mobile_distance; - _transmitter_elevation_ft = _mobile_elevation_ft; + _transmitter_pos.setElevationFt(_mobile_elevation_ft); _transmitter_range_nm = _mobile_range_nm; _transmitter_bias = _mobile_bias; _transmitter_name = _mobile_name; @@ -278,11 +274,7 @@ TACAN::update (double delta_time_sec) double rotation = 0; - /*Point3D location = - sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m)); - double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;*/ - - double range_nm = adjust_range(_transmitter_elevation_ft, + double range_nm = adjust_range(_transmitter_pos.getElevationFt(), altitude_m * SG_METER_TO_FEET, _transmitter_range_nm); @@ -489,9 +481,7 @@ TACAN::search (double frequency_mhz, double longitude_rad, if ( _transmitter_valid ) { SG_LOG( SG_INSTR, SG_DEBUG, "transmitter valid " << _transmitter_valid ); - _transmitter_lat = tacan->get_lat(); - _transmitter_lon = tacan->get_lon(); - _transmitter_elevation_ft = tacan->get_elev_ft(); + _transmitter_pos = tacan->get_pos(); _transmitter_range_nm = tacan->get_range(); _transmitter_bias = tacan->get_multiuse(); _transmitter_name = tacan->get_name(); @@ -500,8 +490,7 @@ TACAN::search (double frequency_mhz, double longitude_rad, _ident_node->setStringValue(_transmitter_ident.c_str()); SG_LOG( SG_INSTR, SG_DEBUG, "name " << _transmitter_name); - SG_LOG( SG_INSTR, SG_DEBUG, "lat " << _transmitter_lat << "lon " << _transmitter_lon); - SG_LOG( SG_INSTR, SG_DEBUG, "elev " << _transmitter_elevation_ft); + SG_LOG( SG_INSTR, SG_DEBUG, _transmitter_pos); } else { SG_LOG( SG_INSTR, SG_DEBUG, "transmitter invalid " << _transmitter_valid ); diff --git a/src/Instrumentation/tacan.hxx b/src/Instrumentation/tacan.hxx index d5c08532a..2056eb788 100755 --- a/src/Instrumentation/tacan.hxx +++ b/src/Instrumentation/tacan.hxx @@ -93,9 +93,8 @@ private: bool _mobile_valid; bool _transmitter_valid; - Point3D _transmitter; - double _transmitter_lat, _transmitter_lon; - double _transmitter_elevation_ft; + SGVec3d _transmitter; + SGGeod _transmitter_pos; double _transmitter_range_nm; double _transmitter_bearing_deg; double _transmitter_bias; diff --git a/src/Navaids/navlist.cxx b/src/Navaids/navlist.cxx index 650cd7a9f..c8832df86 100644 --- a/src/Navaids/navlist.cxx +++ b/src/Navaids/navlist.cxx @@ -157,8 +157,10 @@ bool FGTACANList::add( FGTACANRecord *c ) { 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,14 +170,14 @@ 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(); @@ -266,11 +268,10 @@ 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; @@ -278,11 +279,7 @@ FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft, // 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 ); + d2 = distSqr(stations[i]->get_cart(), aircraft); // cout << " dist = " << sqrt(d) // << " range = " << current->get_range() * SG_NM_TO_METER @@ -317,13 +314,8 @@ FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft, } 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(), + SGGeod geod = SGGeod::fromCart(aircraft); + geo_inverse_wgs_84( geod, stations[i]->get_pos(), &az1, &az2, &s); az1 = az1 - stations[i]->get_multiuse(); if ( az1 > 180.0) az1 -= 360.0; @@ -372,29 +364,23 @@ FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad, 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 + 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 @@ -418,7 +404,7 @@ FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad, // Given a TACAN Channel return the first matching frequency FGTACANRecord *FGTACANList::findByChannel( const string& channel ) { - tacan_list_type stations = ident_channels[channel]; + const tacan_list_type& stations = ident_channels[channel]; SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() ); if (stations.size()) { @@ -430,7 +416,7 @@ FGTACANRecord *FGTACANList::findByChannel( const string& channel ) // Given a frequency, return the first matching station. FGNavRecord *FGNavList::findStationByFreq( double freq ) { - nav_list_type stations = navaids[(int)(freq*100.0 + 0.5)]; + const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)]; SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size() ); diff --git a/src/Navaids/navlist.hxx b/src/Navaids/navlist.hxx index 63e0d8ea1..943f40f73 100644 --- a/src/Navaids/navlist.hxx +++ b/src/Navaids/navlist.hxx @@ -56,8 +56,6 @@ typedef map < string, tacan_list_type > tacan_ident_map_type; class FGNavList { - //nav_list_type navlist; // DCL - this doesn't appear to be used any more - // and can probably be removed. nav_list_type carrierlist; nav_map_type navaids; nav_map_type navaids_by_tile; @@ -65,7 +63,7 @@ class FGNavList { // Given a point and a list of stations, return the closest one to // the specified point. - FGNavRecord *findNavFromList( const Point3D &aircraft, + FGNavRecord *findNavFromList( const SGVec3d &aircraft, const nav_list_type &stations ); public: @@ -78,7 +76,6 @@ public: // add an entry bool add( FGNavRecord *n ); - //bool add( FGTACANRecord *r ); /** Query the database for the specified station. It is assumed * that there will be multiple stations with matching frequencies @@ -94,7 +91,7 @@ public: // (by ASCII code value) to that supplied. // Supplying true for exact forces only exact matches to be returned (similar to above function) // Returns an empty list if no match found - calling function should check for this! - nav_list_type findFirstByIdent( string ident, fg_nav_types type, bool exact = false ); + nav_list_type findFirstByIdent( const string& ident, fg_nav_types type, bool exact = false ); // Given an Ident and optional freqency, return the first matching // station. @@ -113,16 +110,9 @@ public: class FGTACANList { tacan_list_type channellist; - //nav_list_type carrierlist; tacan_map_type channels; - //nav_map_type navaids_by_tile; tacan_ident_map_type ident_channels; - // Given a point and a list of stations, return the closest one to - // the specified point. - /*FGNavRecord *findNavFromList( const Point3D &aircraft, - const nav_list_type &stations );*/ - public: FGTACANList(); @@ -132,7 +122,6 @@ public: bool init(); // add an entry - bool add( FGTACANRecord *r ); // Given a TACAN Channel, return the appropriate frequency. diff --git a/src/Navaids/navrecord.hxx b/src/Navaids/navrecord.hxx index 7be0169a2..90e719a8e 100644 --- a/src/Navaids/navrecord.hxx +++ b/src/Navaids/navrecord.hxx @@ -61,9 +61,8 @@ enum fg_nav_types { class FGNavRecord { int type; - double lon, lat; // location in geodetic coords (degrees) - double elev_ft; - double x, y, z; // location in cartesian coords (earth centered) + SGGeod pos; // location in geodetic coords (degrees) + SGVec3d cart; // location in cartesian coords (earth centered) int freq; int range; double multiuse; // can be slaved variation of VOR @@ -85,15 +84,14 @@ public: inline int get_type() const { return type; } inline fg_nav_types get_fg_type() const; - inline double get_lon() const { return lon; } // degrees - inline void set_lon( double l ) { lon = l; } // degrees - inline double get_lat() const { return lat; } // degrees - inline void set_lat( double l ) { lat = l; } // degrees - inline double get_elev_ft() const { return elev_ft; } - inline void set_elev_ft( double e ) { elev_ft = e; } - inline double get_x() const { return x; } - inline double get_y() const { return y; } - inline double get_z() const { return z; } + inline double get_lon() const { return pos.getLongitudeDeg(); } // degrees + inline void set_lon( double l ) { pos.setLongitudeDeg(l); } // degrees + inline double get_lat() const { return pos.getLatitudeDeg(); } // degrees + inline void set_lat( double l ) { pos.setLatitudeDeg(l); } // degrees + inline double get_elev_ft() const { return pos.getElevationFt(); } + inline void set_elev_ft( double e ) { pos.setElevationFt(e); } + const SGGeod& get_pos() const { return pos; } + const SGVec3d& get_cart() const { return cart; } inline int get_freq() const { return freq; } inline int get_range() const { return range; } inline double get_multiuse() const { return multiuse; } @@ -111,9 +109,8 @@ public: inline FGNavRecord::FGNavRecord(void) : type(0), - lon(0.0), lat(0.0), - elev_ft(0.0), - x(0.0), y(0.0), z(0.0), + pos(SGGeod::fromDeg(0, 0)), + cart(0, 0, 0), freq(0), range(0), multiuse(0.0), @@ -145,8 +142,12 @@ operator >> ( istream& in, FGNavRecord& n ) return in >> skipeol; } - in >> n.lat >> n.lon >> n.elev_ft >> n.freq >> n.range >> n.multiuse + double lat, lon, elev_ft; + in >> lat >> lon >> elev_ft >> n.freq >> n.range >> n.multiuse >> n.ident; + n.pos.setLatitudeDeg(lat); + n.pos.setLongitudeDeg(lon); + n.pos.setElevationFt(elev_ft); getline( in, n.name ); // silently multiply adf frequencies by 100 so that adf @@ -188,13 +189,7 @@ operator >> ( istream& in, FGNavRecord& n ) n.trans_ident = n.ident; // generate cartesian coordinates - Point3D geod( n.lon * SGD_DEGREES_TO_RADIANS, - n.lat * SGD_DEGREES_TO_RADIANS, - n.elev_ft * SG_FEET_TO_METER ); - Point3D cart = sgGeodToCart( geod ); - n.x = cart.x(); - n.y = cart.y(); - n.z = cart.z(); + n.cart = SGVec3d::fromGeod(n.pos); return in; }