+
+
+// Given a localizer record and it's corresponding runway record,
+// adjust the localizer position so it is in perfect alignment with
+// the runway.
+static void update_loc_position( FGNavRecord *loc, FGRunway *rwy,
+ double threshold )
+{
+ double hdg = rwy->_heading;
+ hdg += 180.0;
+ if ( hdg > 360.0 ) {
+ hdg -= 360.0;
+ }
+
+ // calculate runway threshold point
+ double thresh_lat, thresh_lon, return_az;
+ geo_direct_wgs_84 ( 0.0, rwy->_lat, rwy->_lon, hdg,
+ rwy->_length/2.0 * SG_FEET_TO_METER,
+ &thresh_lat, &thresh_lon, &return_az );
+ // cout << "Threshold = " << thresh_lat << "," << thresh_lon << endl;
+
+ // calculate distance from threshold to localizer
+ double az1, az2, dist_m;
+ geo_inverse_wgs_84( 0.0, loc->get_lat(), loc->get_lon(),
+ thresh_lat, thresh_lon,
+ &az1, &az2, &dist_m );
+ // cout << "Distance = " << dist_m << endl;
+
+ // back project that distance along the runway center line
+ double nloc_lat, nloc_lon;
+ geo_direct_wgs_84 ( 0.0, thresh_lat, thresh_lon, hdg + 180.0,
+ dist_m, &nloc_lat, &nloc_lon, &return_az );
+ // printf("New localizer = %.6f %.6f\n", nloc_lat, nloc_lon );
+
+ // sanity check, how far have we moved the localizer?
+ geo_inverse_wgs_84( 0.0, loc->get_lat(), loc->get_lon(),
+ nloc_lat, nloc_lon,
+ &az1, &az2, &dist_m );
+ // cout << "Distance moved = " << dist_m << endl;
+
+ // cout << "orig heading = " << loc->get_multiuse() << endl;
+ // cout << "new heading = " << rwy->_heading << endl;
+
+ double hdg_diff = loc->get_multiuse() - rwy->_heading;
+
+ // clamp to [-180.0 ... 180.0]
+ if ( hdg_diff < -180.0 ) {
+ hdg_diff += 360.0;
+ } else if ( hdg_diff > 180.0 ) {
+ hdg_diff -= 360.0;
+ }
+
+ if ( fabs(hdg_diff) <= threshold ) {
+ loc->set_lat( nloc_lat );
+ loc->set_lon( nloc_lon );
+ loc->set_multiuse( rwy->_heading );
+ }
+}
+
+
+// This routines traverses the localizer list and attempts to match
+// each entry with it's corresponding runway. When it is successful,
+// it then "moves" the localizer and updates it's heading so it
+// *perfectly* aligns with the runway, but is still the same distance
+// from the runway threshold.
+void fgNavDBAlignLOCwithRunway( FGRunwayList *runways, FGNavList *loclist,
+ double threshold ) {
+ nav_map_type navmap = loclist->get_navaids();
+
+ nav_map_const_iterator freq = navmap.begin();
+ while ( freq != navmap.end() ) {
+ nav_list_type locs = freq->second;
+ nav_list_const_iterator loc = locs.begin();
+ while ( loc != locs.end() ) {
+ string name = (*loc)->get_name();
+ string::size_type pos1 = name.find(" ");
+ string id = name.substr(0, pos1);
+ name = name.substr(pos1+1);
+ string::size_type pos2 = name.find(" ");
+ string rwy = name.substr(0, pos2);
+
+ FGRunway r;
+ if ( runways->search(id, rwy, &r) ) {
+ update_loc_position( (*loc), &r, threshold );
+ }
+ ++loc;
+ }
+ ++freq;
+ }
+}
+