Return the closest match, not just the first match. Sometimes there

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

diff --git a/src/Navaids/navlist.cxx b/src/Navaids/navlist.cxx
index 432d23c676d7d506e08653c82760d0a6b2cca8d0..b46b4b840e2e197ddab2ceb952d6cbb90d11a4dd 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.
 //
@@ -21,9 +21,13 @@
 // $Id$
 
 
+#ifdef HAVE_CONFIG_H
+#  include <config.h>
+#endif
+
 #include <simgear/debug/logstream.hxx>
-#include <simgear/misc/fgstream.hxx>
-#include <simgear/math/fg_geodesy.hxx>
+#include <simgear/misc/sgstream.hxx>
+#include <simgear/math/sg_geodesy.hxx>
 
 #include "navlist.hxx"
 
@@ -42,14 +46,13 @@ FGNavList::~FGNavList( void ) {
 
 
 // load the navaids and build the map
-bool FGNavList::init( FGPath path ) {
-    FGNav n;
+bool FGNavList::init( SGPath path ) {
 
     navaids.erase( navaids.begin(), navaids.end() );
 
-    fg_gzifstream in( path.str() );
+    sg_gzifstream in( path.str() );
     if ( !in.is_open() ) {
-        FG_LOG( FG_GENERAL, FG_ALERT, "Cannot open file: " << path.str() );
+        SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() );
         exit(-1);
     }
 
@@ -58,52 +61,49 @@ bool FGNavList::init( FGPath path ) {
     in >> skipeol;
     in >> skipcomment;
 
-#ifdef __MWERKS__
+    // double min = 100000;
+    // double max = 0;
 
+#ifdef __MWERKS__
     char c = 0;
-    while ( in.get(c) && c != '\0' && n.get_type() != '[' ) {
+    while ( in.get(c) && c != '\0'  ) {
         in.putback(c);
-        in >> n;
-       if ( n.get_type() != '[' ) {
-           navaids[n.get_freq()].push_back(n);
-       }
-        in >> skipcomment;
-    }
-
 #else
+    while ( ! in.eof() ) {
+#endif
 
-    double min = 100000;
-    double max = 0;
+        FGNav *n = new FGNav;
+        in >> (*n);
+        if ( n->get_type() == '[' ) {
+            break;
+        }
 
-    while ( ! in.eof() && n.get_type() != '[' ) {
-        in >> n;
        /* cout << "id = " << n.get_ident() << endl;
        cout << " type = " << n.get_type() << endl;
        cout << " lon = " << n.get_lon() << endl;
        cout << " lat = " << n.get_lat() << endl;
        cout << " elev = " << n.get_elev() << endl;
        cout << " freq = " << n.get_freq() << endl;
-       cout << " range = " << n.get_range() << endl; */
-       if ( n.get_type() != '[' ) {
-           navaids[n.get_freq()].push_back(n);
-       }
+       cout << " range = " << n.get_range() << endl << endl; */
+
+        navaids      [n->get_freq() ].push_back(n);
+        ident_navaids[n->get_ident()].push_back(n);
+               
         in >> skipcomment;
 
-       if ( n.get_type() != 'N' ) {
+       /* if ( n.get_type() != 'N' ) {
            if ( n.get_freq() < min ) {
                min = n.get_freq();
            }
            if ( n.get_freq() > max ) {
                max = n.get_freq();
            }
-       }
+       } */
     }
 
     // cout << "min freq = " << min << endl;
     // cout << "max freq = " << max << endl;
 
-#endif
-
     return true;
 }
 
@@ -118,25 +118,61 @@ bool FGNavList::query( double lon, double lat, double elev, double freq,
     nav_list_iterator current = stations.begin();
     nav_list_iterator last = stations.end();
 
+    Point3D aircraft = sgGeodToCart( Point3D(lon, lat, elev) );
+    return findNavFromList(aircraft, current, last, n);
+}
+
+
+bool FGNavList::findByIdent(const char* ident, double lon, double lat,
+                            FGNav *nav)
+{
+    nav_list_type stations = ident_navaids[ident];
+
+    nav_list_iterator current = stations.begin();
+    nav_list_iterator last = stations.end();
+       
+    Point3D aircraft = sgGeodToCart( Point3D(lon, lat, 0.0) );
+    return findNavFromList(aircraft, current, last, nav);
+}
+
+
+bool FGNavList::findNavFromList(const Point3D &aircraft, 
+                                nav_list_iterator current,
+                                nav_list_iterator end, FGNav *n)
+{
     // double az1, az2, s;
-    Point3D aircraft = fgGeodToCart( Point3D(lon, lat, elev) );
+    
     Point3D station;
-    double d;
-    for ( ; current != last ; ++current ) {
+    double d2;
+    double min_dist = 99999999999999.9;
+    bool found_one = false;
+    for ( ; current != end ; ++current ) {
        // cout << "testing " << current->get_ident() << endl;
-       station = Point3D(current->get_x(), current->get_y(), current->get_z());
+       station = Point3D((*current)->get_x(), (*current)->get_y(),
+                          (*current)->get_z());
 
-       d = aircraft.distance3Dsquared( station );
+       d2 = aircraft.distance3Dsquared( station );
 
        // cout << "  dist = " << sqrt(d)
-       //      << "  range = " << current->get_range() * NM_TO_METER << endl;
-       if ( d < (current->get_range() * NM_TO_METER 
-                 * current->get_range() * NM_TO_METER) ) {
-           // cout << "matched = " << current->get_ident() << endl;
-           *n = *current;
-           return true;
+       //      << "  range = " << current->get_range() * SG_NM_TO_METER
+        //      << endl;
+
+       // match d^2 < 2 * range^2 the published range so we can model
+       // reduced signal strength
+       double twiceRange = 2 * (*current)->get_range() * SG_NM_TO_METER;
+       if ( d2 < (twiceRange * twiceRange)) {
+            // cout << "d2 = " << d2 << " min_dist = " << min_dist << endl;
+            if ( d2 < min_dist ) {
+                min_dist = d2;
+                found_one = true;
+                *n = (**current);
+                // cout << "matched = " << (*current)->get_ident() << endl;
+            } else {
+                // cout << "matched, but too far away = "
+                //      << (*current)->get_ident() << endl;
+            }
        }
     }
 
-    return false;
+    return found_one;
 }