Frederic Bouvier:

[flightgear.git] / src / Navaids / navlist.cxx

diff --git a/src/Navaids/navlist.cxx b/src/Navaids/navlist.cxx
index 00566891363337b03b84f77e48703331e906165e..44c830166bf7937aebc0d31a52deb61fd87e8021 100644 (file)
--- a/src/Navaids/navlist.cxx
+++ b/src/Navaids/navlist.cxx
@@ -1,4 +1,4 @@
-// navaids.cxx -- navaids management class
+// navlist.cxx -- navaids management class
 //
 // Written by Curtis Olson, started April 2000.
 //
@@ -72,9 +72,9 @@ bool FGNavList::init( SGPath path ) {
     while ( ! in.eof() ) {
 #endif
 
-        FGNav n;
-        in >> n;
-        if ( n.get_type() == '[' ) {
+        FGNav *n = new FGNav;
+        in >> (*n);
+        if ( n->get_type() == '[' ) {
             break;
         }
 
@@ -86,7 +86,9 @@ bool FGNavList::init( SGPath path ) {
        cout << " freq = " << n.get_freq() << endl;
        cout << " range = " << n.get_range() << endl << endl; */
 
-        navaids[n.get_freq()].push_back(n);
+        navaids      [n->get_freq() ].push_back(n);
+        ident_navaids[n->get_ident()].push_back(n);
+               
         in >> skipcomment;
 
        /* if ( n.get_type() != 'N' ) {
@@ -106,38 +108,87 @@ bool FGNavList::init( SGPath path ) {
 }
 
 
-// query the database for the specified frequency, lon and lat are in
-// degrees, elev is in meters
-bool FGNavList::query( double lon, double lat, double elev, double freq,
-                      FGNav *n )
+// Query the database for the specified frequency.  It is assumed that
+// there will be multiple stations with matching frequencies so a
+// position must be specified.  Lon and lat are in degrees, elev is in
+// meters.
+FGNav *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) );
 
-    nav_list_iterator current = stations.begin();
-    nav_list_iterator last = stations.end();
+    return findNavFromList( aircraft, stations );
+}
 
-    // double az1, az2, s;
-    Point3D aircraft = sgGeodToCart( Point3D(lon, lat, elev) );
+
+FGNav *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) );
+
+    return findNavFromList( aircraft, stations );
+}
+
+
+// Given an Ident and optional freqency, return the first matching
+// station.
+FGNav *FGNavList::findByIdentAndFreq( const char* ident, const double freq )
+{
+    nav_list_type stations = ident_navaids[ident];
+
+    if ( freq > 0.0 ) {
+        // 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];
+            }
+        }
+    } else {
+        return stations[0];
+    }
+
+    return NULL;
+}
+
+
+// Given a point and a list of stations, return the closest one to the
+// specified point.
+FGNav *FGNavList::findNavFromList( const Point3D &aircraft, 
+                                   const nav_list_type &stations )
+{
+    FGNav *nav = NULL;
     Point3D station;
-    double d;
-    for ( ; current != last ; ++current ) {
+    double d2;
+    double min_dist = 999999999.0;
+
+    // prime the pump with info from stations[0]
+    if ( stations.size() > 0 ) {
+        nav = stations[0];
+       station = Point3D( nav->get_x(), nav->get_y(), nav->get_z());
+       min_dist = aircraft.distance3Dsquared( station );
+    }
+
+    // check if any of the remaining stations are closer
+    for ( unsigned int i = 1; i < stations.size(); ++i ) {
        // cout << "testing " << current->get_ident() << endl;
-       station = Point3D(current->get_x(), current->get_y(), current->get_z());
+       station = Point3D( stations[i]->get_x(),
+                           stations[i]->get_y(),
+                           stations[i]->get_z() );
 
-       d = aircraft.distance3Dsquared( station );
+       d2 = aircraft.distance3Dsquared( station );
 
        // cout << "  dist = " << sqrt(d)
-       //      << "  range = " << current->get_range() * SG_NM_TO_METER << endl;
-
-       // match up to twice the published range so we can model
-       // reduced signal strength
-       if ( d < (2 * current->get_range() * SG_NM_TO_METER 
-                 * 2 * current->get_range() * SG_NM_TO_METER ) ) {
-           // cout << "matched = " << current->get_ident() << endl;
-           *n = *current;
-           return true;
-       }
+       //      << "  range = " << current->get_range() * SG_NM_TO_METER
+        //      << endl;
+
+        if ( d2 < min_dist ) {
+            min_dist = d2;
+            nav = stations[i];
+        }
     }
 
-    return false;
+    return nav;
 }