Merge branch 'next' of git://gitorious.org/fg/flightgear into next

projects/VC90/FlightGear/FlightGear.vcproj

@@ -4203,6 +4203,46 @@
 				RelativePath="..\..\..\src\Navaids\positioned.hxx"
 				>
 			</File>
+			<File
+				RelativePath="..\..\..\src\Navaids\airways.hxx"
+				>
+			</File>
+			<File
+				RelativePath="..\..\..\src\Navaids\airways.cxx"
+				>
+			</File>
+			<File
+				RelativePath="..\..\..\src\Navaids\procedure.hxx"
+				>
+			</File>
+			<File
+				RelativePath="..\..\..\src\Navaids\procedure.cxx"
+				>
+			</File>
+			<File
+				RelativePath="..\..\..\src\Navaids\route.cxx"
+				>
+			</File>
+			<File
+				RelativePath="..\..\..\src\Navaids\route.hxx"
+				>
+			</File>
+			<File
+				RelativePath="..\..\..\src\Navaids\routePath.hxx"
+				>
+			</File>
+			<File
+				RelativePath="..\..\..\src\Navaids\routePath.cxx"
+				>
+			</File>
+			<File
+				RelativePath="..\..\..\src\Navaids\waypoint.cxx"
+				>
+			</File>
+			<File
+				RelativePath="..\..\..\src\Navaids\waypoint.hxx"
+				>
+			</File>
 		</Filter>
 		<Filter
 			Name="Lib_Network"
@@ -4855,6 +4895,14 @@
 				RelativePath="..\..\..\src\Instrumentation\gps.hxx"
 				>
 			</File>
+			<File
+				RelativePath="..\..\..\src\Instrumentation\rnav_waypt_controller.cxx"
+				>
+			</File>
+			<File
+				RelativePath="..\..\..\src\Instrumentation\rnav_waypt_controller.hxx"
+				>
+			</File>
 			<File
 				RelativePath="..\..\..\src\Instrumentation\groundradar.cxx"
 				>

src/AIModel/AIAircraft.cxx

@@ -29,6 +29,7 @@
 #include <Scenery/scenery.hxx>
 #include <Scenery/tilemgr.hxx>
 #include <Airports/dynamics.hxx>
+#include <Airports/simple.hxx>
 
 #include <string>
 #include <math.h>

src/AIModel/AIBase.cxx

@@ -194,17 +194,26 @@ void FGAIBase::Transform() {
 }
 
 bool FGAIBase::init(bool search_in_AI_path) {
-    osg::ref_ptr<osgDB::ReaderWriter::Options> opt=
-        new osgDB::ReaderWriter::Options(*osgDB::Registry::instance()->getOptions());
     
+    string f;
     if(search_in_AI_path)
     {
-        SGPath ai_path(globals->get_fg_root());
+    // setup a modified Options strucutre, with only the $fg-root/AI defined;
-        ai_path.append("AI");
+    // we'll check that first, then give the normal search logic a chance.
-        opt->getDatabasePathList().push_front(ai_path.str());
+    // this ensures that models in AI/ are preferred to normal models, where
+    // both exist.
+        osg::ref_ptr<osgDB::ReaderWriter::Options> 
+          opt(osg::clone(osgDB::Registry::instance()->getOptions(), osg::CopyOp::SHALLOW_COPY));
+
+        SGPath ai_path(globals->get_fg_root(), "AI");
+        opt->setDatabasePath(ai_path.str());
+        
+        f = osgDB::findDataFile(model_path, opt.get());
     }
 
-    string f = osgDB::findDataFile(model_path, opt.get());
+    if (f.empty()) {
+      f = simgear::SGModelLib::findDataFile(model_path);
+    }
     
     if(f.empty())
         f = fgGetString("/sim/multiplay/default-model", default_model);

src/AIModel/AIFlightPlan.hxx

@@ -24,26 +24,18 @@
 #include <string>
 
 
-#include <Airports/simple.hxx>
-#include <Navaids/awynet.hxx>
-
-#include "AIBase.hxx"
-
-
-
-using std::vector;
-using std::string;
-
 class FGTaxiRoute;
 class FGRunway;
 class FGAIAircraft;
+class FGAirport;
+class SGGeod;
 
 class FGAIFlightPlan {
 
 public:
 
   typedef struct {
-   string name;
+   std::string name;
    double latitude;
    double longitude;
    double altitude;
@@ -56,11 +48,11 @@ public:
     int routeIndex;  // For AI/ATC purposes;
    double time_sec;
    double trackLength; // distance from previous waypoint (for AI purposes);
-   string time;
+   std::string time;
 
   } waypoint;
   FGAIFlightPlan();
-  FGAIFlightPlan(const string& filename);
+  FGAIFlightPlan(const std::string& filename);
   FGAIFlightPlan(FGAIAircraft *,
                  const std::string& p,
 		 double course,
@@ -73,9 +65,9 @@ public:
                  double lat,
                  double lon,
                  double speed,
-		 const string& fltType,
+		 const std::string& fltType,
-		 const string& acType,
+		 const std::string& acType,
-		 const string& airline);
+		 const std::string& airline);
    ~FGAIFlightPlan();
 
    waypoint* const getPreviousWaypoint( void ) const;
@@ -91,18 +83,18 @@ public:
    double getLeadDistance( void ) const {return lead_distance;}
    double getBearing(waypoint* previous, waypoint* next) const;
    double getBearing(double lat, double lon, waypoint* next) const;
-   double checkTrackLength(string wptName);
+   double checkTrackLength(std::string wptName);
   time_t getStartTime() const { return start_time; }
    time_t getArrivalTime() const { return arrivalTime; }
 
   void    create(FGAIAircraft *, FGAirport *dep, FGAirport *arr, int leg, double alt, double speed, double lat, double lon,
-		 bool firstLeg, double radius, const string& fltType, const string& aircraftType, const string& airline, double distance);
+		 bool firstLeg, double radius, const std::string& fltType, const std::string& aircraftType, const std::string& airline, double distance);
 
   void setLeg(int val) { leg = val;}
   void setTime(time_t st) { start_time = st; }
   int getGate() const { return gateId; }
   double getLeadInAngle() const { return leadInAngle; }
-  const string& getRunway() const;
+  const std::string& getRunway() const;
   
   void setRepeat(bool r) { repeat = r; }
   bool getRepeat(void) const { return repeat; }
@@ -111,16 +103,16 @@ public:
   int getRouteIndex(int i); // returns the AI related index of this current routes. 
   FGTaxiRoute *getTaxiRoute() { return taxiRoute; }
   void deleteTaxiRoute();
-  string getRunway() { return activeRunway; }
+  std::string getRunway() { return activeRunway; }
   bool isActive(time_t time) {return time >= this->getStartTime();}
 
-  void setRunway(string rwy) { activeRunway = rwy; };
+  void setRunway(std::string rwy) { activeRunway = rwy; };
-  string getRunwayClassFromTrafficType(string fltType);
+  std::string getRunwayClassFromTrafficType(std::string fltType);
 
   void addWaypoint(waypoint* wpt) { waypoints.push_back(wpt); };
 
-  void setName(string n) { name = n; };
+  void setName(std::string n) { name = n; };
-  string getName() { return name; };
+  std::string getName() { return name; };
 
   void setSID(FGAIFlightPlan* fp) { sid = fp;};
   FGAIFlightPlan* getSID() { return sid; };
@@ -128,7 +120,7 @@ public:
 private:
   FGRunway* rwy;
   FGAIFlightPlan *sid;
-  typedef vector <waypoint*> wpt_vector_type;
+  typedef std::vector <waypoint*> wpt_vector_type;
   typedef wpt_vector_type::const_iterator wpt_vector_iterator;
 
 
@@ -143,26 +135,25 @@ private:
   time_t arrivalTime;       // For AI/ATC purposes.
   int leg;
   int gateId, lastNodeVisited;
-  string activeRunway;
+  std::string activeRunway;
-  FGAirRoute airRoute;
   FGTaxiRoute *taxiRoute;
-  string name;
+  std::string name;
 
-  void createPushBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const string&, const string&, const string&);
+  void createPushBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const std::string&, const std::string&, const std::string&);
-  void createPushBackFallBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const string&, const string&, const string&);
+  void createPushBackFallBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const std::string&, const std::string&, const std::string&);
-  void createTakeOff(FGAIAircraft *, bool, FGAirport *, double, const string&);
+  void createTakeOff(FGAIAircraft *, bool, FGAirport *, double, const std::string&);
-  void createClimb(FGAIAircraft *, bool, FGAirport *, double, double, const string&);
+  void createClimb(FGAIAircraft *, bool, FGAirport *, double, double, const std::string&);
-  void createCruise(FGAIAircraft *, bool, FGAirport*, FGAirport*, double, double, double, double, const string&);
+  void createCruise(FGAIAircraft *, bool, FGAirport*, FGAirport*, double, double, double, double, const std::string&);
-  void createDescent(FGAIAircraft *, FGAirport *,  double latitude, double longitude, double speed, double alt,const string&, double distance);
+  void createDescent(FGAIAircraft *, FGAirport *,  double latitude, double longitude, double speed, double alt,const std::string&, double distance);
-  void createLanding(FGAIAircraft *, FGAirport *, const string&);
+  void createLanding(FGAIAircraft *, FGAirport *, const std::string&);
   void createParking(FGAIAircraft *, FGAirport *, double radius);
   void deleteWaypoints(); 
   void resetWaypoints();
 
-  void createLandingTaxi(FGAIAircraft *, FGAirport *apt, double radius, const string& fltType, const string& acType, const string& airline);
+  void createLandingTaxi(FGAIAircraft *, FGAirport *apt, double radius, const std::string& fltType, const std::string& acType, const std::string& airline);
   void createDefaultLandingTaxi(FGAIAircraft *, FGAirport* aAirport);
   void createDefaultTakeoffTaxi(FGAIAircraft *, FGAirport* aAirport, FGRunway* aRunway);
-  void createTakeoffTaxi(FGAIAircraft *, bool firstFlight, FGAirport *apt, double radius, const string& fltType, const string& acType, const string& airline);
+  void createTakeoffTaxi(FGAIAircraft *, bool firstFlight, FGAirport *apt, double radius, const std::string& fltType, const std::string& acType, const std::string& airline);
 
   double getTurnRadius(double, bool);
         

src/AIModel/AIFlightPlanCreate.cxx

@@ -27,6 +27,7 @@
 #include <simgear/props/props.hxx>
 #include <simgear/props/props_io.hxx>
 
+#include <Airports/simple.hxx>
 #include <Airports/runways.hxx>
 #include <Airports/dynamics.hxx>
 #include "AIAircraft.hxx"

src/AIModel/AIFlightPlanCreateCruise.cxx

@@ -27,7 +27,7 @@
 #include <iostream>
 #include <simgear/route/waypoint.hxx>
 
-#include <Navaids/awynet.hxx>
+#include <Airports/simple.hxx>
 #include <Airports/runways.hxx>
 #include <Airports/dynamics.hxx>
 
@@ -40,6 +40,7 @@
 
 using std::iostream;
 
+/*
 void FGAIFlightPlan::evaluateRoutePart(double deplat,
 				       double deplon,
 				       double arrlat,
@@ -97,7 +98,7 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
 	}
 }
 
-
+*/
 /*
 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
 				  FGAirport *arr, double latitude,

src/AIModel/AIFlightPlanCreatePushBack.cxx

@@ -23,6 +23,8 @@
 #endif
 
 #include <simgear/math/sg_geodesy.hxx>
+
+#include <Airports/simple.hxx>
 #include <Airports/runways.hxx>
 #include <Airports/dynamics.hxx>
 

src/ATC/trafficcontrol.cxx

@@ -34,7 +34,7 @@
 #include <Traffic/TrafficMgr.hxx>
 #include <Airports/groundnetwork.hxx>
 #include <Airports/dynamics.hxx>
-
+#include <Airports/simple.hxx>
 
 using std::sort;
 

src/Airports/groundnetwork.cxx

@@ -30,6 +30,7 @@
 #include <simgear/debug/logstream.hxx>
 #include <simgear/route/waypoint.hxx>
 
+#include <Airports/simple.hxx>
 #include <Airports/dynamics.hxx>
 
 #include <AIModel/AIAircraft.hxx>

src/Airports/runways.cxx

@@ -37,6 +37,9 @@
 
 #include "runways.hxx"
 
+#include <Airports/simple.hxx>
+#include <Navaids/procedure.hxx>
+
 using std::string;
 
 static std::string cleanRunwayNo(const std::string& aRwyNo)
@@ -168,3 +171,30 @@ void FGRunway::setReciprocalRunway(FGRunway* other)
   _reciprocal = other;
 }
 
+std::vector<flightgear::SID*> FGRunway::getSIDs()
+{
+  std::vector<flightgear::SID*> result;
+  for (unsigned int i=0; i<_airport->numSIDs(); ++i) {
+    flightgear::SID* s = _airport->getSIDByIndex(i);
+    if (s->isForRunway(this)) {
+      result.push_back(s);
+    }
+  } // of SIDs at the airport iteration
+  
+  return result;
+}
+
+std::vector<flightgear::STAR*> FGRunway::getSTARs()
+{
+  std::vector<flightgear::STAR*> result;
+  for (unsigned int i=0; i<_airport->numSTARs(); ++i) {
+    flightgear::STAR* s = _airport->getSTARByIndex(i);
+    if (s->isForRunway(this)) {
+      result.push_back(s);
+    }
+  } // of STARs at the airport iteration
+  
+  return result;
+}
+
+

src/Airports/runways.hxx

@@ -33,6 +33,11 @@ class FGAirport;
 class FGNavRecord;
 class SGPropertyNode;
 
+namespace flightgear {
+  class SID;
+  class STAR;
+}
+
 class FGRunway : public FGRunwayBase
 {
   FGAirport* _airport;
@@ -115,6 +120,16 @@ public:
    * Helper to process property data loaded from an ICAO.threshold.xml file
    */
   void processThreshold(SGPropertyNode* aThreshold);
+  
+  /**
+   * Get SIDs (DPs) associated with this runway
+   */
+  std::vector<flightgear::SID*> getSIDs();
+  
+  /**
+   * Get STARs associared with this runway
+   */ 
+  std::vector<flightgear::STAR*> getSTARs();
 };
 
 #endif // _FG_RUNWAYS_HXX

src/Airports/sidstar.hxx

@@ -24,6 +24,7 @@
 #define _SIDSTAR_HXX_
 
 #include <string>
+#include <map>
 
 #include <simgear/misc/sg_path.hxx>
 
@@ -36,12 +37,10 @@
 #include "runwayprefs.hxx"
 
 
-using std::string;
-
 class FGAirport;
 
-typedef vector<FGAIFlightPlan*>           FlightPlanVec;
+typedef std::vector<FGAIFlightPlan*>           FlightPlanVec;
-typedef vector<FGAIFlightPlan*>::iterator FlightPlanVecIterator;
+typedef std::vector<FGAIFlightPlan*>::iterator FlightPlanVecIterator;
 
 typedef std::map < std::string, FlightPlanVec > FlightPlanVecMap;
 
@@ -49,7 +48,7 @@ typedef std::map < std::string, FlightPlanVec > FlightPlanVecMap;
 class FGSidStar 
 {
    private:
-      string id;
+      std::string id;
       bool initialized;
       FlightPlanVecMap data;
 
@@ -57,9 +56,9 @@ class FGSidStar
       FGSidStar(FGAirport *ap);
       FGSidStar(const FGSidStar &other);
 
-      string getId() { return id; };
+      std::string getId() { return id; };
       void load(SGPath path);
-      FGAIFlightPlan *getBest(string activeRunway, double heading);
+      FGAIFlightPlan *getBest(std::string activeRunway, double heading);
 };
 
 

src/Airports/simple.cxx

@@ -45,6 +45,11 @@
 #include <Airports/pavement.hxx>
 #include <Airports/dynamics.hxx>
 #include <Airports/xmlloader.hxx>
+#include <Navaids/procedure.hxx>
+#include <Navaids/waypoint.hxx>
+
+using std::vector;
+using namespace flightgear;
 
 // magic import of a helper which uses FGPositioned internals
 extern char** searchAirportNamesAndIdents(const std::string& aFilter);
@@ -187,6 +192,30 @@ FGRunway* FGAirport::findBestRunwayForHeading(double aHeading) const
   return result;
 }
 
+FGRunway* FGAirport::findBestRunwayForPos(const SGGeod& aPos) const
+{
+  loadRunways();
+  
+  Runway_iterator it = mRunways.begin();
+  FGRunway* result = NULL;
+  double currentLowestDev = 180.0;
+  
+  for (; it != mRunways.end(); ++it) {
+    double inboundCourse = SGGeodesy::courseDeg(aPos, (*it)->end());
+    double dev = inboundCourse - (*it)->headingDeg();
+    SG_NORMALIZE_RANGE(dev, -180.0, 180.0);
+
+    dev = fabs(dev);
+    if (dev < currentLowestDev) { // new best match
+      currentLowestDev = dev;
+      result = *it;
+    }
+  } // of runway iteration
+  
+  return result;
+
+}
+
 bool FGAirport::hasHardRunwayOfLengthFt(double aLengthFt) const
 {
   loadRunways();
@@ -350,6 +379,23 @@ void FGAirport::loadTaxiways() const
   }
 }
 
+void FGAirport::loadProcedures() const
+{
+  if (mProceduresLoaded) {
+    return;
+  }
+  
+  mProceduresLoaded = true;
+  SGPath path;
+  if (!XMLLoader::findAirportData(ident(), "procedures", path)) {
+    SG_LOG(SG_GENERAL, SG_INFO, "no procedures data available for " << ident());
+    return;
+  }
+  
+  SG_LOG(SG_GENERAL, SG_INFO, ident() << ": loading procedures from " << path.str());
+  Route::loadAirportProcedures(path, const_cast<FGAirport*>(this));
+}
+
 void FGAirport::loadSceneryDefintions() const
 {  
   // allow users to disable the scenery data in the short-term
@@ -412,6 +458,203 @@ void FGAirport::readTowerData(SGPropertyNode* aRoot)
   _tower_location = SGGeod::fromDegM(lon, lat, elevM);
 }
 
+bool FGAirport::buildApproach(Waypt* aEnroute, STAR* aSTAR, FGRunway* aRwy, WayptVec& aRoute)
+{
+  loadProcedures();
+
+  if ((aRwy && (aRwy->airport() != this))) {
+    throw sg_exception("invalid parameters", "FGAirport::buildApproach");
+  }
+  
+  if (aSTAR) {
+    bool ok = aSTAR->route(aRwy, aEnroute, aRoute);
+    if (!ok) {
+      SG_LOG(SG_GENERAL, SG_WARN, ident() << ": build approach, STAR " << aSTAR->ident() 
+         << " failed to route from transition " << aEnroute->ident());
+      return false;
+    }
+  } else if (aEnroute) {
+    // no a STAR specified, just use enroute point directly
+    aRoute.push_back(aEnroute);
+  }
+  
+  if (!aRwy) {
+    // no runway selected yet, but we loaded the STAR, so that's fine, we're done
+    return true;
+  }
+  
+// build the approach (possibly including transition), and including the missed segment
+  vector<Approach*> aps;
+  for (unsigned int j=0; j<mApproaches.size();++j) {
+    if (mApproaches[j]->runway() == aRwy) {
+      aps.push_back(mApproaches[j]);
+    }
+  } // of approach filter by runway
+  
+  if (aps.empty()) {
+    SG_LOG(SG_GENERAL, SG_INFO, ident() << "; no approaches defined for runway " << aRwy->ident());
+    // could build a fallback approach here
+    return false;
+  }
+  
+  for (unsigned int k=0; k<aps.size(); ++k) {
+    if (aps[k]->route(aRoute.back(), aRoute)) {
+      return true;
+    }
+  } // of initial approach iteration
+  
+  SG_LOG(SG_GENERAL, SG_INFO, ident() << ": unable to find transition to runway "
+    << aRwy->ident() << ", assume vectors");
+  
+  WayptRef v(new ATCVectors(NULL, this));
+  aRoute.push_back(v);
+  return aps.front()->routeFromVectors(aRoute);
+}
+
+pair<flightgear::SID*, WayptRef>
+FGAirport::selectSID(const SGGeod& aDest, FGRunway* aRwy)
+{
+  loadProcedures();
+  
+  WayptRef enroute;
+  flightgear::SID* sid = NULL;
+  double d = 1e9;
+  
+  for (unsigned int i=0; i<mSIDs.size(); ++i) {
+    if (aRwy && !mSIDs[i]->isForRunway(aRwy)) {
+      continue;
+    }
+  
+    WayptRef e = mSIDs[i]->findBestTransition(aDest);
+    if (!e) {
+      continue; // strange, but let's not worry about it
+    }
+    
+    // assert(e->isFixedPosition());
+    double ed = SGGeodesy::distanceM(aDest, e->position());
+    if (ed < d) { // new best match
+      enroute = e;
+      d = ed;
+      sid = mSIDs[i];
+    }
+  } // of SID iteration
+  
+  if (!mSIDs.empty() && !sid) {
+    SG_LOG(SG_GENERAL, SG_INFO, ident() << "selectSID, no SID found (runway=" 
+      << (aRwy ? aRwy->ident() : "no runway preference"));
+  }
+  
+  return make_pair(sid, enroute);
+}
+    
+pair<STAR*, WayptRef>
+FGAirport::selectSTAR(const SGGeod& aOrigin, FGRunway* aRwy)
+{
+  loadProcedures();
+  
+  WayptRef enroute;
+  STAR* star = NULL;
+  double d = 1e9;
+  
+  for (unsigned int i=0; i<mSTARs.size(); ++i) {
+    if (!mSTARs[i]->isForRunway(aRwy)) {
+      continue;
+    }
+    
+    SG_LOG(SG_GENERAL, SG_INFO, "STAR " << mSTARs[i]->ident() << " is valid for runway");
+    WayptRef e = mSTARs[i]->findBestTransition(aOrigin);
+    if (!e) {
+      continue; // strange, but let's not worry about it
+    }
+    
+    // assert(e->isFixedPosition());
+    double ed = SGGeodesy::distanceM(aOrigin, e->position());
+    if (ed < d) { // new best match
+      enroute = e;
+      d = ed;
+      star = mSTARs[i];
+    }
+  } // of STAR iteration
+  
+  return make_pair(star, enroute);
+}
+
+
+void FGAirport::addSID(flightgear::SID* aSid)
+{
+  mSIDs.push_back(aSid);
+}
+
+void FGAirport::addSTAR(STAR* aStar)
+{
+  mSTARs.push_back(aStar);
+}
+
+void FGAirport::addApproach(Approach* aApp)
+{
+  mApproaches.push_back(aApp);
+}
+
+unsigned int FGAirport::numSIDs() const
+{
+  loadProcedures();
+  return mSIDs.size();
+}
+
+flightgear::SID* FGAirport::getSIDByIndex(unsigned int aIndex) const
+{
+  loadProcedures();
+  return mSIDs[aIndex];
+}
+
+flightgear::SID* FGAirport::findSIDWithIdent(const std::string& aIdent) const
+{
+  loadProcedures();
+  for (unsigned int i=0; i<mSIDs.size(); ++i) {
+    if (mSIDs[i]->ident() == aIdent) {
+      return mSIDs[i];
+    }
+  }
+  
+  return NULL;
+}
+
+unsigned int FGAirport::numSTARs() const
+{
+  loadProcedures();
+  return mSTARs.size();
+}
+
+STAR* FGAirport::getSTARByIndex(unsigned int aIndex) const
+{
+  loadProcedures();
+  return mSTARs[aIndex];
+}
+
+STAR* FGAirport::findSTARWithIdent(const std::string& aIdent) const
+{
+  loadProcedures();
+  for (unsigned int i=0; i<mSTARs.size(); ++i) {
+    if (mSTARs[i]->ident() == aIdent) {
+      return mSTARs[i];
+    }
+  }
+  
+  return NULL;
+}
+
+unsigned int FGAirport::numApproaches() const
+{
+  loadProcedures();
+  return mApproaches.size();
+}
+
+Approach* FGAirport::getApproachByIndex(unsigned int aIndex) const
+{
+  loadProcedures();
+  return mApproaches[aIndex];
+}
+
 // get airport elevation
 double fgGetAirportElev( const string& id )
 {

src/Airports/simple.hxx

@@ -45,6 +45,19 @@ typedef SGSharedPtr<FGRunway> FGRunwayPtr;
 typedef SGSharedPtr<FGTaxiway> FGTaxiwayPtr;
 typedef SGSharedPtr<FGPavement> FGPavementPtr;
 
+namespace flightgear {
+  class SID;
+  class STAR;
+  class Approach;
+  class Waypt;
+
+
+  typedef SGSharedPtr<Waypt> WayptRef;
+  typedef std::vector<WayptRef> WayptVec;
+}
+
+
+
 /***************************************************************************************
  *
  **************************************************************************************/
@@ -85,6 +98,15 @@ public:
     FGRunway* getRunwayByIdent(const std::string& aIdent) const;
     FGRunway* findBestRunwayForHeading(double aHeading) const;
     
+    /**
+     * return the most likely target runway based on a position.
+     * Specifically, return the runway for which the course from aPos
+     * to the runway end, mostly closely matches the runway heading.
+     * This is a good approximation of which runway the position is on or
+     * aiming towards.
+     */
+    FGRunway* findBestRunwayForPos(const SGGeod& aPos) const;
+    
      /**
      * Useful predicate for FMS/GPS/NAV displays and similar - check if this
      * aiport has a hard-surfaced runway of at least the specified length.
@@ -143,6 +165,26 @@ public:
        double mMinLengthFt;
      };
      
+     
+     void setProcedures(const std::vector<flightgear::SID*>& aSids,
+      const std::vector<flightgear::STAR*>& aStars,
+      const std::vector<flightgear::Approach*>& aApproaches);
+     
+     void addSID(flightgear::SID* aSid);
+      void addSTAR(flightgear::STAR* aStar);
+      void addApproach(flightgear::Approach* aApp);
+
+      unsigned int numSIDs() const;
+      flightgear::SID* getSIDByIndex(unsigned int aIndex) const;
+      flightgear::SID* findSIDWithIdent(const std::string& aIdent) const;
+      
+      unsigned int numSTARs() const;
+      flightgear::STAR* getSTARByIndex(unsigned int aIndex) const;
+      flightgear::STAR* findSTARWithIdent(const std::string& aIdent) const;
+      
+      unsigned int numApproaches() const;
+      flightgear::Approach* getApproachByIndex(unsigned int aIndex) const;
+
      /**
       * Syntactic wrapper around FGPositioned::findClosest - find the closest
       * match for filter, and return it cast to FGAirport. The default filter
@@ -169,6 +211,23 @@ public:
       * matches in a format suitable for use by a puaList. 
       */
      static char** searchNamesAndIdents(const std::string& aFilter);
+     
+     bool buildApproach(flightgear::Waypt* aEnroute, flightgear::STAR* aSTAR, 
+      FGRunway* aRwy, flightgear::WayptVec& aRoute);
+    
+    /**
+     * Given a destiation point, select the best SID and transition waypt from 
+     * this airport. Returns (NULL,NULL) is no SIDs are defined, otherwise the
+     * best SID/transition is that which is closest to the destination point.
+     */
+    std::pair<flightgear::SID*, flightgear::WayptRef> selectSID(const SGGeod& aDest, FGRunway* aRwy);
+    
+    /**
+     * Select a STAR and enroute transition waypt, given an origin (departure) position.
+     * returns (NULL, NULL) is no suitable STAR is exists
+     */
+    std::pair<flightgear::STAR*, flightgear::WayptRef> selectSTAR(const SGGeod& aOrigin, FGRunway* aRwy);
+    
 private:
     typedef std::vector<FGRunwayPtr>::const_iterator Runway_iterator;
     /**
@@ -203,13 +262,19 @@ private:
 
     void loadRunways() const;
     void loadTaxiways() const;
+    void loadProcedures() const;
     
     mutable bool mRunwaysLoaded;
     mutable bool mTaxiwaysLoaded;
+    mutable bool mProceduresLoaded;
     
     std::vector<FGRunwayPtr> mRunways;
     std::vector<FGTaxiwayPtr> mTaxiways;
     std::vector<FGPavementPtr> mPavements;
+    
+    std::vector<flightgear::SID*> mSIDs;
+    std::vector<flightgear::STAR*> mSTARs;
+    std::vector<flightgear::Approach*> mApproaches;
 };
 
 // find basic airport location info from airport database

src/Autopilot/route_mgr.hxx

@@ -28,9 +28,11 @@
 #include <simgear/route/waypoint.hxx>
 #include <simgear/structure/subsystem_mgr.hxx>
 
+#include <Navaids/route.hxx>
+
 // forward decls
-class SGRoute;
 class SGPath;
+class PropertyWatcher;
 
 class FGAirport;
 typedef SGSharedPtr<FGAirport> FGAirportRef;
@@ -42,9 +44,91 @@ typedef SGSharedPtr<FGAirport> FGAirportRef;
 
 class FGRouteMgr : public SGSubsystem
 {
+public:
+  FGRouteMgr();
+  ~FGRouteMgr();
 
+  void init ();
+  void postinit ();
+  void bind ();
+  void unbind ();
+  void update (double dt);
+
+  void insertWayptAtIndex(flightgear::Waypt* aWpt, int aIndex);
+  flightgear::WayptRef removeWayptAtIndex(int index);
+  
+  void clearRoute();
+  
+  typedef enum {
+    ROUTE_HIGH_AIRWAYS, ///< high-level airways routing
+    ROUTE_LOW_AIRWAYS, ///< low-level airways routing
+    ROUTE_VOR ///< VOR-VOR routing
+  } RouteType;
+  
+  /**
+   * Insert waypoints from index-1 to index. In practice this means you can
+   * 'fill in the gaps' between defined waypoints. If index=0, the departure
+   * airport is used as index-1; if index is -1, the destination airport is
+   * used as the final waypoint.
+   */
+  bool routeToIndex(int index, RouteType aRouteType);
+
+  void autoRoute();
+        
+  bool isRouteActive() const;
+         
+  int currentIndex() const
+    { return _currentIndex; }
+    
+  flightgear::Waypt* currentWaypt() const;
+  flightgear::Waypt* nextWaypt() const;
+  flightgear::Waypt* previousWaypt() const;
+  
+  const flightgear::WayptVec& waypts() const
+    { return _route; }
+  
+  int numWaypts() const
+    { return _route.size(); }
+    
+  flightgear::Waypt* wayptAtIndex(int index) const;
+             
+  /**
+   * Find a waypoint in the route, by position, and return its index, or
+   * -1 if no matching waypoint was found in the route.
+   */
+  int findWayptIndex(const SGGeod& aPos) const;
+        
+  /**
+   * Activate a built route. This checks for various mandatory pieces of
+   * data, such as departure and destination airports, and creates waypoints
+   * for them on the route structure.
+   *
+   * returns true if the route was activated successfully, or false if the
+   * route could not be activated for some reason
+   */
+  bool activate();
+
+  /**
+   * Step to the next waypoint on the active route
+   */
+  void sequence();
+  
+  /**
+   * Set the current waypoint to the specified index.
+   */
+  void jumpToIndex(int index);
+  
+  void saveRoute();
+  void loadRoute();
+  
+  /**
+   * Helper command to setup current airport/runway if necessary
+   */
+  void initAtPosition();
 private:
-    SGRoute* _route;
+  flightgear::WayptVec _route;
+  int _currentIndex;
+  
     time_t _takeoffTime;
     time_t _touchdownTime;
     FGAirportRef _departure;
@@ -77,6 +161,8 @@ private:
     SGPropertyNode_ptr _pathNode;
     SGPropertyNode_ptr _currentWpt;
     
+    /// integer property corresponding to the RouteType enum
+    SGPropertyNode_ptr _routingType;
     
     /** 
      * Signal property to notify people that the route was edited
@@ -111,10 +197,17 @@ private:
      *  - airport-id/runway-id
      *  - navaid/radial-deg/offset-nm
      */
-    SGWayPoint* make_waypoint(const string& target);
+    flightgear::WayptRef waypointFromString(const std::string& target);
+    
+    
+    void departureChanged();
+    void buildDeparture(flightgear::WayptRef enroute, flightgear::WayptVec& wps);
+    
+    void arrivalChanged();
+    void buildArrival(flightgear::WayptRef enroute, flightgear::WayptVec& wps);
     
     /**
-     * Helper to keep various pieces of state in sync when the SGRoute is
+     * Helper to keep various pieces of state in sync when the route is
      * modified (waypoints added, inserted, removed). Notably, this fires the
      * 'edited' signal.
      */
@@ -138,6 +231,17 @@ private:
     
     void loadPlainTextRoute(const SGPath& path);
     
+    void loadVersion1XMLRoute(SGPropertyNode_ptr routeData);
+    void loadVersion2XMLRoute(SGPropertyNode_ptr routeData);
+    void loadXMLRouteHeader(SGPropertyNode_ptr routeData);
+    flightgear::WayptRef parseVersion1XMLWaypt(SGPropertyNode* aWP);
+    
+    /**
+     * Predicate for helping the UI - test if at least one waypoint was
+     * entered by the user (as opposed to being generated by the route-manager)
+     */
+    bool haveUserWaypoints() const;
+    
 // tied getters and setters
     const char* getDepartureICAO() const;
     const char* getDepartureName() const;
@@ -147,63 +251,8 @@ private:
     const char* getDestinationName() const;
     void setDestinationICAO(const char* aIdent);
 
-public:
+    PropertyWatcher* _departureWatcher;
-
+    PropertyWatcher* _arrivalWatcher;
-    FGRouteMgr();
-    ~FGRouteMgr();
-
-    void init ();
-    void postinit ();
-    void bind ();
-    void unbind ();
-    void update (double dt);
-
-    bool build ();
-
-    void new_waypoint( const string& tgt_alt, int n = -1 );
-    void add_waypoint( const SGWayPoint& wp, int n = -1 );
-    SGWayPoint pop_waypoint( int i = 0 );
-
-    SGWayPoint get_waypoint( int i ) const;
-    int size() const;
-        
-    bool isRouteActive() const;
-        
-    int currentWaypoint() const;
-       
-    /**
-     * Find a waypoint in the route, by position, and return its index, or
-     * -1 if no matching waypoint was found in the route.
-     */
-    int findWaypoint(const SGGeod& aPos) const;
-          
-    /**
-     * Activate a built route. This checks for various mandatory pieces of
-     * data, such as departure and destination airports, and creates waypoints
-     * for them on the route structure.
-     *
-     * returns true if the route was activated successfully, or false if the
-     * route could not be activated for some reason
-     */
-    bool activate();
-
-    /**
-     * Step to the next waypoint on the active route
-     */
-    void sequence();
-    
-    /**
-     *
-     */
-    void jumpToIndex(int index);
-    
-    /**
-     * 
-     */
-    void setWaypointTargetAltitudeFt(unsigned int index, int altFt);
-    
-    void saveRoute();
-    void loadRoute();
 };
 
 

src/GUI/MapWidget.cxx

@@ -25,6 +25,7 @@
 #include <Airports/simple.hxx>
 #include <Airports/runways.hxx>
 #include <Main/fg_os.hxx>      // fgGetKeyModifiers()
+#include <Navaids/routePath.hxx>
 
 const char* RULER_LEGEND_KEY = "ruler-legend";
   
@@ -657,77 +658,73 @@ void MapWidget::paintAircraftLocation(const SGGeod& aircraftPos)
 
 void MapWidget::paintRoute()
 {
-  if (_route->size() < 2) {
+  if (_route->numWaypts() < 2) {
     return;
   }
   
-// first pass, draw the actual line
+  RoutePath path(_route->waypts());
+  
+// first pass, draw the actual lines
   glLineWidth(2.0);
-  glBegin(GL_LINE_STRIP);
   
-  SGVec2d prev = project(_route->get_waypoint(0).get_target());
+  for (int w=0; w<_route->numWaypts(); ++w) {
-  glVertex2d(prev.x(), prev.y());
+    SGGeodVec gv(path.pathForIndex(w));
+    if (gv.empty()) {
+      continue;
+    }
     
-  for (int w=1; w < _route->size(); ++w) {
+    if (w < _route->currentIndex()) {
-    
-    SGVec2d p = project(_route->get_waypoint(w).get_target());
-    
-    if (w < _route->currentWaypoint()) {
       glColor4f(0.5, 0.5, 0.5, 0.7);
     } else {
       glColor4f(1.0, 0.0, 1.0, 1.0);
     }
     
-    glVertex2d(p.x(), p.y());
+    flightgear::WayptRef wpt(_route->wayptAtIndex(w));
+    if (wpt->flag(flightgear::WPT_MISS)) {
+      glEnable(GL_LINE_STIPPLE);
+      glLineStipple(1, 0x00FF);
+    }
     
+    glBegin(GL_LINE_STRIP);
+    for (unsigned int i=0; i<gv.size(); ++i) {
+      SGVec2d p = project(gv[i]);
+      glVertex2d(p.x(), p.y());
+    }
+    
+    glEnd();
+    glDisable(GL_LINE_STIPPLE);
   }
-  glEnd();
   
   glLineWidth(1.0);
 // second pass, draw waypoint symbols and data
-  for (int w=0; w < _route->size(); ++w) {
+  for (int w=0; w < _route->numWaypts(); ++w) {
-    const SGWayPoint& wpt(_route->get_waypoint(w));
+    flightgear::WayptRef wpt(_route->wayptAtIndex(w));
-    SGVec2d p = project(wpt.get_target());
+    SGGeod g = path.positionForIndex(w);
+    if (g == SGGeod()) {
+      continue; // Vectors or similar
+    }
+    
+    SGVec2d p = project(g);
     glColor4f(1.0, 0.0, 1.0, 1.0);
     circleAtAlt(p, 8, 12, 5);
     
     std::ostringstream legend;
-    legend << wpt.get_id();
+    legend << wpt->ident();
-    if (wpt.get_target_alt() > -9990.0) {
+    if (wpt->altitudeRestriction() != flightgear::RESTRICT_NONE) {
-      legend << '\n' << SGMiscd::roundToInt(wpt.get_target_alt()) << '\'';
+      legend << '\n' << SGMiscd::roundToInt(wpt->altitudeFt()) << '\'';
     }
     
-    if (wpt.get_speed() > 0.0) {
+    if (wpt->speedRestriction() != flightgear::RESTRICT_NONE) {
-      legend << '\n' << SGMiscd::roundToInt(wpt.get_speed()) << "Kts";
+      legend << '\n' << SGMiscd::roundToInt(wpt->speedKts()) << "Kts";
     }
 	
     MapData* d = getOrCreateDataForKey(reinterpret_cast<void*>(w * 2));
     d->setText(legend.str());
-    d->setLabel(wpt.get_id());
+    d->setLabel(wpt->ident());
     d->setAnchor(p);
     d->setOffset(MapData::VALIGN_TOP | MapData::HALIGN_CENTER, 15);
-    d->setPriority(w < _route->currentWaypoint() ? 9000 : 12000);
+    d->setPriority(w < _route->currentIndex() ? 9000 : 12000);
         
-    if (w > 0) {
-      SGVec2d legMid = (prev + p) * 0.5;
-      std::ostringstream legLegend;
-      
-      double track = wpt.get_track();
-      if (_magneticHeadings) {
-        track -= _magVar->get_magvar(); // show magnetic track for leg
-      }
-      
-      legLegend << SGMiscd::roundToInt(track) << " " 
-        << SGMiscd::roundToInt(wpt.get_distance() * SG_METER_TO_NM) << "Nm";
-        
-      MapData* ld = getOrCreateDataForKey(reinterpret_cast<void*>(w * 2 + 1));
-      ld->setText(legLegend.str());
-      ld->setAnchor(legMid);
-      ld->setOffset(MapData::VALIGN_TOP | MapData::HALIGN_CENTER, 15);
-      ld->setPriority(w < _route->currentWaypoint() ? 8000 : 11000);
-    } // of draw leg data
-    
-    prev = p;
   } // of second waypoint iteration
 }
 

src/GUI/WaypointList.cxx

@@ -7,6 +7,8 @@
 #include "WaypointList.hxx"
 
 #include <algorithm>
+#include <boost/tuple/tuple.hpp>
+
 #include <plib/puAux.h>
 
 #include <simgear/route/waypoint.hxx>
@@ -16,14 +18,18 @@
 #include <Main/globals.hxx>
 #include <Main/fg_props.hxx>
 
+#include <Navaids/positioned.hxx>
 #include <Autopilot/route_mgr.hxx>
 
+using namespace flightgear;
+
 enum {
   SCROLL_NO = 0,
   SCROLL_UP,
   SCROLL_DOWN
 };
   
+static const double BLINK_TIME = 0.3;
 static const int DRAG_START_DISTANCE_PX = 5;
   
 class RouteManagerWaypointModel : 
@@ -48,37 +54,38 @@ public:
 // implement WaypointList::Model
   virtual unsigned int numWaypoints() const
   {
-    return _rm->size();
+    return _rm->numWaypts();
   }
   
   virtual int currentWaypoint() const
   {
-    return _rm->currentWaypoint();
+    return _rm->currentIndex();
   }
   
-  virtual SGWayPoint waypointAt(unsigned int index) const
+  virtual flightgear::Waypt* waypointAt(unsigned int index) const
   {
-    return _rm->get_waypoint(index);
+    if (index >= numWaypoints()) {
+      return NULL;
+    }
+    
+    return _rm->wayptAtIndex(index);
   }
 
   virtual void deleteAt(unsigned int index)
   {
-    _rm->pop_waypoint(index);
+    _rm->removeWayptAtIndex(index);
-  }
-  
-  virtual void setWaypointTargetAltitudeFt(unsigned int index, int altFt)
-  {
-    _rm->setWaypointTargetAltitudeFt(index, altFt);
   }
   
   virtual void moveWaypointToIndex(unsigned int srcIndex, unsigned int destIndex)
   {
+    SG_LOG(SG_GENERAL, SG_INFO, "moveWaypoint: from " << srcIndex << " to " << destIndex);
     if (destIndex > srcIndex) {
       --destIndex;
     }
     
-    SGWayPoint wp = _rm->pop_waypoint(srcIndex);
+    WayptRef w(_rm->removeWayptAtIndex(srcIndex));
-    _rm->add_waypoint(wp, destIndex);
+    SG_LOG(SG_GENERAL, SG_INFO, "wpt:" << w->ident());
+    _rm->insertWayptAtIndex(w, destIndex);
   }
   
   virtual void setUpdateCallback(SGCallback* cb)
@@ -129,13 +136,16 @@ WaypointList::WaypointList(int x, int y, int width, int height) :
   _showLatLon(false),
   _model(NULL),
   _updateCallback(NULL),
-  _scrollCallback(NULL)
+  _scrollCallback(NULL),
+  _blink(false)
 {
   // pretend to be a list, so fgPopup doesn't mess with our mouse events
   type |= PUCLASS_LIST;  
   setModel(new RouteManagerWaypointModel());
   setSize(width, height);
   setValue(-1);
+  
+  _blinkTimer.stamp();
 }
 
 WaypointList::~WaypointList()
@@ -230,6 +240,11 @@ void WaypointList::handleDrag(int x, int y)
     }
     
     _dragSourceRow = rowForY(y - abox.min[1]);
+    Waypt* wp = _model->waypointAt(_dragSourceRow);
+    if (!wp || wp->flag(WPT_GENERATED)) {
+      return; // don't allow generated points to be dragged
+    }
+    
     _dragging = true;
     _dragScroll = SCROLL_NO;
   }
@@ -255,20 +270,26 @@ void WaypointList::doDrop(int x, int y)
   _dragging = false;
   puDeactivateWidget();
   
+  SG_LOG(SG_GENERAL, SG_INFO, "doDrop");
+  
   if ((y < abox.min[1]) || (y >= abox.max[1])) {
+    SG_LOG(SG_GENERAL, SG_INFO, "y out of bounds:" << y);
     return;
   }
   
-  if (_dragSourceRow != _dragTargetRow) {
+  if (_dragSourceRow == _dragTargetRow) {
-    _model->moveWaypointToIndex(_dragSourceRow, _dragTargetRow);
+    SG_LOG(SG_GENERAL, SG_INFO, "source and target row match");
-    
+    return;
-    // keep row indexes linged up when moving an item down the list
-    if (_dragSourceRow < _dragTargetRow) {
-      --_dragTargetRow;
-    }
-    
-    setSelected(_dragTargetRow);
   }
+  
+  _model->moveWaypointToIndex(_dragSourceRow, _dragTargetRow);
+  
+  // keep row indexes linged up when moving an item down the list
+  if (_dragSourceRow < _dragTargetRow) {
+    --_dragTargetRow;
+  }
+  
+  setSelected(_dragTargetRow);
 }
 
 void WaypointList::invokeDownCallback(void)
@@ -302,6 +323,12 @@ void WaypointList::draw( int dx, int dy )
     doDragScroll();
   }
   
+  double dt = (SGTimeStamp::now() - _blinkTimer).toSecs();
+  if (dt > BLINK_TIME) {
+    _blinkTimer.stamp();
+    _blink = !_blink;
+  }
+  
   glEnable(GL_SCISSOR_TEST);
   GLint sx = (int) abox.min[0],
     sy = abox.min[1];
@@ -321,6 +348,7 @@ void WaypointList::draw( int dx, int dy )
   
   y -= (_scrollPx % rowHeight); // partially draw the first row
   
+  _arrowWidth = legendFont.getStringWidth(">");
   for ( ; row <= final; ++row, y += rowHeight) {
     drawRow(dx, dy, row, y);
   } // of row drawing iteration
@@ -343,6 +371,8 @@ void WaypointList::draw( int dx, int dy )
 
 void WaypointList::drawRow(int dx, int dy, int rowIndex, int y)
 {
+  flightgear::Waypt* wp(_model->waypointAt(rowIndex));
+    
   bool isSelected = (rowIndex == getSelected());
   bool isCurrent = (rowIndex == _model->currentWaypoint());
   bool isDragSource = (_dragging && (rowIndex == _dragSourceRow));
@@ -351,62 +381,100 @@ void WaypointList::drawRow(int dx, int dy, int rowIndex, int y)
   bkgBox.min[1] = abox.max[1] - y;
   bkgBox.max[1] = bkgBox.min[1] + rowHeightPx();
   
-  puColour currentColor;
+  puColour col;
-  puSetColor(currentColor, 1.0, 1.0, 0.0, 0.5);
+  puFont* f = &legendFont;
+  bool drawBox = false;
+  
+  if (wp->flag(WPT_MISS)) {
+    drawBox = true;
+    puSetColor(col, 1.0, 0.0, 0.0, 0.3);  // red
+  } else if (wp->flag(WPT_ARRIVAL)) {
+    drawBox = true;
+    puSetColor(col, 0.0, 0.0, 0.0, 0.3);
+  } else if (wp->flag(WPT_DEPARTURE)) {
+    drawBox = true;
+    puSetColor(col, 0.0, 0.0, 0.0, 0.3);
+  }
   
   if (isDragSource) {
     // draw later, on *top* of text string
-  } else  if (isCurrent) {
-    bkgBox.draw(dx, dy, PUSTYLE_PLAIN, &currentColor, false, 0);
   } else if (isSelected) { // -PLAIN means selected, apparently
     bkgBox.draw(dx, dy, -PUSTYLE_PLAIN, colour, false, 0);
+  } else if (drawBox) {
+    bkgBox.draw(dx, dy, PUSTYLE_PLAIN, &col, false, 0);
+  }
+  
+  if (isCurrent) {
+    glColor4f (1.0, 0.5, 0.0, 1.0) ;
+  } else {
+    glColor4fv ( colour [ PUCOL_LEGEND ] ) ;
   }
   
   int xx = dx + abox.min[0] + PUSTR_LGAP;
   int yy = dy + abox.max[1] - y ;
   yy += 4; // center text in row height
   
-  // row textual data
+  if (isCurrent) {
-  const SGWayPoint wp(_model->waypointAt(rowIndex));
+    f->drawString(">", xx, yy);
-  char buffer[128];
-  int count = ::snprintf(buffer, 128, "%03d   %-5s", rowIndex, wp.get_id().c_str());
-  
-  if (wp.get_name().size() > 0 && (wp.get_name() != wp.get_id())) { 
-    // append name if present, and different to id
-    ::snprintf(buffer + count, 128 - count, " (%s)", wp.get_name().c_str());
   }
   
-  glColor4fv ( colour [ PUCOL_LEGEND ] ) ;
+  int x = xx;
-  drawClippedString(legendFont, buffer, xx, yy, 300);
+  x += _arrowWidth + PUSTR_LGAP;
+  
+  // row textual data
+
+  char buffer[128];
+  int count = ::snprintf(buffer, 128, "%03d   %-5s", rowIndex, wp->ident().c_str());
+ 
+  FGPositioned* src = wp->source(); 
+  if (src && !src->name().empty() && (src->name() != wp->ident())) { 
+    // append name if present, and different to id
+    ::snprintf(buffer + count, 128 - count, " (%s)", src->name().c_str());
+  }
+
+  drawClippedString(legendFont, buffer, x, yy, 300);
+  x += 300 + PUSTR_LGAP;
   
   if (_showLatLon) {
-    char ns = (wp.get_target_lat() > 0.0) ? 'N' : 'S';
+    SGGeod p(wp->position());
-    char ew = (wp.get_target_lon() > 0.0) ? 'E' : 'W';
+    char ns = (p.getLatitudeDeg() > 0.0) ? 'N' : 'S';
+    char ew = (p.getLongitudeDeg() > 0.0) ? 'E' : 'W';
     
     ::snprintf(buffer, 128 - count, "%4.2f%c %4.2f%c",
-      fabs(wp.get_target_lon()), ew, fabs(wp.get_target_lat()), ns);
+      fabs(p.getLongitudeDeg()), ew, fabs(p.getLatitudeDeg()), ns);
-  } else {
+  } else if (rowIndex > 0) {
+    double courseDeg;
+    double distanceM;
+    Waypt* prev = _model->waypointAt(rowIndex - 1);
+    boost::tie(courseDeg, distanceM) = wp->courseAndDistanceFrom(prev->position());
+  
     ::snprintf(buffer, 128 - count, "%03.0f %5.1fnm",
-      wp.get_track(), wp.get_distance() * SG_METER_TO_NM);
+      courseDeg, distanceM * SG_METER_TO_NM);
   }
 
-  legendFont.drawString(buffer, xx + 300 + PUSTR_LGAP, yy);
+  f->drawString(buffer, x, yy);
+  x += 100 + PUSTR_LGAP;
   
-  int altFt = (int) wp.get_target_alt() * SG_METER_TO_FEET;
+  if (wp->altitudeRestriction() != RESTRICT_NONE) {
-  if (altFt > -9990) {
+    int altHundredFt = (wp->altitudeFt() + 50) / 100; // round to nearest 100ft
-    int altHundredFt = (altFt + 50) / 100; // round to nearest 100ft
     if (altHundredFt < 100) {
       count = ::snprintf(buffer, 128, "%d'", altHundredFt * 100);
     } else { // display as a flight-level
       count = ::snprintf(buffer, 128, "FL%d", altHundredFt);
     }
     
-    legendFont.drawString(buffer, xx + 400 + PUSTR_LGAP, yy);
+    f->drawString(buffer, x, yy);
   } // of valid wp altitude
+  x += 60 + PUSTR_LGAP;
+  
+  if (wp->speedRestriction() != RESTRICT_NONE) {
+    count = ::snprintf(buffer, 126, "%dKts", (int) wp->speedKts());
+    f->drawString(buffer, x, yy);
+  }
   
   if (isDragSource) {
-    puSetColor(currentColor, 1.0, 0.5, 0.0, 0.5);
+    puSetColor(col, 1.0, 0.5, 0.0, 0.5);
-    bkgBox.draw(dx, dy, PUSTYLE_PLAIN, &currentColor, false, 0);
+    bkgBox.draw(dx, dy, PUSTYLE_PLAIN, &col, false, 0);
   }
 }
 
@@ -613,22 +681,34 @@ int WaypointList::checkKey (int key, int updown )
   
   case '-':
     if (getSelected() >= 0) {
-      int newAlt = wayptAltFtHundreds(getSelected()) - 10;
+      Waypt* wp = _model->waypointAt(getSelected());
-      if (newAlt < 0) {
+      if (wp->flag(WPT_GENERATED)) {
-        _model->setWaypointTargetAltitudeFt(getSelected(), -9999);
+        break;
-      } else {
+      }
-        _model->setWaypointTargetAltitudeFt(getSelected(), newAlt * 100);
+      
+      if (wp->altitudeRestriction() != RESTRICT_NONE) {
+        int curAlt = (static_cast<int>(wp->altitudeFt()) + 50) / 100;
+        if (curAlt <= 0) {
+          wp->setAltitude(0, RESTRICT_NONE);
+        } else {
+          wp->setAltitude((curAlt - 10) * 100, wp->altitudeRestriction());
+        }
       }
     }
     break;
     
   case '=':
     if (getSelected() >= 0) {
-      int newAlt = wayptAltFtHundreds(getSelected()) + 10;
+      flightgear::Waypt* wp = _model->waypointAt(getSelected());
-      if (newAlt < 0) {
+      if (wp->flag(WPT_GENERATED)) {
-        _model->setWaypointTargetAltitudeFt(getSelected(), 0);
+        break;
+      }
+        
+      if (wp->altitudeRestriction() == RESTRICT_NONE) {
+        wp->setAltitude(1000, RESTRICT_AT);
       } else {
-        _model->setWaypointTargetAltitudeFt(getSelected(), newAlt * 100);
+        int curAlt = (static_cast<int>(wp->altitudeFt()) + 50) / 100;
+        wp->setAltitude((curAlt + 10) * 100, wp->altitudeRestriction());
       }
     }
     break;
@@ -636,6 +716,11 @@ int WaypointList::checkKey (int key, int updown )
   case 0x7f: // delete
     if (getSelected() >= 0) {
       int index = getSelected();
+      Waypt* wp = _model->waypointAt(getSelected());
+      if (wp->flag(WPT_GENERATED)) {
+        break;
+      }
+      
       _model->deleteAt(index);
       setSelected(index - 1);
     }
@@ -648,17 +733,6 @@ int WaypointList::checkKey (int key, int updown )
   return TRUE ;
 }
 
-int WaypointList::wayptAltFtHundreds(int index) const
-{
-  double alt = _model->waypointAt(index).get_target_alt();
-  if (alt < -9990.0) {
-    return -9999;
-  }
-  
-  int altFt = (int) alt * SG_METER_TO_FEET;
-  return (altFt + 50) / 100; // round to nearest 100ft
-}
-
 void WaypointList::modelUpdateCallback()
 {
   // local stuff

src/GUI/WaypointList.hxx

@@ -14,9 +14,12 @@
 
 // forward decls
 class puaScrollBar;
-class SGWayPoint;
 class SGCallback;
 
+namespace flightgear {
+  class Waypt;
+}
+
 class WaypointList : public puFrame, public GUI_ID
 {
 public:
@@ -71,14 +74,13 @@ public:
     
     virtual unsigned int numWaypoints() const = 0;
     virtual int currentWaypoint() const = 0;
-    virtual SGWayPoint waypointAt(unsigned int index) const = 0;
+    virtual flightgear::Waypt* waypointAt(unsigned int index) const = 0;
   
   // update notifications
     virtual void setUpdateCallback(SGCallback* cb) = 0;
   
   // editing operations
     virtual void deleteAt(unsigned int index) = 0;
-    virtual void setWaypointTargetAltitudeFt(unsigned int index, int altFt) = 0;
     virtual void moveWaypointToIndex(unsigned int srcIndex, unsigned int dstIndex) = 0;
   };
   
@@ -122,8 +124,6 @@ private:
   int firstFullyVisibleRow() const;
   int lastFullyVisibleRow() const;
     
-  int wayptAltFtHundreds(int index) const;
-  
   void modelUpdateCallback();
   
   int _scrollPx; // scroll ammount (in pixels)
@@ -141,6 +141,10 @@ private:
   Model* _model;
   SGCallback* _updateCallback;
   SGCallback* _scrollCallback;
+ 
+  SGTimeStamp _blinkTimer;
+  bool _blink;
+  int _arrowWidth;
 };
 
 class ScrolledWaypointList : public puGroup

src/GUI/dialog.cxx

@@ -1339,8 +1339,41 @@ fgList::update()
 
 void fgComboBox::update()
 {
+  if (_inHit) {
+    return;
+  }
+  
+  std::string curValue(getStringValue());
   fgValueList::update();
   newList(_list);
+  int currentItem = puaComboBox::getCurrentItem();
+
+  
+// look for the previous value, in the new list
+  for (int i = 0; _list[i] != 0; i++) {
+    if (_list[i] == curValue) {
+    // don't set the current item again; this is important to avoid
+    // recursion here if the combo callback ultimately causes another dialog-update
+      if (i != currentItem) {
+        puaComboBox::setCurrentItem(i);
+      }
+      
+      return;
+    }
+  } // of list values iteration
+  
+// cound't find current item, default to first
+  if (currentItem != 0) {
+    puaComboBox::setCurrentItem(0);
+  }
+}
+
+int fgComboBox::checkHit(int b, int up, int x, int y)
+{
+  _inHit = true;
+  int r = puaComboBox::checkHit(b, up, x, y);
+  _inHit = false;
+  return r;
 }
 
 // end of dialog.cxx

src/GUI/dialog.hxx

@@ -259,9 +259,17 @@ public:
 class fgComboBox : public fgValueList, public puaComboBox {
 public:
     fgComboBox(int x1, int y1, int x2, int y2, SGPropertyNode *p, bool editable) :
-        fgValueList(p), puaComboBox(x1, y1, x2, y2, _list, editable) {}
+        fgValueList(p), 
+        puaComboBox(x1, y1, x2, y2, _list, editable),
+        _inHit(false)
+      {}
         
     void update();
+    
+    virtual int checkHit(int b, int up, int x, int y);
+
+private:
+    bool _inHit;
 };
 
 class fgSelectBox : public fgValueList, public puaSelectBox {

src/Instrumentation/Makefile.am

@@ -31,7 +31,8 @@ libInstrumentation_a_SOURCES = \
 	tacan.cxx tacan.hxx mk_viii.cxx mk_viii.hxx \
 	dclgps.cxx dclgps.hxx \
 	render_area_2d.cxx render_area_2d.hxx        \
-		groundradar.cxx groundradar.hxx \
+	groundradar.cxx groundradar.hxx \
-		agradar.cxx agradar.hxx rad_alt.cxx rad_alt.hxx
+	agradar.cxx agradar.hxx rad_alt.cxx rad_alt.hxx \
+	rnav_waypt_controller.cxx rnav_waypt_controller.hxx
 
 INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src -I$(top_builddir)/src

src/Instrumentation/gps.cxx

@@ -9,6 +9,8 @@
 
 #include "gps.hxx"
 
+#include <boost/tuple/tuple.hpp>
+
 #include <memory>
 #include <set>
 #include <cstring>
@@ -17,6 +19,7 @@
 #include "Main/globals.hxx" // for get_subsystem
 #include "Main/util.hxx" // for fgLowPass
 #include "Navaids/positioned.hxx"
+#include <Navaids/waypoint.hxx>
 #include "Navaids/navrecord.hxx"
 #include "Airports/simple.hxx"
 #include "Airports/runways.hxx"
@@ -29,6 +32,7 @@
 
 using std::auto_ptr;
 using std::string;
+using namespace flightgear;
 
 ///////////////////////////////////////////////////////////////////
 
@@ -227,6 +231,9 @@ GPS::GPS ( SGPropertyNode *node) :
   string branch = "/instrumentation/" + _name;
   _gpsNode = fgGetNode(branch.c_str(), _num, true );
   _scratchNode = _gpsNode->getChild("scratch", 0, true);
+  
+  SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
+  _currentWayptNode = wp_node->getChild("wp", 1, true);
 }
 
 GPS::~GPS ()
@@ -255,18 +262,10 @@ GPS::init ()
   _northSouthVelocity = _gpsNode->getChild("ns-velocity-msec", 0, true);
   
 // waypoints
-  SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
-  SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true);
-
   // for compatability, alias selected course down to wp/wp[1]/desired-course-deg
-  SGPropertyNode* wp1Crs = wp1_node->getChild("desired-course-deg", 0, true);
+  SGPropertyNode* wp1Crs = _currentWayptNode->getChild("desired-course-deg", 0, true);
   wp1Crs->alias(_gpsNode->getChild("desired-course-deg", 0, true));
 
-//    _true_wp1_bearing_error_node =
-//        wp1_node->getChild("true-bearing-error-deg", 0, true);
-//    _magnetic_wp1_bearing_error_node =
-  //      wp1_node->getChild("magnetic-bearing-error-deg", 0, true);
-
   _tracking_bug_node = _gpsNode->getChild("tracking-bug", 0, true);
          
 // reference navid
@@ -299,10 +298,10 @@ GPS::init ()
   
 // navradio slaving properties  
   SGPropertyNode* toFlag = _gpsNode->getChild("to-flag", 0, true);
-  toFlag->alias(wp1_node->getChild("to-flag"));
+  toFlag->alias(_currentWayptNode->getChild("to-flag"));
   
   SGPropertyNode* fromFlag = _gpsNode->getChild("from-flag", 0, true);
-  fromFlag->alias(wp1_node->getChild("from-flag"));
+  fromFlag->alias(_currentWayptNode->getChild("from-flag"));
     
 // autopilot drive properties
   _apDrivingFlag = fgGetNode("/autopilot/settings/gps-driving-true-heading", true);
@@ -358,48 +357,38 @@ GPS::bind()
   tie(_scratchNode, "has-next", SGRawValueMethods<GPS, bool>(*this, &GPS::getScratchHasNext, NULL));
   _scratchValid = false;
 
-// waypoint data (including various historical things)
+  
   SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
-  SGPropertyNode *wp0_node = wp_node->getChild("wp", 0, true);
+  SGPropertyNode* wp0_node = wp_node->getChild("wp", 0, true);
-  SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true);
   
   tieSGGeodReadOnly(wp0_node, _wp0_position, "longitude-deg", "latitude-deg", "altitude-ft");
-  tie(wp0_node, "ID", SGRawValueMethods<GPS, const char*>
+  tie(_currentWayptNode, "ID", SGRawValueMethods<GPS, const char*>
-    (*this, &GPS::getWP0Ident, NULL));
-  tie(wp0_node, "name", SGRawValueMethods<GPS, const char*>
-    (*this, &GPS::getWP0Name, NULL));
-    
-  tieSGGeodReadOnly(wp1_node, _wp1_position, "longitude-deg", "latitude-deg", "altitude-ft");
-  tie(wp1_node, "ID", SGRawValueMethods<GPS, const char*>
     (*this, &GPS::getWP1Ident, NULL));
-  tie(wp1_node, "name", SGRawValueMethods<GPS, const char*>
-    (*this, &GPS::getWP1Name, NULL));
   
-  tie(wp1_node, "distance-nm", SGRawValueMethods<GPS, double>
+  tie(_currentWayptNode, "distance-nm", SGRawValueMethods<GPS, double>
     (*this, &GPS::getWP1Distance, NULL));
-  tie(wp1_node, "bearing-true-deg", SGRawValueMethods<GPS, double>
+  tie(_currentWayptNode, "bearing-true-deg", SGRawValueMethods<GPS, double>
     (*this, &GPS::getWP1Bearing, NULL));
-  tie(wp1_node, "bearing-mag-deg", SGRawValueMethods<GPS, double>
+  tie(_currentWayptNode, "bearing-mag-deg", SGRawValueMethods<GPS, double>
     (*this, &GPS::getWP1MagBearing, NULL));
-  tie(wp1_node, "TTW-sec", SGRawValueMethods<GPS, double>
+  tie(_currentWayptNode, "TTW-sec", SGRawValueMethods<GPS, double>
     (*this, &GPS::getWP1TTW, NULL));
-  tie(wp1_node, "TTW", SGRawValueMethods<GPS, const char*>
+  tie(_currentWayptNode, "TTW", SGRawValueMethods<GPS, const char*>
     (*this, &GPS::getWP1TTWString, NULL));
   
-  tie(wp1_node, "course-deviation-deg", SGRawValueMethods<GPS, double>
+  tie(_currentWayptNode, "course-deviation-deg", SGRawValueMethods<GPS, double>
     (*this, &GPS::getWP1CourseDeviation, NULL));
-  tie(wp1_node, "course-error-nm", SGRawValueMethods<GPS, double>
+  tie(_currentWayptNode, "course-error-nm", SGRawValueMethods<GPS, double>
     (*this, &GPS::getWP1CourseErrorNm, NULL));
-  tie(wp1_node, "to-flag", SGRawValueMethods<GPS, bool>
+  tie(_currentWayptNode, "to-flag", SGRawValueMethods<GPS, bool>
     (*this, &GPS::getWP1ToFlag, NULL));
-  tie(wp1_node, "from-flag", SGRawValueMethods<GPS, bool>
+  tie(_currentWayptNode, "from-flag", SGRawValueMethods<GPS, bool>
     (*this, &GPS::getWP1FromFlag, NULL));
 
 // leg properties (only valid in DTO/LEG modes, not OBS)
   tie(wp_node, "leg-distance-nm", SGRawValueMethods<GPS, double>(*this, &GPS::getLegDistance, NULL));
   tie(wp_node, "leg-true-course-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getLegCourse, NULL));
   tie(wp_node, "leg-mag-course-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getLegMagCourse, NULL));
-  tie(wp_node, "alt-dist-ratio", SGRawValueMethods<GPS, double>(*this, &GPS::getAltDistanceRatio, NULL));
 
 // navradio slaving properties  
   tie(_gpsNode, "cdi-deflection", SGRawValueMethods<GPS,double>
@@ -426,12 +415,11 @@ GPS::clearOutput()
   _last_vertical_speed = 0.0;
   _last_true_track = 0.0;
   _lastEWVelocity = _lastNSVelocity = 0.0;
+  _currentWaypt = _prevWaypt = NULL;
+  _legDistanceNm = -1.0;
   
   _raim_node->setDoubleValue(0.0);
   _indicated_pos = SGGeod();
-  _wp1DistanceM = 0.0;
-  _wp1TrueBearing = 0.0;
-  _wp1_position = SGGeod();
   _odometer_node->setDoubleValue(0);
   _trip_odometer_node->setDoubleValue(0);
   _tracking_bug_node->setDoubleValue(0);
@@ -455,61 +443,28 @@ GPS::update (double delta_time_sec)
   if (delta_time_sec <= 0.0) {
     return; // paused, don't bother
   }    
-    // TODO: Add noise and other errors.
-/*
   
-    // Bias and random error
-    double random_factor = sg_random();
-    double random_error = 1.4;
-    double error_radius = 5.1;
-    double bias_max_radius = 5.1;
-    double random_max_radius = 1.4;
-
-    bias_length += (random_factor-0.5) * 1.0e-3;
-    if (bias_length <= 0.0) bias_length = 0.0;
-    else if (bias_length >= bias_max_radius) bias_length = bias_max_radius;
-    bias_angle  += (random_factor-0.5) * 1.0e-3;
-    if (bias_angle <= 0.0) bias_angle = 0.0;
-    else if (bias_angle >= 360.0) bias_angle = 360.0;
-
-    double random_length = random_factor * random_max_radius;
-    double random_angle = random_factor * 360.0;
-
-    double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0);
-    double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0);
-    double random_x = random_length * cos(random_angle * SG_PI / 180.0);
-    double random_y = random_length * sin(random_angle * SG_PI / 180.0);
-    double error_x = bias_x + random_x;
-    double error_y = bias_y + random_y;
-    double error_length = sqrt(error_x*error_x + error_y*error_y);
-    double error_angle = atan(error_y / error_x) * 180.0 / SG_PI;
-
-    double lat2;
-    double lon2;
-    double az2;
-    geo_direct_wgs_84 ( altitude_m, latitude_deg,
-                        longitude_deg, error_angle,
-                        error_length, &lat2, &lon2,
-                        &az2 );
-    //cout << lat2 << " " << lon2 << endl;
-    printf("%f %f \n", bias_length, bias_angle);
-    printf("%3.7f %3.7f \n", lat2, lon2);
-    printf("%f %f \n", error_length, error_angle);
-
-*/
   _raim_node->setDoubleValue(1.0);
   _indicated_pos = _position.get();
   updateBasicData(delta_time_sec);
 
   if (_dataValid) {
-    if (_mode != "obs") {
+    if (_wayptController.get()) {
+      _wayptController->update();
+      SGGeod p(_wayptController->position());
+      _currentWayptNode->setDoubleValue("longitude-deg", p.getLongitudeDeg());
+      _currentWayptNode->setDoubleValue("latitude-deg", p.getLatitudeDeg());
+      _currentWayptNode->setDoubleValue("altitude-ft", p.getElevationFt());
+      
+      _desiredCourse = getLegMagCourse();
+      
       updateTurn();
+      updateRouteData();
     }
 
-    updateWaypoints();
+    
     updateTrackingBug();
     updateReferenceNavaid(delta_time_sec);
-    updateRouteData();
     driveAutopilot();
   }
   
@@ -538,6 +493,49 @@ GPS::update (double delta_time_sec)
   _lastPosValid = true;
 }
 
+///////////////////////////////////////////////////////////////////////////
+// implement the RNAV interface 
+SGGeod GPS::position()
+{
+  if (!_dataValid) {
+    return SGGeod();
+  }
+  
+  return _indicated_pos;
+}
+
+double GPS::trackDeg()
+{
+  return _last_true_track;
+}
+
+double GPS::groundSpeedKts()
+{
+  return _last_speed_kts;
+}
+
+double GPS::vspeedFPM()
+{
+  return _last_vertical_speed;
+}
+
+double GPS::magvarDeg()
+{
+  return _magvar_node->getDoubleValue();
+}
+
+double GPS::overflightArmDistanceM()
+{
+  return _config.overflightArmDistanceNm() * SG_NM_TO_METER;
+}
+
+double GPS::selectedMagCourse()
+{
+  return _selectedCourse;
+}
+
+///////////////////////////////////////////////////////////////////////////
+
 void
 GPS::updateBasicData(double dt)
 {
@@ -601,13 +599,6 @@ GPS::updateTrackingBug()
   _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
 }
 
-void
-GPS::updateWaypoints()
-{  
-  double az2;
-  SGGeodesy::inverse(_indicated_pos, _wp1_position, _wp1TrueBearing, az2,_wp1DistanceM);
-}
-
 void GPS::updateReferenceNavaid(double dt)
 {
   if (!_ref_navaid_set) {
@@ -690,7 +681,9 @@ void GPS::routeActivated()
     }
   } else if (_mode == "leg") {
     SG_LOG(SG_INSTR, SG_INFO, "GPS::route deactivated, switching to OBS mode");
-    selectOBSMode();
+    // select OBS mode, but keep current waypoint-as is
+    _mode = "obs";
+    wp1Changed();
   }
 }
 
@@ -701,8 +694,8 @@ void GPS::routeManagerSequenced()
     return;
   }
   
-  int index = _routeMgr->currentWaypoint(),
+  int index = _routeMgr->currentIndex(),
-    count = _routeMgr->size();
+    count = _routeMgr->numWaypts();
   if ((index < 0) || (index >= count)) {
     SG_LOG(SG_INSTR, SG_ALERT, "GPS: malformed route, index=" << index);
     return;
@@ -711,17 +704,15 @@ void GPS::routeManagerSequenced()
   SG_LOG(SG_INSTR, SG_INFO, "GPS waypoint index is now " << index);
   
   if (index > 0) {
-    SGWayPoint wp0(_routeMgr->get_waypoint(index - 1));
+    _prevWaypt = _routeMgr->previousWaypt();
-    _wp0Ident = wp0.get_id();
+    if (_prevWaypt->flag(WPT_DYNAMIC)) {
-    _wp0Name = wp0.get_name();
+      _wp0_position = _indicated_pos;
-    _wp0_position = wp0.get_target();
+    } else {
-
+      _wp0_position = _prevWaypt->position();
+    }
   }
   
-  SGWayPoint wp1(_routeMgr->get_waypoint(index));
+  _currentWaypt = _routeMgr->currentWaypt();
-  _wp1Ident = wp1.get_id();
-  _wp1Name = wp1.get_name();
-  _wp1_position = wp1.get_target();
 
   _desiredCourse = getLegMagCourse();
   _desiredCourseNode->fireValueChanged();
@@ -745,8 +736,8 @@ void GPS::routeFinished()
   }
   
   SG_LOG(SG_INSTR, SG_INFO, "GPS route finished, reverting to OBS");
+  // select OBS mode, but keep current waypoint-as is
   _mode = "obs";
-  _wp0_position = _indicated_pos;
   wp1Changed();
 }
 
@@ -800,7 +791,7 @@ void GPS::updateTurn()
     double deviationNm = (distanceM * SG_METER_TO_NM) - _turnRadius;
     double deviationDeg = desiredCourse - getMagTrack();
     deviationNm = copysign(deviationNm, deviationDeg);
-    // FXIME
+    // FIXME
     //_wp1_course_deviation_node->setDoubleValue(deviationDeg);
     //_wp1_course_error_nm_node->setDoubleValue(deviationNm);
     //_cdiDeflectionNode->setDoubleValue(deviationDeg);
@@ -809,26 +800,29 @@ void GPS::updateTurn()
 
 void GPS::updateOverflight()
 {
-  if ((_wp1DistanceM * SG_METER_TO_NM) > _config.overflightArmDistanceNm()) {
+  if (!_wayptController->isDone()) {
     return;
   }
   
-  if (getWP1ToFlag()) {
-    return; // still heading towards the WP
-  }
-  
   if (_mode == "dto") {
     SG_LOG(SG_INSTR, SG_INFO, "GPS DTO reached destination point");
     
     // check for wp1 being on active route - resume leg mode
     if (_routeMgr->isRouteActive()) {
-      int index = _routeMgr->findWaypoint(_wp1_position);
+      int index = _routeMgr->findWayptIndex(_currentWaypt->position());
       if (index >= 0) {
         SG_LOG(SG_INSTR, SG_INFO, "GPS DTO, resuming LEG mode at wp:" << index);
         _mode = "leg";
         _routeMgr->jumpToIndex(index);
       }
     }
+    
+    if (_mode == "dto") {
+      // if we didn't enter leg mode, drop back to OBS mode
+      // select OBS mode, but keep current waypoint-as is
+      _mode = "obs";
+      wp1Changed();
+    }
   } else if (_mode == "leg") {
     SG_LOG(SG_INSTR, SG_INFO, "GPS doing overflight sequencing");
     _routeMgr->sequence();
@@ -867,8 +861,8 @@ void GPS::computeTurnData()
     return;
   }
   
-  int curIndex = _routeMgr->currentWaypoint();
+  WayptRef next = _routeMgr->nextWaypt();
-  if ((curIndex + 1) >= _routeMgr->size()) {
+  if (!next || next->flag(WPT_DYNAMIC)) {
     _anticipateTurn = false;
     return;
   }
@@ -880,10 +874,8 @@ void GPS::computeTurnData()
   
   _turnStartBearing = _desiredCourse;
 // compute next leg course
-  SGWayPoint wp1(_routeMgr->get_waypoint(curIndex)),
-    wp2(_routeMgr->get_waypoint(curIndex + 1));
   double crs, dist;
-  wp2.CourseAndDistance(wp1, &crs, &dist);
+  boost::tie(crs, dist) = next->courseAndDistanceFrom(_currentWaypt->position());
     
 
 // compute offset bearing
@@ -897,9 +889,9 @@ void GPS::computeTurnData()
     ", out=" << crs << "; turnAngle=" << _turnAngle << ", median=" << median 
     << ", offset=" << offsetBearing);
 
-  SG_LOG(SG_INSTR, SG_INFO, "next leg is now:" << wp1.get_id() << "->" << wp2.get_id());
+  SG_LOG(SG_INSTR, SG_INFO, "next leg is now:" << _currentWaypt->ident() << "->" << next->ident());
 
-  _turnPt = _wp1_position;
+  _turnPt = _currentWaypt->position();
   _anticipateTurn = true;
 }
 
@@ -945,10 +937,10 @@ double GPS::computeTurnRadiusNm(double aGroundSpeedKts) const
 
 void GPS::updateRouteData()
 {
-  double totalDistance = _wp1DistanceM * SG_METER_TO_NM;
+  double totalDistance = _wayptController->distanceToWayptM() * SG_METER_TO_NM;
   // walk all waypoints from wp2 to route end, and sum
-  for (int i=_routeMgr->currentWaypoint()+1; i<_routeMgr->size(); ++i) {
+  for (int i=_routeMgr->currentIndex()+1; i<_routeMgr->numWaypts(); ++i) {
-    totalDistance += _routeMgr->get_waypoint(i).get_distance();
+    //totalDistance += _routeMgr->get_waypoint(i).get_distance();
   }
   
   _routeDistanceNm->setDoubleValue(totalDistance * SG_METER_TO_NM);
@@ -979,13 +971,36 @@ void GPS::driveAutopilot()
 
 void GPS::wp1Changed()
 {
+  if (_mode == "leg") {
+    _wayptController.reset(WayptController::createForWaypt(this, _currentWaypt));
+  } else if (_mode == "obs") {
+    _wayptController.reset(new OBSController(this, _currentWaypt));
+  } else if (_mode == "dto") {
+    _wayptController.reset(new DirectToController(this, _currentWaypt, _wp0_position));
+  }
+
+  _wayptController->init();
+
+  if (_mode == "obs") {
+    _legDistanceNm = -1.0;
+  } else {
+    _wayptController->update();
+    _legDistanceNm = _wayptController->distanceToWayptM() * SG_METER_TO_NM;
+  }
+  
   if (!_config.driveAutopilot()) {
     return;
   }
   
-  double altFt = _wp1_position.getElevationFt();
+  RouteRestriction ar = _currentWaypt->altitudeRestriction();
-  if (altFt > -9990.0) {
+  double restrictAlt = _currentWaypt->altitudeFt();
-    _apTargetAltitudeFt->setDoubleValue(altFt);
+  double alt = _indicated_pos.getElevationFt();
+  if ((ar == RESTRICT_AT) ||
+       ((ar == RESTRICT_ABOVE) && (alt < restrictAlt)) ||
+       ((ar == RESTRICT_BELOW) && (alt > restrictAlt)))
+  {
+    SG_LOG(SG_AUTOPILOT, SG_INFO, "current waypt has altitude set, setting on AP");
+    _apTargetAltitudeFt->setDoubleValue(restrictAlt);
   }
 }
 
@@ -1011,16 +1026,16 @@ double GPS::getLegDistance() const
     return -1;
   }
   
-  return SGGeodesy::distanceNm(_wp0_position, _wp1_position);
+  return _legDistanceNm;
 }
 
 double GPS::getLegCourse() const
 {
-  if (!_dataValid) {
+  if (!_dataValid || !_wayptController.get()) {
     return -9999.0;
   }
   
-  return SGGeodesy::courseDeg(_wp0_position, _wp1_position);
+  return _wayptController->targetTrackDeg();
 }
 
 double GPS::getLegMagCourse() const
@@ -1034,21 +1049,6 @@ double GPS::getLegMagCourse() const
   return m;
 }
 
-double GPS::getAltDistanceRatio() const
-{
-  if (!_dataValid || (_mode == "obs")) {
-    return 0.0;
-  }
-  
-  double dist = SGGeodesy::distanceM(_wp0_position, _wp1_position);
-  if ( dist <= 0.0 ) {
-    return 0.0;
-  }
-  
-  double alt_difference_m = _wp0_position.getElevationM() - _wp1_position.getElevationM();
-  return alt_difference_m / dist;
-}
-
 double GPS::getMagTrack() const
 {
   if (!_dataValid) {
@@ -1086,16 +1086,12 @@ const char* GPS::getWP0Ident() const
     return "";
   }
   
-  return _wp0Ident.c_str();
+  return _prevWaypt->ident().c_str();
 }
 
 const char* GPS::getWP0Name() const
 {
-  if (!_dataValid || (_mode != "leg")) {
+  return "";
-    return "";
-  }
-  
-  return _wp0Name.c_str();
 }
 
 const char* GPS::getWP1Ident() const
@@ -1104,56 +1100,49 @@ const char* GPS::getWP1Ident() const
     return "";
   }
   
-  return _wp1Ident.c_str();
+  return _currentWaypt->ident().c_str();
 }
 
 const char* GPS::getWP1Name() const
 {
-  if (!_dataValid) {
+  return "";
-    return "";
-  }
-
-  return _wp1Name.c_str();
 }
 
 double GPS::getWP1Distance() const
 {
-  if (!_dataValid) {
+  if (!_dataValid || !_wayptController.get()) {
     return -1.0;
   }
   
-  return _wp1DistanceM * SG_METER_TO_NM;
+  return _wayptController->distanceToWayptM() * SG_METER_TO_NM;
 }
 
 double GPS::getWP1TTW() const
 {
-  if (!_dataValid) {
+  if (!_dataValid || !_wayptController.get()) {
     return -1.0;
   }
   
-  if (_last_speed_kts < 1.0) {
+  return _wayptController->timeToWaypt();
-    return -1.0;
-  }
-  
-  return (getWP1Distance() / _last_speed_kts) * 3600.0;
 }
 
 const char* GPS::getWP1TTWString() const
 {
-  if (!_dataValid) {
+  double t = getWP1TTW();
+  if (t <= 0.0) {
     return "";
   }
   
-  return makeTTWString(getWP1TTW());
+  return makeTTWString(t);
 }
 
 double GPS::getWP1Bearing() const
 {
-  if (!_dataValid) {
+  if (!_dataValid || !_wayptController.get()) {
     return -9999.0;
   }
   
-  return _wp1TrueBearing;
+  return _wayptController->trueBearingDeg();
 }
 
 double GPS::getWP1MagBearing() const
@@ -1162,56 +1151,41 @@ double GPS::getWP1MagBearing() const
     return -9999.0;
   }
 
-  double magBearing = _wp1TrueBearing - _magvar_node->getDoubleValue();
+  double magBearing =  _wayptController->trueBearingDeg() - _magvar_node->getDoubleValue();
   SG_NORMALIZE_RANGE(magBearing, 0.0, 360.0);
   return magBearing;
 }
 
 double GPS::getWP1CourseDeviation() const
 {
-  if (!_dataValid) {
+  if (!_dataValid || !_wayptController.get()) {
     return 0.0;
   }
 
-  double dev = getWP1MagBearing() - _desiredCourse;
+  return _wayptController->courseDeviationDeg();
-  SG_NORMALIZE_RANGE(dev, -180.0, 180.0);
-  
-  if (fabs(dev) > 90.0) {
-    // When the course is away from the waypoint, 
-    // it makes sense to change the sign of the deviation.
-    dev *= -1.0;
-    SG_NORMALIZE_RANGE(dev, -90.0, 90.0);
-  }
-  
-  return dev;
 }
 
 double GPS::getWP1CourseErrorNm() const
 {
-  if (!_dataValid) {
+  if (!_dataValid || !_wayptController.get()) {
     return 0.0;
   }
   
-  double radDev = getWP1CourseDeviation() * SG_DEGREES_TO_RADIANS;
+  return _wayptController->xtrackErrorNm();
-  double course_error_m = sin(radDev) * _wp1DistanceM;
-  return course_error_m * SG_METER_TO_NM;
 }
 
 bool GPS::getWP1ToFlag() const
 {
-  if (!_dataValid) {
+  if (!_dataValid || !_wayptController.get()) {
     return false;
   }
   
-  double dev = getWP1MagBearing() - _desiredCourse;
+  return _wayptController->toFlag();
-  SG_NORMALIZE_RANGE(dev, -180.0, 180.0);
-
-  return (fabs(dev) < 90.0);
 }
 
 bool GPS::getWP1FromFlag() const
 {
-  if (!_dataValid) {
+  if (!_dataValid || !_wayptController.get()) {
     return false;
   }
   
@@ -1278,9 +1252,9 @@ void GPS::setCommand(const char* aCmd)
     defineWaypoint();
   } else if (!strcmp(aCmd, "route-insert-before")) {
     int index = _scratchNode->getIntValue("index");
-    if (index < 0 || (_routeMgr->size() == 0)) {
+    if (index < 0 || (_routeMgr->numWaypts() == 0)) {
-      index = _routeMgr->size();
+      index = _routeMgr->numWaypts();
-    } else if (index >= _routeMgr->size()) {
+    } else if (index >= _routeMgr->numWaypts()) {
       SG_LOG(SG_INSTR, SG_WARN, "GPS:route-insert-before, bad index:" << index);
       return;
     }
@@ -1288,9 +1262,9 @@ void GPS::setCommand(const char* aCmd)
     insertWaypointAtIndex(index);
   } else if (!strcmp(aCmd, "route-insert-after")) {
     int index = _scratchNode->getIntValue("index");
-    if (index < 0 || (_routeMgr->size() == 0)) {
+    if (index < 0 || (_routeMgr->numWaypts() == 0)) {
-      index = _routeMgr->size();
+      index = _routeMgr->numWaypts();
-    } else if (index >= _routeMgr->size()) {
+    } else if (index >= _routeMgr->numWaypts()) {
       SG_LOG(SG_INSTR, SG_WARN, "GPS:route-insert-after, bad index:" << index);
       return;
     } else {
@@ -1301,8 +1275,8 @@ void GPS::setCommand(const char* aCmd)
   } else if (!strcmp(aCmd, "route-delete")) {
     int index = _scratchNode->getIntValue("index");
     if (index < 0) {
-      index = _routeMgr->size();
+      index = _routeMgr->numWaypts();
-    } else if (index >= _routeMgr->size()) {
+    } else if (index >= _routeMgr->numWaypts()) {
       SG_LOG(SG_INSTR, SG_WARN, "GPS:route-delete, bad index:" << index);
       return;
     }
@@ -1333,17 +1307,15 @@ bool GPS::isScratchPositionValid() const
 
 void GPS::directTo()
 {  
-  _wp0_position = _indicated_pos;
+  if (!isScratchPositionValid()) {
-  
+    return;
-  if (isScratchPositionValid()) {
-    _wp1Ident = _scratchNode->getStringValue("ident");
-    _wp1Name = _scratchNode->getStringValue("name");
-    _wp1_position = _scratchPos;
   }
   
+  _prevWaypt = NULL;
+  _wp0_position = _indicated_pos;
+  _currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL);
   _mode = "dto";
-  _desiredCourse = getLegMagCourse();
+
-  _desiredCourseNode->fireValueChanged();
   clearScratch();
   wp1Changed();
 }
@@ -1359,8 +1331,8 @@ void GPS::loadRouteWaypoint()
   int index = _scratchNode->getIntValue("index", -9999);
   clearScratch();
   
-  if ((index < 0) || (index >= _routeMgr->size())) { // no index supplied, use current wp
+  if ((index < 0) || (index >= _routeMgr->numWaypts())) { // no index supplied, use current wp
-    index = _routeMgr->currentWaypoint();
+    index = _routeMgr->currentIndex();
   }
   
   _searchIsRoute = true;
@@ -1370,18 +1342,16 @@ void GPS::loadRouteWaypoint()
 void GPS::setScratchFromRouteWaypoint(int aIndex)
 {
   assert(_searchIsRoute);
-  if ((aIndex < 0) || (aIndex >= _routeMgr->size())) {
+  if ((aIndex < 0) || (aIndex >= _routeMgr->numWaypts())) {
     SG_LOG(SG_INSTR, SG_WARN, "GPS:setScratchFromRouteWaypoint: route-index out of bounds");
     return;
   }
   
   _searchResultIndex = aIndex;
-  SGWayPoint wp(_routeMgr->get_waypoint(aIndex));
+  WayptRef wp = _routeMgr->wayptAtIndex(aIndex);
-  _scratchNode->setStringValue("name", wp.get_name());
+  _scratchNode->setStringValue("ident", wp->ident());
-  _scratchNode->setStringValue("ident", wp.get_id());
+  _scratchPos = wp->position();
-  _scratchPos = wp.get_target();
   _scratchValid = true;
-  _scratchNode->setDoubleValue("course", wp.get_track());
   _scratchNode->setIntValue("index", aIndex);
 }
 
@@ -1476,9 +1446,9 @@ void GPS::search()
 
   auto_ptr<FGPositioned::Filter> f(createFilter(_searchType));
   if (_searchNames) {
-    _searchResults = FGPositioned::findAllWithName(_searchQuery, f.get());
+    _searchResults = FGPositioned::findAllWithName(_searchQuery, f.get(), _searchExact);
   } else {
-    _searchResults = FGPositioned::findAllWithIdent(_searchQuery, f.get());
+    _searchResults = FGPositioned::findAllWithIdent(_searchQuery, f.get(), _searchExact);
   }
   
   bool orderByRange = _scratchNode->getBoolValue("order-by-distance", true);
@@ -1498,7 +1468,7 @@ bool GPS::getScratchHasNext() const
 {
   int lastResult;
   if (_searchIsRoute) {
-    lastResult = _routeMgr->size() - 1;
+    lastResult = _routeMgr->numWaypts() - 1;
   } else {
     lastResult = (int) _searchResults.size() - 1;
   }
@@ -1584,14 +1554,14 @@ void GPS::addAirportToScratch(FGAirport* aAirport)
 
 void GPS::selectOBSMode()
 {
-  if (isScratchPositionValid()) {
+  if (!isScratchPositionValid()) {
-    _wp1Ident = _scratchNode->getStringValue("ident");
+    return;
-    _wp1Name = _scratchNode->getStringValue("name");
-    _wp1_position = _scratchPos;
   }
   
   SG_LOG(SG_INSTR, SG_INFO, "GPS switching to OBS mode");
   _mode = "obs";
+
+  _currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL);
   _wp0_position = _indicated_pos;
   wp1Changed();
 }
@@ -1680,7 +1650,7 @@ void GPS::defineWaypoint()
 void GPS::insertWaypointAtIndex(int aIndex)
 {
   // note we do allow index = routeMgr->size(), that's an append
-  if ((aIndex < 0) || (aIndex > _routeMgr->size())) {
+  if ((aIndex < 0) || (aIndex > _routeMgr->numWaypts())) {
     throw sg_range_exception("GPS::insertWaypointAtIndex: index out of bounds");
   }
   
@@ -1690,18 +1660,18 @@ void GPS::insertWaypointAtIndex(int aIndex)
   }
   
   string ident = _scratchNode->getStringValue("ident");
-  string name = _scratchNode->getStringValue("name");
 
-  _routeMgr->add_waypoint(SGWayPoint(_scratchPos, ident, name), aIndex);
+  WayptRef wpt = new BasicWaypt(_scratchPos, ident, NULL);
+  _routeMgr->insertWayptAtIndex(wpt, aIndex);
 }
 
 void GPS::removeWaypointAtIndex(int aIndex)
 {
-  if ((aIndex < 0) || (aIndex >= _routeMgr->size())) {
+  if ((aIndex < 0) || (aIndex >= _routeMgr->numWaypts())) {
     throw sg_range_exception("GPS::removeWaypointAtIndex: index out of bounds");
   }
   
-  _routeMgr->pop_waypoint(aIndex);
+  _routeMgr->removeWayptAtIndex(aIndex);
 }
 
 void GPS::tieSGGeod(SGPropertyNode* aNode, SGGeod& aRef, 

src/Instrumentation/gps.hxx

@@ -8,12 +8,14 @@
 #define __INSTRUMENTS_GPS_HXX 1
 
 #include <cassert>
+#include <memory>
 
 #include <simgear/props/props.hxx>
 #include <simgear/structure/subsystem_mgr.hxx>
 #include <simgear/math/SGMath.hxx>
 
-#include "Navaids/positioned.hxx"
+#include <Navaids/positioned.hxx>
+#include <Instrumentation/rnav_waypt_controller.hxx>
 
 // forward decls
 class SGRoute;
@@ -73,20 +75,29 @@ private:
  * /instrumentation/gps/magnetic-bug-error-deg
 
  */
-class GPS : public SGSubsystem
+class GPS : public SGSubsystem, public flightgear::RNAV
 {
-
 public:
-
     GPS (SGPropertyNode *node);
     GPS ();
     virtual ~GPS ();
 
+  // SGSubsystem interface
     virtual void init ();
     virtual void update (double delta_time_sec);
     
     virtual void bind();
     virtual void unbind();
+
+  // RNAV interface
+    virtual SGGeod position();
+    virtual double trackDeg();
+    virtual double groundSpeedKts();
+    virtual double vspeedFPM();
+    virtual double magvarDeg();
+    virtual double selectedMagCourse();
+    virtual double overflightArmDistanceM();
+    
 private:
     friend class GPSListener;
     friend class SearchFilter;
@@ -188,7 +199,6 @@ private:
     void clearOutput();
 
     void updateBasicData(double dt);
-    void updateWaypoints();
 
     void updateTrackingBug();
     void updateReferenceNavaid(double dt);
@@ -275,7 +285,6 @@ private:
   double getLegDistance() const;
   double getLegCourse() const;
   double getLegMagCourse() const;
-  double getAltDistanceRatio() const;
   
   double getTrueTrack() const { return _last_true_track; }
   double getMagTrack() const;
@@ -325,6 +334,7 @@ private:
 
 // members
   SGPropertyNode_ptr _gpsNode;
+  SGPropertyNode_ptr _currentWayptNode;
   SGPropertyNode_ptr _magvar_node;
   SGPropertyNode_ptr _serviceable_node;
   SGPropertyNode_ptr _electrical_node;
@@ -379,10 +389,8 @@ private:
   
   SGGeodProperty _position;
   SGGeod _wp0_position;
-  SGGeod _wp1_position;
   SGGeod _indicated_pos;
-  std::string _wp0Ident, _wp0Name, _wp1Ident, _wp1Name;
+  double _legDistanceNm;
-  double _wp1DistanceM, _wp1TrueBearing;
   
 // scratch data
   SGGeod _scratchPos;
@@ -410,7 +418,11 @@ private:
     SGGeod _turnPt;
     SGGeod _turnCentre;
   
+  std::auto_ptr<flightgear::WayptController> _wayptController;
+  
   SGPropertyNode_ptr _realismSimpleGps; ///< should the GPS be simple or realistic?
+  flightgear::WayptRef _prevWaypt;
+  flightgear::WayptRef _currentWaypt;
   
 // autopilot drive properties
   SGPropertyNode_ptr _apDrivingFlag;

src/Instrumentation/rnav_waypt_controller.cxx

@@ -0,0 +1,699 @@
+// rnav_waypt_controller.cxx - Waypoint-specific behaviours for RNAV systems
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#include "rnav_waypt_controller.hxx"
+
+#include <cassert>
+
+#include <simgear/sg_inlines.h>
+#include <simgear/structure/exception.hxx>
+
+#include <Airports/runways.hxx>
+
+namespace flightgear
+{
+
+const double KNOTS_TO_METRES_PER_SECOND = SG_NM_TO_METER / 3600.0;
+
+double pmod(double x, double y)
+{
+  if (x < 0.0) {
+    return -fmod(x, y);
+  } else {
+    return fmod(x,y);
+  }
+}
+
+// implementation of
+// http://williams.best.vwh.net/avform.htm#Intersection
+bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result)
+{
+  double crs13 = r1 * SG_DEGREES_TO_RADIANS;
+  double crs23 = r2 * SG_DEGREES_TO_RADIANS;
+  double dst12 = SGGeodesy::distanceRad(a, b);
+  
+  //IF sin(lon2-lon1)<0
+  // crs12=acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1)))
+  // crs21=2.*pi-acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2)))
+  // ELSE
+  // crs12=2.*pi-acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1)))
+  // crs21=acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2)))
+  // ENDIF
+  
+  
+ // double diffLon = b.getLongitudeRad() - a.getLongitudeRad();
+  
+  double sinLat1 = sin(a.getLatitudeRad());
+  double cosLat1 = cos(a.getLatitudeRad());
+ // double sinLat2 = sin(b.getLatitudeRad());
+  //double cosLat2 = cos(b.getLatitudeRad());
+  double sinDst12 = sin(dst12);
+  double cosDst12 = cos(dst12);
+  
+  double crs12 = SGGeodesy::courseRad(a, b),
+    crs21 = SGGeodesy::courseRad(b, a);
+    
+  double degCrs12 = crs12 * SG_RADIANS_TO_DEGREES;
+  double degCrs21 = crs21 * SG_RADIANS_TO_DEGREES;
+    
+ /* 
+  if (sin(diffLon) < 0.0) {
+    crs12 = acos((sinLat2 - sinLat1 * cosDst12) / (sinDst12 * cosLat1));
+    crs21 = SGMiscd::twopi() - acos((sinLat1 - sinLat2*cosDst12)/(sinDst12*cosLat2));
+  } else {
+    crs12 = SGMiscd::twopi() - acos((sinLat2 - sinLat1 * cosDst12)/(sinDst12 * cosLat1));
+    crs21 = acos((sinLat1 - sinLat2 * cosDst12)/(sinDst12 * cosLat2));
+  }
+  */
+  
+  double ang1 = SGMiscd::normalizeAngle2(crs13-crs12);
+  double ang2 = SGMiscd::normalizeAngle2(crs21-crs23);
+    
+  if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) {
+    SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: infinity of intersections");
+    return false;
+  }
+  
+  if ((sin(ang1)*sin(ang2))<0.0) {
+    SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: intersection ambiguous");
+    return false;
+  }
+  
+  ang1 = fabs(ang1);
+  ang2 = fabs(ang2);
+
+  //ang3=acos(-cos(ang1)*cos(ang2)+sin(ang1)*sin(ang2)*cos(dst12)) 
+  //dst13=atan2(sin(dst12)*sin(ang1)*sin(ang2),cos(ang2)+cos(ang1)*cos(ang3))
+  //lat3=asin(sin(lat1)*cos(dst13)+cos(lat1)*sin(dst13)*cos(crs13))
+  
+  //lon3=mod(lon1-dlon+pi,2*pi)-pi
+
+  double ang3 = acos(-cos(ang1) * cos(ang2) + sin(ang1) * sin(ang2) * cosDst12);
+  double dst13 = atan2(sinDst12 * sin(ang1) * sin(ang2), cos(ang2) + cos(ang1)*cos(ang3));
+
+  SGGeoc pt3;
+  SGGeodesy::advanceRadM(a, crs13, dst13 * SG_RAD_TO_NM * SG_NM_TO_METER, pt3);
+
+  double lat3 = asin(sinLat1 * cos(dst13) + cosLat1 * sin(dst13) * cos(crs13));
+  
+  //dlon=atan2(sin(crs13)*sin(dst13)*cos(lat1),cos(dst13)-sin(lat1)*sin(lat3))
+  double dlon = atan2(sin(crs13)*sin(dst13)*cosLat1, cos(dst13)- (sinLat1 * sin(lat3)));
+  double lon3 = SGMiscd::normalizeAngle(-a.getLongitudeRad()-dlon);
+  
+  result = SGGeoc::fromRadM(-lon3, lat3, a.getRadiusM());
+  //result = pt3;
+  return true;
+}
+
+////////////////////////////////////////////////////////////////////////////
+
+WayptController::~WayptController()
+{
+}
+
+void WayptController::init()
+{
+}
+
+void WayptController::setDone()
+{
+  if (_isDone) {
+    SG_LOG(SG_AUTOPILOT, SG_WARN, "already done @ WayptController::setDone");
+  }
+  
+  _isDone = true;
+}
+
+double WayptController::timeToWaypt() const
+{
+  double gs = _rnav->groundSpeedKts();
+  if (gs < 1.0) {
+    return -1.0; // stationary
+  }
+  
+  gs*= KNOTS_TO_METRES_PER_SECOND;
+  return (distanceToWayptM() / gs);
+}
+
+//////////////
+
+class BasicWayptCtl : public WayptController
+{
+public:
+  BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
+    WayptController(aRNAV, aWpt)
+  {
+    if (aWpt->flag(WPT_DYNAMIC)) {
+      throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints");
+    }
+  }
+  
+  virtual void init()
+  {
+    _targetTrack = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
+  }
+
+  virtual void update()
+  {
+    double brg, az2;
+    SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); 
+    _courseDev = brg - _targetTrack;
+    SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
+    
+    if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
+      setDone();
+    }
+  } 
+
+  virtual double distanceToWayptM() const
+  {
+    return _distanceM;
+  }
+  
+  virtual double xtrackErrorNm() const
+  {
+    double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
+    return x * SG_METER_TO_NM;
+  }
+  
+  virtual bool toFlag() const
+  {
+    return (fabs(_courseDev) < 90.0);
+  }
+  
+  virtual double courseDeviationDeg() const
+  {
+    return _courseDev;
+  }
+  
+  virtual double trueBearingDeg() const
+  {
+    return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
+  }
+  
+  virtual SGGeod position() const
+  {
+    return _waypt->position();
+  }
+
+private:
+  double _distanceM;
+  double _courseDev;
+};
+
+/**
+ * Special controller for runways. For runways, we want very narrow deviation
+ * contraints, and to understand that any point along the paved area is
+ * equivalent to being 'at' the runway.
+ */
+class RunwayCtl : public WayptController
+{
+public:
+  RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) :
+    WayptController(aRNAV, aWpt)
+  {
+  }
+  
+  virtual void init()
+  {
+    _runway = static_cast<RunwayWaypt*>(_waypt.get())->runway();
+    _targetTrack = _runway->headingDeg();
+  }
+
+  virtual void update()
+  {
+    double brg, az2;
+    // use the far end of the runway for course deviation calculations. 
+    // this should do the correct thing both for takeoffs (including entering 
+    // the runway at a taxiway after the threshold) and also landings.
+    // seperately compute the distance to the threshold for timeToWaypt calc
+    SGGeodesy::inverse(_rnav->position(), _runway->end(), brg, az2, _distanceM); 
+    double _courseDev = brg - _targetTrack;
+    SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
+    
+    if (fabs(_courseDev) > 90.0) {
+      setDone();
+    }
+  } 
+  
+  virtual double distanceToWayptM() const
+  {
+    return SGGeodesy::distanceM(_rnav->position(), _runway->threshold());
+  }
+  
+  virtual double xtrackErrorNm() const
+  {
+    double x = sin(_courseDev * SG_RADIANS_TO_DEGREES) * _distanceM;
+    return x * SG_METER_TO_NM;
+  }
+
+  virtual double courseDeviationDeg() const
+  {
+    return _courseDev;
+  }
+
+  virtual double trueBearingDeg() const
+  {
+    // as in update(), use runway->end here, so the value remains
+    // sensible whether taking off or landing.
+    return SGGeodesy::courseDeg(_rnav->position(), _runway->end());
+  }
+  
+  virtual SGGeod position() const
+  {
+    return _runway->threshold();
+  }
+private:
+  FGRunway* _runway;
+  double _distanceM;
+  double _courseDev;
+};
+
+class ConstHdgToAltCtl : public WayptController
+{
+public:
+  ConstHdgToAltCtl(RNAV* aRNAV, const WayptRef& aWpt) :
+    WayptController(aRNAV, aWpt)
+    
+  {
+    if (_waypt->type() != "hdgToAlt") {
+      throw sg_exception("invalid waypoint type", "ConstHdgToAltCtl ctor");
+    }
+    
+    if (_waypt->altitudeRestriction() == RESTRICT_NONE) {
+      throw sg_exception("invalid waypoint alt restriction", "ConstHdgToAltCtl ctor");
+    }
+  }
+
+  virtual void init()
+  {
+    HeadingToAltitude* w = (HeadingToAltitude*) _waypt.get();
+    _targetTrack = w->headingDegMagnetic() + _rnav->magvarDeg();
+  }
+  
+  virtual void update()
+  {
+    double curAlt = _rnav->position().getElevationFt();
+    
+    switch (_waypt->altitudeRestriction()) {
+    case RESTRICT_AT: {
+      double d = curAlt - _waypt->altitudeFt();
+      if (fabs(d) < 50.0) {
+        SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt());
+        setDone();
+      }
+    } break;
+      
+    case RESTRICT_ABOVE:
+      if (curAlt >= _waypt->altitudeFt()) {
+        SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt());
+        setDone();
+      }
+      break;
+      
+    case RESTRICT_BELOW:
+      if (curAlt <= _waypt->altitudeFt()) {
+        SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt());
+        setDone();
+      }
+      break;
+    
+    case RESTRICT_NONE:
+      assert(false);
+      break;
+    }
+  }
+  
+  virtual double timeToWaypt() const
+  {
+    double d = fabs(_rnav->position().getElevationFt() - _waypt->altitudeFt());
+    return (d / _rnav->vspeedFPM()) * 60.0; // low pass filter here, probably
+  }
+  
+  virtual double distanceToWayptM() const
+  {
+    double gsMsec = _rnav->groundSpeedKts() * KNOTS_TO_METRES_PER_SECOND;
+    return timeToWaypt() * gsMsec;
+  }
+  
+  virtual SGGeod position() const
+  {
+    SGGeod p;
+    double az2;
+    SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
+    return p;
+  }
+};
+
+class InterceptCtl : public WayptController
+{
+public:
+  InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
+    WayptController(aRNAV, aWpt)
+    
+  {
+    if (_waypt->type() != "radialIntercept") {
+      throw sg_exception("invalid waypoint type", "InterceptCtl ctor");
+    }
+  }
+
+  virtual void init()
+  {
+    RadialIntercept* w = (RadialIntercept*) _waypt.get();
+    _trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
+    _targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg();
+  }
+  
+  virtual void update()
+  {
+    // note we want the outbound radial from the waypt, hence the ordering
+    // of arguments to courseDeg
+    double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
+    SG_LOG(SG_AUTOPILOT, SG_INFO, "current radial=" << r);
+    if (fabs(r - _trueRadial) < 0.5) {
+      SG_LOG(SG_GENERAL, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial);
+      setDone();
+    }
+  }
+  
+  virtual double distanceToWayptM() const
+  {
+    return SGGeodesy::distanceM(_rnav->position(), position());
+  }
+
+  virtual SGGeod position() const
+  {
+    SGGeoc c;
+    geocRadialIntersection(SGGeoc::fromGeod(_rnav->position()), _rnav->trackDeg(), 
+      SGGeoc::fromGeod(_waypt->position()), _trueRadial, c);
+    return SGGeod::fromGeoc(c);
+  }
+private:
+  double _trueRadial;
+};
+
+class DMEInterceptCtl : public WayptController
+{
+public:
+  DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
+    WayptController(aRNAV, aWpt)
+    
+  {
+    if (_waypt->type() != "dmeIntercept") {
+      throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor");
+    }
+  }
+
+  virtual void init()
+  {
+    _dme  = (DMEIntercept*) _waypt.get();
+    _targetTrack = _dme->courseDegMagnetic() + _rnav->magvarDeg();
+  }
+  
+  virtual void update()
+  {
+    _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position());
+    double d = fabs(_distanceNm - _dme->dmeDistanceNm());
+    if (d < 0.1) {
+      SG_LOG(SG_GENERAL, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm());
+      setDone();
+    }
+  }
+  
+  virtual double distanceToWayptM() const
+  {
+    return fabs(_distanceNm - _dme->dmeDistanceNm()) * SG_NM_TO_METER;
+  }
+  
+  virtual SGGeod position() const
+  {
+    SGGeod p;
+    double az2;
+    SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
+    return p;
+  }
+
+private:
+  DMEIntercept* _dme;
+  double _distanceNm;
+};
+
+class HoldCtl : public WayptController
+{
+public:
+  HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
+    WayptController(aRNAV, aWpt)
+    
+  {
+
+  }
+
+  virtual void init()
+  {
+  }
+  
+  virtual void update()
+  {
+    // fly inbound / outbound sides, or execute the turn
+  #if 0
+    if (inTurn) {
+    
+      targetTrack += dt * turnRateSec * turnDirection;
+      if (inbound) {
+        if .. targetTrack has passed inbound radial, doen with this turn
+      } else {
+        if target track has passed reciprocal radial done with turn
+      }
+    } else {
+      check time / distance elapsed
+      
+      if (sideDone) {
+        inTurn = true;
+        inbound = !inbound;
+        nextHeading = inbound;
+        if (!inbound) {
+          nextHeading += 180.0;
+          SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0);
+        }
+      }
+    
+    }
+  
+  #endif
+    setDone();
+  }
+  
+  virtual double distanceToWayptM() const
+  {
+    return -1.0;
+  }
+
+  virtual SGGeod position() const
+  {
+    return _waypt->position();
+  }
+};
+
+class VectorsCtl : public WayptController
+{
+public:
+  VectorsCtl(RNAV* aRNAV, const WayptRef& aWpt) :
+    WayptController(aRNAV, aWpt)
+    
+  {
+  }
+
+  virtual void init()
+  {
+ 
+  }
+  
+  virtual void update()
+  {
+    setDone();
+  }
+  
+  virtual double distanceToWayptM() const
+  {
+    return -1.0;
+  }
+  
+  virtual SGGeod position() const
+  {
+    return _waypt->position();
+  }
+
+private:
+};
+
+///////////////////////////////////////////////////////////////////////////////
+
+DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
+  WayptController(aRNAV, aWpt),
+  _origin(aOrigin)
+{
+}
+
+void DirectToController::init()
+{
+  if (_waypt->flag(WPT_DYNAMIC)) {
+    throw sg_exception("can't direct-to a dynamic waypoint");
+  }
+  
+  _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position());
+}
+
+void DirectToController::update()
+{
+  double brg, az2;
+  SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); 
+  _courseDev = brg - _targetTrack;
+  SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
+    
+  if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
+    setDone();
+  }
+}
+
+double DirectToController::distanceToWayptM() const
+{
+  return _distanceM;
+}
+
+double DirectToController::xtrackErrorNm() const
+{
+  double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
+  return x * SG_METER_TO_NM;
+}
+
+double DirectToController::courseDeviationDeg() const
+{
+  return _courseDev;
+}
+
+double DirectToController::trueBearingDeg() const
+{
+  return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
+}
+
+SGGeod DirectToController::position() const
+{
+  return _waypt->position();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
+  WayptController(aRNAV, aWpt)
+{
+}
+
+void OBSController::init()
+{
+  if (_waypt->flag(WPT_DYNAMIC)) {
+    throw sg_exception("can't use a dynamic waypoint for OBS mode");
+  }
+  
+  _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
+}
+
+void OBSController::update()
+{
+  _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
+  double brg, az2;
+  SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); 
+  _courseDev = brg - _targetTrack;
+  SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
+}
+
+bool OBSController::toFlag() const
+{
+  return (fabs(_courseDev) < 90.0);
+}
+
+double OBSController::distanceToWayptM() const
+{
+  return _distanceM;
+}
+
+double OBSController::xtrackErrorNm() const
+{
+  double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceM;
+  return x * SG_METER_TO_NM;
+}
+
+double OBSController::courseDeviationDeg() const
+{
+//  if (fabs(_courseDev) > 90.0) {
+ //   double d = -_courseDev;
+ //   SG_NORMALIZE_RANGE(d, -90.0, 90.0);
+  //  return d;
+  //}
+  
+  return _courseDev;
+}
+
+double OBSController::trueBearingDeg() const
+{
+  return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
+}
+
+SGGeod OBSController::position() const
+{
+  return _waypt->position();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aWpt)
+{
+  if (!aWpt) {
+    throw sg_exception("Passed null waypt", "WayptController::createForWaypt");
+  }
+  
+  const std::string& wty(aWpt->type());
+  if (wty == "runway") {
+    return new RunwayCtl(aRNAV, aWpt);
+  }
+  
+  if (wty == "radialIntercept") {
+    return new InterceptCtl(aRNAV, aWpt);
+  }
+  
+  if (wty == "dmeIntercept") {
+    return new DMEInterceptCtl(aRNAV, aWpt);
+  }
+  
+  if (wty == "hdgToAlt") {
+    return new ConstHdgToAltCtl(aRNAV, aWpt);
+  }
+  
+  if (wty == "vectors") {
+    return new VectorsCtl(aRNAV, aWpt);
+  }
+  
+  if (wty == "hold") {
+    return new HoldCtl(aRNAV, aWpt);
+  }
+  
+  return new BasicWayptCtl(aRNAV, aWpt);
+}
+
+} // of namespace flightgear
+

src/Instrumentation/rnav_waypt_controller.hxx

@@ -0,0 +1,193 @@
+// rnav_waypt_controller.hxx - Waypoint-specific behaviours for RNAV systems
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#ifndef FG_WAYPT_CONTROLLER_HXX
+#define FG_WAYPT_CONTROLLER_HXX
+
+#include <Navaids/waypoint.hxx>
+
+namespace flightgear
+{
+
+/**
+ * Abstract RNAV interface, for devices which implement an RNAV
+ * system - INS / GPS / FMS
+ */
+class RNAV
+{
+public:
+  virtual SGGeod position() = 0;
+  
+  /**
+   * True track in degrees
+   */
+  virtual double trackDeg() = 0;
+  
+  /**
+   * Ground speed (along the track) in knots
+   */
+  virtual double groundSpeedKts() = 0;
+  
+  /**
+   * Vertical speed in ft/minute
+   */
+  virtual double vspeedFPM()= 0;
+  
+  /**
+   * Magnetic variation at current position
+   */
+  virtual double magvarDeg() = 0;
+  
+  /**
+   * device selected course (eg, from autopilot / MCP / OBS) in degrees
+   */
+  virtual double selectedMagCourse() = 0;
+
+  /**
+   * minimum distance to a waypoint for overflight sequencing. 
+   */
+  virtual double overflightArmDistanceM() = 0;
+};
+
+class WayptController
+{
+public:
+  virtual ~WayptController();
+  
+  virtual void init();
+
+  virtual void update() = 0;
+
+  /**
+   * Compute time until the waypoint is done
+   */
+  virtual double timeToWaypt() const;
+
+  /**
+   * Compute distance until the waypoint is done
+   */
+  virtual double distanceToWayptM() const = 0;
+
+  /**
+   * Bearing to the waypoint, if this value is meaningful.
+   * Default implementation returns the target track
+   */
+  virtual double trueBearingDeg() const
+    { return _targetTrack; }
+
+  virtual double targetTrackDeg() const 
+    { return _targetTrack; }
+
+  virtual double xtrackErrorNm() const
+    { return 0.0; }
+
+  virtual double courseDeviationDeg() const
+    { return 0.0; }
+
+  /**
+   * Position associated with the waypt. For static waypoints, this is
+   * simply the waypoint position itself; for dynamic points, it's the
+   * estimated location at which the controller will be done.
+   */
+  virtual SGGeod position() const = 0;
+
+  /**
+   * Is this controller finished?
+   */
+  bool isDone() const
+    { return _isDone; }
+
+  /**
+   * to/from flag - true = to, false = from. Defaults to 'true' because
+   * nearly all waypoint controllers become done as soon as this value would
+   * become false.
+   */
+  virtual bool toFlag() const
+    { return true; }
+    
+  /**
+   * Static factory method, given a waypoint, return a controller bound
+   * to it, of the appropriate type
+   */
+  static WayptController* createForWaypt(RNAV* rnav, const WayptRef& aWpt);
+protected:
+  WayptController(RNAV* aRNAV, const WayptRef& aWpt) :
+    _waypt(aWpt),
+    _rnav(aRNAV),
+    _isDone(false)
+  { }
+  
+  WayptRef _waypt;
+  double _targetTrack;
+  RNAV* _rnav;
+  
+  void setDone();
+private:
+  bool _isDone;
+};
+
+/**
+ * Controller supports 'directTo' (DTO) navigation to a waypoint. This
+ * creates a course from a starting point, to the waypoint, and reports
+ * deviation from that course.
+ *
+ * The controller is done when the waypoint is reached (to/from goes to 'from')
+ */
+class DirectToController : public WayptController
+{
+public:
+  DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin);
+  
+  virtual void init();
+  virtual void update();
+  virtual double distanceToWayptM() const;  
+  virtual double xtrackErrorNm() const;  
+  virtual double courseDeviationDeg() const;  
+  virtual double trueBearingDeg() const;
+  virtual SGGeod position() const;
+private:
+  SGGeod _origin;
+  double _distanceM;
+  double _courseDev;
+};
+
+/**
+ *
+ */
+class OBSController : public WayptController
+{
+public:
+  OBSController(RNAV* aRNAV, const WayptRef& aWpt);
+  
+  virtual void init();
+  virtual void update();
+  virtual double distanceToWayptM() const;  
+  virtual double xtrackErrorNm() const;  
+  virtual double courseDeviationDeg() const;  
+  virtual double trueBearingDeg() const;
+  virtual bool toFlag() const;
+  virtual SGGeod position() const;
+private:
+  double _distanceM;
+  double _courseDev;
+};
+
+} // of namespace flightgear
+
+#endif

src/Instrumentation/testgps.cxx

@@ -1,4 +1,9 @@
 
+#include <fstream>
+
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/structure/exception.hxx>
+
 #include <Main/fg_init.hxx>
 #include <Main/globals.hxx>
 #include <Main/fg_props.hxx>
@@ -6,8 +11,12 @@
 #include <Instrumentation/gps.hxx>
 #include <Autopilot/route_mgr.hxx>
 #include <Environment/environment_mgr.hxx>
+#include <Navaids/airways.hxx>
+#include <Navaids/waypoint.hxx>
+#include <Navaids/procedure.hxx>
 
 using std::string;
+using namespace flightgear;
 
 char *homedir = ::getenv( "HOME" );
 char *hostname = ::getenv( "HOSTNAME" );
@@ -45,6 +54,16 @@ void printScratch(SGPropertyNode* scratch)
   }
 }
 
+void printRoute(const WayptVec& aRoute)
+{
+  SG_LOG(SG_GENERAL, SG_INFO, "route size=" << aRoute.size());
+  for (unsigned int r=0; r<aRoute.size();++r) {
+    Waypt* w = aRoute[r];
+    SG_LOG(SG_GENERAL, SG_ALERT, "\t" << r << ": " << w->ident() << " "
+      << w->owner()->ident());
+  }
+}
+
 void createDummyRoute(FGRouteMgr* rm)
 {
   SGPropertyNode* rmInput = fgGetNode("/autopilot/route-manager/input", true);
@@ -59,6 +78,8 @@ void createDummyRoute(FGRouteMgr* rm)
 
 int main(int argc, char* argv[])
 {
+
+try{
   globals = new FGGlobals;
     
   fgInitFGRoot(argc, argv);
@@ -69,6 +90,9 @@ int main(int argc, char* argv[])
   
   
   fgInitNav();
+  fgSetDouble("/environment/magnetic-variation-deg", 0.0);
+  
+  Airway::load();
   
   SG_LOG(SG_GENERAL, SG_ALERT, "hello world!");
   
@@ -94,12 +118,15 @@ int main(int argc, char* argv[])
  // globals->add_subsystem("environment", envMgr);
  // envMgr->init();
   
+  fgSetBool("/sim/realism/simple-gps", true);
+  
+  // _realismSimpleGps
   
   SGPropertyNode* nd = fgGetNode("/instrumentation/gps", true);
   GPS* gps = new GPS(nd);
   globals->add_subsystem("gps", gps);
 
-  
+  const FGAirport* egph = fgFindAirportID("EGPH");
   testSetPosition(egph->geod());
   
   // startup the route manager
@@ -116,6 +143,7 @@ int main(int argc, char* argv[])
   // update a few times
   gps->update(0.05);
   gps->update(0.05);
+  gps->update(0.05);
   
   scratch->setStringValue("query", "TL");
   scratch->setStringValue("type", "Vor");
@@ -221,7 +249,105 @@ int main(int argc, char* argv[])
   nd->setStringValue("command", "define-user-wpt");
   printScratch(scratch);
   
+// airways
+  FGPositioned::TypeFilter vorFilt(FGPositioned::VOR);
+  FGPositionedRef tla = FGPositioned::findClosestWithIdent("TLA", pos, &vorFilt);
+  FGPositionedRef big = FGPositioned::findClosestWithIdent("BIG", pos, &vorFilt); 
+  FGPositionedRef pol = FGPositioned::findClosestWithIdent("POL", pos, &vorFilt);
+   
+  const FGAirport* eddm = fgFindAirportID("EDDM");
+  FGPositionedRef mun = FGPositioned::findClosestWithIdent("MUN", 
+    eddm->geod(), &vorFilt);
+  
+  const FGAirport* ksfo = fgFindAirportID("KSFO");
+  FGPositionedRef sfo = FGPositioned::findClosestWithIdent("SFO", 
+    ksfo->geod(), &vorFilt);
+
+
+  WayptRef awy1 = new NavaidWaypoint(tla, NULL);
+  WayptRef awy2 = new NavaidWaypoint(big, NULL);
+  WayptRef awy3 = new NavaidWaypoint(pol, NULL);
+  WayptRef awy4 = new NavaidWaypoint(mun, NULL);
+  WayptRef awy5 = new NavaidWaypoint(sfo, NULL);
+  
+  WayptRef awy6 = new NavaidWaypoint(
+    (FGPositioned*) fgFindAirportID("KJFK"), NULL);
+  
+  SGPath p("/Users/jmt/Desktop/airways.kml");
+  std::fstream f;
+  f.open(p.str().c_str(), fstream::out | fstream::trunc);
+  
+// pre-amble
+  f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+      "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
+    "<Document>\n";
+
+  WayptVec route;
+  Airway::highLevel()->route(awy1, awy3, route);
+  Route::dumpRouteToLineString("egph-egcc", route, f);
+  
+  Airway::lowLevel()->route(awy1, awy2, route);
+  Route::dumpRouteToLineString("egph-big", route, f);
+  
+  Airway::lowLevel()->route(awy2, awy4, route);
+  Route::dumpRouteToLineString("big-mun", route, f);
+  
+  Airway::highLevel()->route(awy4, awy5, route);
+  Route::dumpRouteToLineString("mun-sfo", route, f);
+  
+  Airway::lowLevel()->route(awy5, awy6, route);
+  Route::dumpRouteToLineString("sfo-jfk", route, f);
+  
+  // post-amble
+  f << "</Document>\n" 
+    "</kml>" << endl;
+  f.close();
+  
+// procedures
+  SGPath op("/Users/jmt/Desktop/procedures.kml");
+  f.open(op.str().c_str(), fstream::out | fstream::trunc);
+  
+  FGAirport* eham = (FGAirport*) fgFindAirportID("EHAM");
+  FGPositioned::TypeFilter fixFilt(FGPositioned::FIX);
+  
+  WayptVec approach;
+  FGPositionedRef redfa = FGPositioned::findClosestWithIdent("REDFA", 
+    eham->geod(), &fixFilt);
+  bool ok = eham->buildApproach(new NavaidWaypoint(redfa, NULL), 
+    eham->getRunwayByIdent("18R"), approach);
+  if (!ok ) {
+    SG_LOG(SG_GENERAL, SG_INFO, "failed to build approach");
+  }
+  
+  
+  FGAirport* egll = (FGAirport*) fgFindAirportID("EGLL");  
+  WayptVec approach2;
+  ok = egll->buildApproach(new NavaidWaypoint(big, NULL), 
+    egll->getRunwayByIdent("27R"), approach2);
+  if (!ok ) {
+    SG_LOG(SG_GENERAL, SG_INFO, "failed to build approach");
+  }
+  
+// pre-amble
+  f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+      "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
+    "<Document>\n";
+    
+  Route::dumpRouteToLineString("REDFA 18R", approach, f);
+  Route::dumpRouteToLineString("EGLL 27R", approach2, f);
+  
+  // post-amble
+  f << "</Document>\n" 
+    "</kml>" << endl;
+  f.close();  
   
              
   return EXIT_SUCCESS;
+
+
+  
+} catch (sg_exception& ex) {
+  SG_LOG(SG_GENERAL, SG_ALERT, "exception:" << ex.getFormattedMessage());
+}
+  return EXIT_FAILURE;
 }

src/Main/fg_init.cxx

@@ -58,6 +58,8 @@
 #include <simgear/structure/event_mgr.hxx>
 #include <simgear/misc/sg_path.hxx>
 #include <simgear/misc/sg_dir.hxx>
+#include <simgear/misc/sgstream.hxx>
+
 #include <simgear/misc/interpolator.hxx>
 #include <simgear/scene/material/matlib.hxx>
 #include <simgear/scene/model/particles.hxx>
@@ -97,6 +99,7 @@
 #include <Navaids/navlist.hxx>
 #include <Navaids/fix.hxx>
 #include <Navaids/fixlist.hxx>
+#include <Navaids/airways.hxx>
 #include <Scenery/scenery.hxx>
 #include <Scenery/tilemgr.hxx>
 #include <Scripting/NasalSys.hxx>
@@ -1071,14 +1074,7 @@ fgInitNav ()
     fixlist.init( p_fix );  // adds fixes to the DB in positioned.cxx
 
     SG_LOG(SG_GENERAL, SG_INFO, "  Airways");
-    SGPath p_awy( globals->get_fg_root() );
+    flightgear::Airway::load();
-    p_awy.append( "Navaids/awy.dat" );
-    FGAirwayNetwork *awyNet = new FGAirwayNetwork;
-    //cerr << "Loading Airways" << endl;
-    awyNet->load (p_awy );
-    awyNet->init();
-    //cerr << "initializing airways" << endl;
-    globals->set_airwaynet( awyNet );
     
     return true;
 }

src/Main/globals.cxx

@@ -45,7 +45,6 @@
 #include <Model/acmodel.hxx>
 #include <Model/modelmgr.hxx>
 #include <MultiPlayer/multiplaymgr.hxx>
-#include <Navaids/awynet.hxx>
 #include <Scenery/scenery.hxx>
 #include <Scenery/tilemgr.hxx>
 #include <Navaids/navlist.hxx>
@@ -149,9 +148,7 @@ FGGlobals::FGGlobals() :
     dmelist( NULL ),
     tacanlist( NULL ),
     carrierlist( NULL ),
-    channellist( NULL ),
+    channellist( NULL )    
-    airwaynet( NULL )
-    
 {
   simgear::ResourceManager::instance()->addProvider(new AircraftResourceProvider());
 }
@@ -199,7 +196,6 @@ FGGlobals::~FGGlobals()
     delete tacanlist;
     delete carrierlist;
     delete channellist;
-    delete airwaynet;
 
     soundmgr->unbind();
     delete soundmgr;

src/Main/globals.hxx

@@ -60,7 +60,6 @@ class FGAircraftModel;
 class FGControls;
 class FGFlightPlanDispatcher;
 class FGNavList;
-class FGAirwayNetwork;
 class FGTACANList;
 class FGModelMgr;
 class FGRouteMgr;
@@ -171,7 +170,6 @@ private:
     FGNavList *tacanlist;
     FGNavList *carrierlist;
     FGTACANList *channellist;
-    FGAirwayNetwork *airwaynet;
 
     /// roots of Aircraft trees
     string_list fg_aircraft_dirs;
@@ -322,10 +320,6 @@ public:
     inline FGTACANList *get_channellist() const { return channellist; }
     inline void set_channellist( FGTACANList *c ) { channellist = c; }
 
-    inline FGAirwayNetwork *get_airwaynet() const { return airwaynet; }
-    inline void set_airwaynet( FGAirwayNetwork *a ) { airwaynet = a; }
-
-
    /**
      * Save the current state as the initial state.
      */

src/Navaids/Makefile.am

@@ -5,11 +5,15 @@ noinst_LIBRARIES = libNavaids.a
 libNavaids_a_SOURCES = \
 	navdb.hxx navdb.cxx \
 	fix.hxx fixlist.hxx fixlist.cxx \
-	awynet.hxx awynet.cxx \
 	navrecord.hxx navrecord.cxx \
 	navlist.hxx navlist.cxx \
 	positioned.hxx positioned.cxx \
-	markerbeacon.hxx markerbeacon.cxx
+	markerbeacon.hxx markerbeacon.cxx \
+	routePath.hxx routePath.cxx \
+	airways.hxx airways.cxx \
+	route.hxx route.cxx \
+	waypoint.hxx waypoint.cxx \
+	procedure.hxx procedure.cxx
 
 #
 # testnavs_SOURCES = testnavs.cxx

src/Navaids/airways.cxx

@@ -0,0 +1,454 @@
+// airways.cxx - storage of airways network, and routing between nodes
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#ifndef HAVE_CONFIG_H
+# include "config.h"
+#endif
+
+#include "airways.hxx"
+
+#include <algorithm>
+#include <set>
+
+#include <simgear/sg_inlines.h>
+#include <simgear/structure/exception.hxx>
+#include <simgear/misc/sgstream.hxx>
+#include <simgear/misc/sg_path.hxx>
+
+#include <boost/tuple/tuple.hpp>
+
+#include <Main/globals.hxx>
+#include <Navaids/positioned.hxx>
+#include <Navaids/waypoint.hxx>
+
+using std::make_pair;
+using std::string;
+using std::set;
+using std::vector;
+
+#define DEBUG_AWY_SEARCH 1
+
+namespace flightgear
+{
+
+Airway::Network* Airway::static_lowLevel = NULL;
+Airway::Network* Airway::static_highLevel = NULL;
+
+//////////////////////////////////////////////////////////////////////////////
+
+/**
+ * information about an edge in the network.
+ * Some of this information is computed lazily
+ */
+class AdjacentWaypoint 
+{
+public:
+  AdjacentWaypoint(const FGPositionedRef& aOrigin,
+    const FGPositionedRef& aDest, Airway* aWay); 
+
+  double distanceM() const;
+  
+  const FGPositionedRef& other(const FGPositionedRef& aEnd) const;
+  
+  const FGPositionedRef origin;
+  const FGPositionedRef destination;
+  const Airway* airway;
+  
+private:
+  void validate() const;
+  mutable double _distanceM;
+};
+
+class AStarOpenNode : public SGReferenced
+{
+public:
+  AStarOpenNode(FGPositionedRef aNode, double aLegDist, 
+    Airway* aAirway,
+    FGPositionedRef aDest, AStarOpenNode* aPrev) :
+    node(aNode),
+    airway(aAirway),
+    previous(aPrev)
+  { 
+    distanceFromStart = aLegDist;
+    if (previous) {
+      distanceFromStart +=  previous->distanceFromStart;
+    }
+    
+		directDistanceToDestination = SGGeodesy::distanceM(node->geod(), aDest->geod());
+  }
+  
+  virtual ~AStarOpenNode()
+  {
+  }
+  
+  FGPositionedRef node;
+  Airway* airway;
+  SGSharedPtr<AStarOpenNode> previous;
+  double distanceFromStart; // aka 'g(x)'
+  double directDistanceToDestination; // aka 'h(x)'
+  
+  /**
+	 * aka 'f(x)'
+	 */
+	double totalCost() const {
+		return distanceFromStart + directDistanceToDestination;
+	}
+};
+
+typedef SGSharedPtr<AStarOpenNode> AStarOpenNodeRef;
+
+////////////////////////////////////////////////////////////////////////////
+
+Airway::Network* Airway::lowLevel()
+{
+  return static_lowLevel;
+}
+
+Airway::Network* Airway::highLevel()
+{
+  return static_highLevel;
+}
+
+Airway::Airway(const std::string& aIdent, double aTop, double aBottom) :
+  _ident(aIdent),
+  _topAltitudeFt(aTop),
+  _bottomAltitudeFt(aBottom)
+{
+}
+
+void Airway::load()
+{
+  static_lowLevel = new Network;
+  static_highLevel = new Network;
+
+  SGPath path( globals->get_fg_root() );
+  path.append( "Navaids/awy.dat" );
+    
+  std::string identStart, identEnd, name;
+  double latStart, lonStart, latEnd, lonEnd;
+  int type, base, top;
+  //int airwayIndex = 0;
+  //FGNode *n;
+
+  sg_gzifstream in( path.str() );
+  if ( !in.is_open() ) {
+    SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() );
+    throw sg_io_exception("Could not open airways data", sg_location(path.str()));
+  }
+// toss the first two lines of the file
+  in >> skipeol;
+  in >> skipeol;
+
+// read in each remaining line of the file
+  while (!in.eof()) {
+    in >> identStart;
+
+    if (identStart == "99") {
+      break;
+    }
+    
+    in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
+    in >> skipeol;
+
+    // type = 1; low-altitude
+    // type = 2; high-altitude
+    Network* net = (type == 1) ? static_lowLevel : static_highLevel;
+  
+    SGGeod startPos(SGGeod::fromDeg(lonStart, latStart)),
+      endPos(SGGeod::fromDeg(lonEnd, latEnd));
+    
+    Airway* awy = net->findAirway(name, top, base);
+    net->addEdge(awy, startPos, identStart, endPos, identEnd);
+  } // of file line iteration
+}
+
+Airway* Airway::Network::findAirway(const std::string& aName, double aTop, double aBase)
+{
+  AirwayDict::iterator it = _airways.find(aName);
+  if (it == _airways.end()) {
+    Airway* awy = new Airway(aName, aTop, aBase);
+    it = _airways.insert(it, make_pair(aName, awy));
+  }
+  
+  return it->second;
+}
+
+void Airway::Network::addEdge(Airway* aWay, const SGGeod& aStartPos, 
+  const std::string& aStartIdent, 
+  const SGGeod& aEndPos, const std::string& aEndIdent)
+{
+  FGPositionedRef start = FGPositioned::findClosestWithIdent(aStartIdent, aStartPos);
+  FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos);
+    
+  if (!start) {
+    SG_LOG(SG_GENERAL, SG_INFO, "unknown airways start pt: '" << aStartIdent << "'");
+    start = FGPositioned::createUserWaypoint(aStartIdent, aStartPos);
+    return;
+  }
+  
+  if (!end) {
+    SG_LOG(SG_GENERAL, SG_INFO, "unknown airways end pt: '" << aEndIdent << "'");
+    end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos);
+    return;
+  }
+  
+  AdjacentWaypoint* edge = new AdjacentWaypoint(start, end, aWay);
+  _graph.insert(make_pair(start, edge));
+  _graph.insert(make_pair(end, edge));
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+bool Airway::Network::inNetwork(const FGPositioned* aPos) const
+{
+  FGPositioned* pos = const_cast<FGPositioned*>(aPos);
+  return (_graph.find(pos) != _graph.end());
+}
+
+bool Airway::Network::route(WayptRef aFrom, WayptRef aTo, 
+  WayptVec& aPath)
+{
+  if (!aFrom || !aTo) {
+    throw sg_exception("invalid waypoints to route between");
+  }
+  
+// find closest nodes on the graph to from/to
+// if argument waypoints are directly on the graph (which is frequently the
+// case), note this so we don't duplicate them in the output.
+
+  FGPositionedRef from, to;
+  bool exactTo, exactFrom;
+  boost::tie(from, exactFrom) = findClosestNode(aFrom);
+  boost::tie(to, exactTo) = findClosestNode(aTo);
+  
+#ifdef DEBUG_AWY_SEARCH
+  SG_LOG(SG_GENERAL, SG_INFO, "from:" << from->ident() << "/" << from->name());
+  SG_LOG(SG_GENERAL, SG_INFO, "to:" << to->ident() << "/" << to->name());
+#endif
+
+  bool ok = search2(from, to, aPath);
+  if (!ok) {
+    return false;
+  }
+  
+  if (exactTo) {
+    aPath.pop_back();
+  }
+  
+  if (exactFrom) {
+    // edge case - if from and to are equal, which can happen, don't
+    // crash here. This happens routing EGPH -> EGCC; 'DCS' is common
+    // to the EGPH departure and EGCC STAR.
+    if (!aPath.empty()) {
+      aPath.erase(aPath.begin());
+    }
+  }
+  
+  return true;
+}
+
+std::pair<FGPositionedRef, bool> 
+Airway::Network::findClosestNode(WayptRef aRef)
+{
+  return findClosestNode(aRef->position());
+}
+
+class InAirwayFilter : public FGPositioned::Filter
+{
+public:
+  InAirwayFilter(Airway::Network* aNet) :
+    _net(aNet)
+  { ; }
+  
+  virtual bool pass(FGPositioned* aPos) const
+  {
+    return _net->inNetwork(aPos);
+  }
+  
+  virtual FGPositioned::Type minType() const
+  { return FGPositioned::WAYPOINT; }
+  
+  virtual FGPositioned::Type maxType() const
+  { return FGPositioned::NDB; }
+  
+private:
+  Airway::Network* _net;
+};
+
+std::pair<FGPositionedRef, bool> 
+Airway::Network::findClosestNode(const SGGeod& aGeod)
+{
+  InAirwayFilter f(this);
+  FGPositionedRef r = FGPositioned::findClosest(aGeod, 800.0, &f);
+  bool exact = false;
+  
+  if (r && (SGGeodesy::distanceM(aGeod, r->geod()) < 100.0)) {
+    exact = true; // within 100 metres, let's call that exact
+  }
+  
+  return make_pair(r, exact);
+}
+
+/////////////////////////////////////////////////////////////////////////////
+
+typedef vector<AStarOpenNodeRef> OpenNodeHeap;
+
+static void buildWaypoints(AStarOpenNodeRef aNode, WayptVec& aRoute)
+{
+// count the route length, and hence pre-size aRoute
+  int count = 0;
+  AStarOpenNodeRef n = aNode;
+  for (; n != NULL; ++count, n = n->previous) {;}
+  aRoute.resize(count);
+  
+// run over the route, creating waypoints
+  for (n = aNode; n; n=n->previous) {
+    aRoute[--count] = new NavaidWaypoint(n->node, n->airway);
+  }
+}
+
+/**
+ * Inefficent (linear) helper to find an open node in the heap
+ */
+static AStarOpenNodeRef
+findInOpen(const OpenNodeHeap& aHeap, FGPositioned* aPos)
+{
+  for (unsigned int i=0; i<aHeap.size(); ++i) {
+    if (aHeap[i]->node == aPos) {
+      return aHeap[i];
+    }
+  }
+  
+  return NULL;
+}
+
+class HeapOrder
+{
+public:
+  bool operator()(AStarOpenNode* a, AStarOpenNode* b)
+  {
+    return a->totalCost() > b->totalCost();
+  }
+};
+
+bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
+  WayptVec& aRoute)
+{
+  
+  typedef set<FGPositioned*> ClosedNodeSet;
+  typedef std::pair<AdjacencyMap::iterator,AdjacencyMap::iterator> AdjacencyMapRange;
+  
+  OpenNodeHeap openNodes;
+  ClosedNodeSet closedNodes;
+  HeapOrder ordering;
+  
+  openNodes.push_back(new AStarOpenNode(aStart, 0.0, NULL, aDest, NULL));
+  
+// A* open node iteration
+  while (!openNodes.empty()) {
+    std::pop_heap(openNodes.begin(), openNodes.end(), ordering);
+    AStarOpenNodeRef x = openNodes.back();
+    FGPositioned* xp = x->node;    
+    openNodes.pop_back();
+    closedNodes.insert(xp);
+    
+  //  SG_LOG(SG_GENERAL, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost());
+    
+  // check if xp is the goal; if so we're done, since there cannot be an open
+  // node with lower f(x) value.
+    if (xp == aDest) {
+      buildWaypoints(x, aRoute);
+      return true;
+    }
+    
+  // adjacent (neighbour) iteration
+    AdjacencyMapRange r(_graph.equal_range(xp));
+    for (; r.first != r.second; ++r.first) {
+      AdjacentWaypoint* adj(r.first->second);
+      FGPositioned* yp = adj->other(xp);
+      if (closedNodes.count(yp)) {
+        continue; // closed, ignore
+      }
+
+      AStarOpenNodeRef y = findInOpen(openNodes, yp);
+      if (y) { // already open
+        double g = x->distanceFromStart + adj->distanceM();
+        if (g > y->distanceFromStart) {
+          // worse path, ignore
+          //SG_LOG(SG_GENERAL, SG_INFO, "\tabandoning " << yp->ident() <<
+          // " path is worse: g(y)" << y->distanceFromStart << ", g'=" << g);
+          continue;
+        }
+        
+      // we need to update y. Unfortunately this means rebuilding the heap,
+      // since y's score can change arbitrarily
+        //SG_LOG(SG_GENERAL, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g);
+        y->previous = x;
+        y->distanceFromStart = g;
+        y->airway = (Airway*) adj->airway;
+        std::make_heap(openNodes.begin(), openNodes.end(), ordering);
+      } else { // not open, insert a new node for y into the heap
+        y = new AStarOpenNode(yp, adj->distanceM(), 
+          (Airway*) adj->airway, aDest, x);
+        //SG_LOG(SG_GENERAL, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost());
+        openNodes.push_back(y);
+        std::push_heap(openNodes.begin(), openNodes.end(), ordering);
+      }
+    } // of neighbour iteration
+  } // of open node iteration
+  
+  SG_LOG(SG_GENERAL, SG_INFO, "A* failed to find route");
+  return false;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// AdjacentWaypoint definitions 
+
+AdjacentWaypoint::AdjacentWaypoint(
+  const FGPositionedRef& aOrigin, const FGPositionedRef& aDest, Airway* aWay) :
+  origin(aOrigin),
+  destination(aDest), 
+  airway(aWay),
+  _distanceM(-1.0)
+{
+
+}
+
+const FGPositionedRef&
+AdjacentWaypoint::other(const FGPositionedRef& aEnd) const
+{
+  return (aEnd == origin) ? destination : origin;
+}
+
+double AdjacentWaypoint::distanceM() const
+{
+  validate();
+  return _distanceM;
+}
+      
+void AdjacentWaypoint::validate() const
+{
+  if (_distanceM > 0.0) {
+    return; // already validated
+  }
+  
+  _distanceM = SGGeodesy::distanceM(origin->geod(), destination->geod());
+}
+
+} // of namespace flightgear

src/Navaids/airways.hxx

@@ -0,0 +1,134 @@
+// airways.hxx - storage of airways network, and routing between nodes
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#ifndef FG_AIRWAYS_HXX
+#define FG_AIRWAYS_HXX
+
+#include <map>
+#include <vector>
+
+#include <Navaids/route.hxx>
+
+class FGPositioned;
+typedef SGSharedPtr<FGPositioned> FGPositionedRef; 
+
+namespace flightgear {
+
+// forward declare some helpers
+struct SearchContext;
+class AdjacentWaypoint;
+class InAirwayFilter;
+
+class Airway : public Route
+{
+public:
+  virtual std::string ident() const
+  { return _ident; }
+  
+  static void load();
+  
+  /**
+   * Track a network of airways
+   *
+   */
+  class Network
+  {
+  public:
+    friend class Airway;
+    friend class InAirwayFilter;
+    
+    
+  
+    /**
+     * Principal routing algorithm. Attempts to find the best route beween
+     * two points. If either point is part of the airway network (e.g, a SID
+     * or STAR transition), it will <em>not</em> be duplicated in the result
+     * path.
+     *
+     * Returns true if a route could be found, or false otherwise.
+     */
+    bool route(WayptRef aFrom, WayptRef aTo, WayptVec& aPath);
+  private:    
+    void addEdge(Airway* aWay, const SGGeod& aStartPos, 
+                const std::string& aStartIdent, 
+                const SGGeod& aEndPos, const std::string& aEndIdent);
+  
+    Airway* findAirway(const std::string& aName, double aTop, double aBase);
+    
+    typedef std::multimap<FGPositioned*, AdjacentWaypoint*> AdjacencyMap;
+    AdjacencyMap _graph;
+    
+    typedef std::vector<AdjacentWaypoint*> AdjacentWaypointVec;
+
+    typedef std::map<std::string, Airway*> AirwayDict;
+    AirwayDict _airways;
+
+    bool search2(FGPositionedRef aStart, FGPositionedRef aDest, WayptVec& aRoute);
+  
+    /**
+     * Test if a positioned item is part of this airway network or not.
+     */
+    bool inNetwork(const FGPositioned* aRef) const;
+    
+    /**
+     * Find the closest node on the network, to the specified waypoint
+     *
+     * May return NULL,false if no match could be found; the search is
+     * internally limited to avoid very poor performance; for example, 
+     * in the middle of an ocean.
+     *
+     * The second return value indicates if the returned value is
+     * equal (true) or distinct (false) to the input waypoint.
+     * Equality here means being physically within a close tolerance,
+     * on the order of a hundred metres.
+     */
+    std::pair<FGPositionedRef, bool> findClosestNode(WayptRef aRef);
+    
+    /**
+     * Overloaded version working with a raw SGGeod
+     */
+    std::pair<FGPositionedRef, bool> findClosestNode(const SGGeod& aGeod);
+  };
+
+
+  static Network* highLevel();
+  static Network* lowLevel();
+  
+private:
+  Airway(const std::string& aIdent, double aTop, double aBottom);
+
+  friend class Network;
+  
+  static Network* static_highLevel;
+  static Network* static_lowLevel;
+  
+  std::string _ident;
+  double _topAltitudeFt;
+  double _bottomAltitudeFt;
+
+  // high-level vs low-level flag
+  // ... ?
+  
+  WayptVec _elements;
+};
+
+} // of namespace flightgear
+
+
+#endif //of FG_AIRWAYS_HXX

src/Navaids/positioned.cxx

@@ -518,7 +518,9 @@ FGPositioned::Filter::passType(Type aTy) const
 
 static FGPositioned::List 
 findAll(const NamedPositionedIndex& aIndex, 
-                             const std::string& aName, FGPositioned::Filter* aFilter)
+                             const std::string& aName,
+                             FGPositioned::Filter* aFilter,
+                             bool aExact)
 {
   FGPositioned::List result;
   if (aName.empty()) {
@@ -526,9 +528,16 @@ findAll(const NamedPositionedIndex& aIndex,
   }
   
   std::string name = boost::to_upper_copy(aName);
-  std::string upperBoundId = name;
+  NamedPositionedIndex::const_iterator upperBound;
-  upperBoundId[upperBoundId.size()-1]++;
+  
-  NamedPositionedIndex::const_iterator upperBound = aIndex.lower_bound(upperBoundId);
+  if (aExact) {
+    upperBound = aIndex.upper_bound(name);
+  } else {
+    std::string upperBoundId = name;
+    upperBoundId[upperBoundId.size()-1]++;
+    upperBound = aIndex.lower_bound(upperBoundId);
+  }
+  
   NamedPositionedIndex::const_iterator it = aIndex.lower_bound(name);
   
   for (; it != upperBound; ++it) {
@@ -663,7 +672,7 @@ const char* FGPositioned::nameForType(Type aTy)
 FGPositionedRef
 FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
 {
-  FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter));
+  FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter, true));
   if (r.empty()) {
     return FGPositionedRef();
   }
@@ -682,15 +691,15 @@ FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilt
 }
 
 FGPositioned::List
-FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter)
+FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter, bool aExact)
 {
-  return findAll(global_identIndex, aIdent, aFilter);
+  return findAll(global_identIndex, aIdent, aFilter, aExact);
 }
 
 FGPositioned::List
-FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter)
+FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter, bool aExact)
 {
-  return findAll(global_nameIndex, aName, aFilter);
+  return findAll(global_nameIndex, aName, aFilter, aExact);
 }
 
 FGPositionedRef

src/Navaids/positioned.hxx

@@ -43,6 +43,7 @@ public:
     TAXIWAY,
     PAVEMENT,
     PARK_STAND,
+    WAYPOINT,
     FIX,
     VOR,
     NDB,
@@ -55,7 +56,6 @@ public:
     DME,
     TACAN,
     OBSTACLE,
-    WAYPOINT, // user-defined waypoint
     FREQ_GND,
     FREQ_TWR,
     FREQ_ATIS,
@@ -160,12 +160,12 @@ public:
    * Find all items with the specified ident
    * @param aFilter - optional filter on items
    */
-  static List findAllWithIdent(const std::string& aIdent, Filter* aFilter = NULL);
+  static List findAllWithIdent(const std::string& aIdent, Filter* aFilter = NULL, bool aExact = true);
   
   /**
    * As above, but searches names instead of idents
    */
-  static List findAllWithName(const std::string& aName, Filter* aFilter = NULL);
+  static List findAllWithName(const std::string& aName, Filter* aFilter = NULL, bool aExact = true);
   
   /**
    * Sort an FGPositionedList by distance from a position

src/Navaids/procedure.cxx

@@ -0,0 +1,336 @@
+// procedure.cxx - define route storing an approach, arrival or departure procedure
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#include "procedure.hxx"
+
+#include <cassert>
+#include <algorithm> // for reverse_copy
+
+#include <simgear/structure/exception.hxx>
+
+#include <Navaids/waypoint.hxx>
+
+using std::string;
+
+namespace flightgear
+{
+
+Procedure::Procedure(const string& aIdent) :
+  _ident(aIdent)
+{
+}
+
+Approach::Approach(const string& aIdent) : 
+  Procedure(aIdent)
+{
+
+}
+
+void Approach::setRunway(FGRunwayRef aRwy)
+{
+  _runway = aRwy;
+}
+
+void Approach::setPrimaryAndMissed(const WayptVec& aPrimary, const WayptVec& aMissed)
+{
+  _primary = aPrimary;
+  _primary[0]->setFlag(WPT_IAF, true);
+  _primary[_primary.size()-1]->setFlag(WPT_FAF, true);
+  
+  _missed = aMissed;
+  
+  if (!_missed.empty()) {
+    // mark the first point as the published missed-approach point
+    _missed[0]->setFlag(WPT_MAP, true);
+  
+    // mark all the points as being on the missed approach route
+    for (unsigned int i=0; i<_missed.size(); ++i) {
+      _missed[i]->setFlag(WPT_MISS, true);
+    }
+  }
+}
+
+void Approach::addTransition(Transition* aTrans)
+{
+  WayptRef entry = aTrans->enroute();
+  _transitions[entry] = aTrans;
+}
+
+bool Approach::route(WayptRef aIAF, WayptVec& aWps)
+{
+  WptTransitionMap::iterator it;
+  bool haveTrans = false;
+  for (it = _transitions.begin(); it != _transitions.end(); ++it) {
+    Transition* t= it->second;
+    if (t->route(aIAF, aWps)) {
+    haveTrans = true;
+      break;
+    }
+  } // of transitions iteration
+  
+  if (!haveTrans) {
+    SG_LOG(SG_GENERAL, SG_INFO, "approach " << ident() << " has no transition " <<
+      "for IAF: " << aIAF->ident());
+    return false;
+  }
+  
+  aWps.insert(aWps.end(), _primary.begin(), _primary.end());
+  aWps.push_back(new RunwayWaypt(_runway, NULL));
+  aWps.insert(aWps.end(), _missed.begin(), _missed.end());
+  return true;
+}
+
+bool Approach::routeFromVectors(WayptVec& aWps)
+{
+  aWps.insert(aWps.end(), _primary.begin(), _primary.end());
+  aWps.push_back(new RunwayWaypt(_runway, NULL));
+  aWps.insert(aWps.end(), _missed.begin(), _missed.end());
+  return true;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+ArrivalDeparture::ArrivalDeparture(const string& aIdent) :
+    Procedure(aIdent)
+{
+}
+
+void ArrivalDeparture::addRunway(FGRunwayRef aWay)
+{
+  _runways[aWay] = NULL;
+}
+
+bool ArrivalDeparture::isForRunway(FGRunwayRef aWay) const
+{
+  // null runway always passes
+  if (!aWay) {
+    return true;
+  }
+  
+  return (_runways.count(aWay));
+}
+
+void ArrivalDeparture::addTransition(Transition* aTrans)
+{
+  WayptRef entry = aTrans->enroute();
+  _enrouteTransitions[entry] = aTrans;
+}
+
+void ArrivalDeparture::addRunwayTransition(FGRunwayRef aWay, Transition* aTrans)
+{
+  assert(aWay->ident() == aTrans->ident());
+  if (!isForRunway(aWay)) {
+    throw sg_io_exception("adding transition for unspecified runway:" + aWay->ident(), ident());
+  }
+  
+  _runways[aWay] = aTrans;
+}
+
+void ArrivalDeparture::setCommon(const WayptVec& aWps)
+{
+  _common = aWps;
+}
+
+bool ArrivalDeparture::commonRoute(Waypt* aEnroute, WayptVec& aPath, FGRunwayRef aRwy)
+{
+  // assume we're routing from enroute, to the runway.
+  // for departures, we'll flip the result points
+
+  Transition* t = findTransitionByEnroute(aEnroute);
+  WayptVec::iterator firstCommon = _common.begin();
+  if (t) {
+    t->route(aEnroute, aPath);
+
+    Waypt* transEnd = t->procedureEnd();
+    for (; firstCommon != _common.end(); ++firstCommon) {
+      if ((*firstCommon)->matches(transEnd)) {
+        // found transition->common point, stop search
+        break;
+      }
+    } // of common points
+    
+    // if we hit this point, the transition doesn't end (start, for a SID) on
+    // a common point. We assume this means we should just append the entire
+    // common section after the transition.
+    firstCommon = _common.begin();
+  } else {
+    if (aEnroute && !(*firstCommon)->matches(aEnroute)) {
+      return false;
+    }
+  } // of not using a transition
+  
+  // append (some) common points
+  aPath.insert(aPath.end(), firstCommon, _common.end());
+  
+  if (!aRwy) {
+    // no runway specified, we're done
+    return true;
+  }
+  
+  RunwayTransitionMap::iterator r = _runways.find(aRwy);
+  assert(r != _runways.end());
+  if (!r->second) {
+    // no transitions specified. Not great, but not
+    // much we can do about it. Calling code will insert VECTORS to the approach
+    // if required, or maybe there's an approach transition defined.
+    return true;
+  }
+  
+  SG_LOG(SG_GENERAL, SG_INFO, ident() << " using runway transition for " << r->first->ident());
+  r->second->route(NULL, aPath);
+  return true;
+}
+
+Transition* ArrivalDeparture::findTransitionByEnroute(Waypt* aEnroute) const
+{
+  if (!aEnroute) {
+    return NULL;
+  }
+  
+  WptTransitionMap::const_iterator eit;
+  for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
+    if (eit->second->enroute()->matches(aEnroute)) {
+      SG_LOG(SG_GENERAL, SG_INFO, ident() << " using enroute transition " << eit->second->ident());
+      return eit->second;
+    }
+  } // of enroute transition iteration
+  
+  return NULL;
+}
+
+WayptRef ArrivalDeparture::findBestTransition(const SGGeod& aPos) const
+{
+  // no transitions, that's easy
+  if (_enrouteTransitions.empty()) {
+    SG_LOG(SG_GENERAL, SG_INFO, "no enroute transitions for " << ident());
+    return _common.front();
+  }
+  
+  double d = 1e9;
+  WayptRef w;
+  WptTransitionMap::const_iterator eit;
+  for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
+    WayptRef c = eit->second->enroute();
+    SG_LOG(SG_GENERAL, SG_INFO, "findBestTransition for " << ident() << ", looking at " << c->ident());
+    // assert(c->hasFixedPosition());
+    double cd = SGGeodesy::distanceM(aPos, c->position());
+    
+    if (cd < d) { // distance to 'c' is less, new best match
+      d = cd;
+      w = c;
+    }
+  } // of transitions iteration
+  
+  assert(w);
+  return w;
+}
+
+WayptRef ArrivalDeparture::findTransitionByName(const string& aIdent) const
+{
+  WptTransitionMap::const_iterator eit;
+  for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
+    WayptRef c = eit->second->enroute();
+    if (c->ident() == aIdent) {
+      return c;
+    }
+  }
+  
+  return NULL;
+}
+
+////////////////////////////////////////////////////////////////////////////
+
+SID::SID(const string& aIdent) :
+    ArrivalDeparture(aIdent)
+{
+}
+
+bool SID::route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath)
+{
+  if (!isForRunway(aWay)) {
+    SG_LOG(SG_GENERAL, SG_WARN, "SID " << ident() << " not for runway " << aWay->ident());
+    return false;
+  }
+  
+  WayptVec path;
+  if (!commonRoute(aEnroute, path, aWay)) {
+    return false;
+  }
+  
+  // SID waypoints (including transitions) are stored reversed, so we can
+  // re-use the routing code. This is where we fix the ordering for client code
+  std::back_insert_iterator<WayptVec> bi(aPath);
+  std::reverse_copy(path.begin(), path.end(), bi);
+
+  return true;
+}
+
+////////////////////////////////////////////////////////////////////////////
+
+STAR::STAR(const string& aIdent) :
+    ArrivalDeparture(aIdent)
+{
+}
+
+bool STAR::route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath)
+{
+  if (aWay && !isForRunway(aWay)) {
+    return false;
+  }
+    
+  return commonRoute(aEnroute, aPath, aWay);
+}
+
+/////////////////////////////////////////////////////////////////////////////
+
+Transition::Transition(const std::string& aIdent, Procedure* aPr, 
+    const WayptVec& aWps) :
+  _ident(aIdent),
+  _parent(aPr),
+  _primary(aWps)
+{
+  assert(aPr);
+  assert(!_primary.empty());
+  
+  _primary[0]->setFlag(WPT_TRANSITION, true);
+}
+
+WayptRef Transition::enroute() const
+{
+  assert(!_primary.empty());
+  return _primary[0];
+}
+
+WayptRef Transition::procedureEnd() const
+{
+  assert(!_primary.empty());
+  return _primary[_primary.size() - 1];
+}
+
+bool Transition::route(Waypt* aEnroute, WayptVec& aPath)
+{
+  if (aEnroute && !enroute()->matches(aEnroute)) {
+    return false;
+  }
+  
+  aPath.insert(aPath.end(), _primary.begin(), _primary.end());
+  return true;
+}
+
+} // of namespace

src/Navaids/procedure.hxx

@@ -0,0 +1,212 @@
+/// procedure.hxx - define route storing an approach, arrival or departure procedure
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#ifndef FG_NAVAID_PROCEDURE_HXX
+#define FG_NAVAID_PROCEDURE_HXX
+
+#include <set>
+
+#include <Navaids/route.hxx>
+#include <Airports/runways.hxx>
+
+typedef SGSharedPtr<FGRunway> FGRunwayRef;
+
+namespace flightgear {
+
+// forward decls
+class NavdataVisitor;
+
+class Procedure : public Route
+{
+public:
+
+  virtual std::string ident() const
+  { return _ident; }
+protected:
+  Procedure(const std::string& aIdent);
+  
+  std::string _ident;
+};
+
+/**
+ * Encapsulate a transition segment
+ */
+class Transition : public Route
+{
+public:
+  bool route(Waypt* aEnroute, WayptVec& aPath);
+  
+  Procedure* parent() const
+  { return _parent; }
+  
+  /**
+   * Return the enroute end of the transition
+   */
+  WayptRef enroute() const;
+  
+  /**
+   * Return the procedure end of the transition
+   */
+  WayptRef procedureEnd() const;
+  
+  virtual std::string ident() const
+  { return _ident; }
+private:
+  friend class NavdataVisitor;
+
+  Transition(const std::string& aIdent, Procedure* aPr, const WayptVec& aWps);
+
+  std::string _ident;
+  Procedure* _parent;
+  WayptVec _primary;
+};
+
+/**
+ * Describe an approach procedure, including the missed approach
+ * segment
+ */
+class Approach : public Procedure
+{
+public:
+  FGRunwayRef runway() 
+  { return _runway; }
+
+  /**
+   * Build a route from a valid IAF to the runway, including the missed
+   * segment. Return false if no valid transition from the specified IAF
+   * could be found
+   */
+  bool route(WayptRef aIAF, WayptVec& aWps);
+
+  /**
+   * Build route as above, but ignore transitions, and assume radar
+   * vectoring to the start of main approach
+   */
+  bool routeFromVectors(WayptVec& aWps);
+  
+  const WayptVec& primary() const
+    { return _primary; }
+    
+  const WayptVec& missed() const
+    { return _missed; }
+
+private:
+  friend class NavdataVisitor;
+  
+  Approach(const std::string& aIdent);
+  
+  void setRunway(FGRunwayRef aRwy);
+  void setPrimaryAndMissed(const WayptVec& aPrimary, const WayptVec& aMissed);
+  void addTransition(Transition* aTrans);
+  
+  FGRunwayRef _runway;
+  
+  typedef std::map<WayptRef, Transition*> WptTransitionMap;
+  WptTransitionMap _transitions;
+  
+  WayptVec _primary; // unify these?
+  WayptVec _missed;
+};
+
+class ArrivalDeparture : public Procedure
+{
+public:
+  /**
+   * Predicate, test if this procedure applies to the requested runway
+   */
+  virtual bool isForRunway(FGRunwayRef aWay) const;
+
+  /**
+   * Find a path between the runway and enroute structure. Waypoints 
+   * corresponding to the appropriate transitions and segments will be created.
+   */
+  virtual bool route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath) = 0;
+
+  const WayptVec& common() const
+    { return _common; }
+
+  /**
+   * Given an enroute location, find the best enroute transition point for 
+   * this arrival/departure. Best is currently determined as 'closest to the
+   * enroute location'.
+   */
+  WayptRef findBestTransition(const SGGeod& aPos) const;
+  
+  /**
+   * Find an enroute transition waypoint by identifier. This is necessary
+   * for the route-manager and similar code that that needs to talk about
+   * transitions in a human-meaningful way (including persistence).
+   */
+  WayptRef findTransitionByName(const std::string& aIdent) const;
+  
+  Transition* findTransitionByEnroute(Waypt* aEnroute) const;
+protected:
+    
+  bool commonRoute(Waypt* aEnroute, WayptVec& aPath, FGRunwayRef aRwy);
+  
+  ArrivalDeparture(const std::string& aIdent);
+  
+  
+  void addRunway(FGRunwayRef aRwy);
+
+  typedef std::map<FGRunwayRef, Transition*> RunwayTransitionMap;
+  RunwayTransitionMap _runways;
+  
+private:
+  friend class NavdataVisitor;
+  
+  void addTransition(Transition* aTrans);
+  
+  void setCommon(const WayptVec& aWps);
+
+  void addRunwayTransition(FGRunwayRef aRwy, Transition* aTrans);
+  
+  WayptVec _common;
+  
+  typedef std::map<WayptRef, Transition*> WptTransitionMap;
+  WptTransitionMap _enrouteTransitions;
+  
+  
+};
+
+class SID : public ArrivalDeparture
+{
+public:  
+  virtual bool route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath);
+  
+private:
+  friend class NavdataVisitor;
+    
+  SID(const std::string& aIdent);
+};
+
+class STAR : public ArrivalDeparture
+{
+public:  
+  virtual bool route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath);
+  
+private:
+  friend class NavdataVisitor;
+  
+  STAR(const std::string& aIdent);
+};
+
+} // of namespace
+
+#endif 

src/Navaids/route.cxx

@@ -0,0 +1,679 @@
+// route.cxx - classes supporting waypoints and route structures
+
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#ifdef HAVE_CONFIG_H
+# include "config.h"
+#endif
+
+#include "route.hxx"
+
+// std
+#include <map>
+#include <fstream>
+
+// Boost
+#include <boost/algorithm/string/case_conv.hpp>
+#include <boost/algorithm/string.hpp>
+
+// SimGear
+#include <simgear/structure/exception.hxx>
+#include <simgear/xml/easyxml.hxx>
+#include <simgear/misc/sg_path.hxx>
+
+// FlightGear
+#include <Navaids/procedure.hxx>
+#include <Navaids/waypoint.hxx>
+#include <Airports/simple.hxx>
+
+using std::string;
+using std::vector;
+using std::endl;
+using std::fstream;
+
+namespace flightgear {
+
+Waypt::Waypt(Route* aOwner) :
+  _altitudeFt(0.0),
+  _speedKts(0.0),
+  _altRestrict(RESTRICT_NONE),
+  _speedRestrict(RESTRICT_NONE),
+  _owner(aOwner),
+  _flags(0)
+{
+}
+
+std::string Waypt::ident() const
+{
+  return "";
+}
+  
+bool Waypt::flag(WayptFlag aFlag) const
+{
+  return (_flags & aFlag);
+}
+	
+void Waypt::setFlag(WayptFlag aFlag, bool aV)
+{
+  _flags = (_flags & ~aFlag);
+  if (aV) _flags |= aFlag;
+}
+
+bool Waypt::matches(Waypt* aOther) const
+{
+  assert(aOther);
+  if (ident() != aOther->ident()) { // cheap check first
+    return false;
+  }
+  
+  return matches(aOther->position());
+}
+
+
+bool Waypt::matches(const SGGeod& aPos) const
+{
+  double d = SGGeodesy::distanceM(position(), aPos);
+  return (d < 100.0); // 100 metres seems plenty
+}
+
+void Waypt::setAltitude(double aAlt, RouteRestriction aRestrict)
+{
+  _altitudeFt = aAlt;
+  _altRestrict = aRestrict;
+}
+
+void Waypt::setSpeed(double aSpeed, RouteRestriction aRestrict)
+{
+  _speedKts = aSpeed;
+  _speedRestrict = aRestrict;
+}
+
+std::pair<double, double>
+Waypt::courseAndDistanceFrom(const SGGeod& aPos) const
+{
+  if (flag(WPT_DYNAMIC)) {
+    return std::make_pair(0.0, 0.0);
+  }
+  
+  double course, az2, distance;
+  SGGeodesy::inverse(aPos, position(), course, az2, distance);
+  return std::make_pair(course, distance);
+}
+  
+///////////////////////////////////////////////////////////////////////////
+// persistence
+
+static RouteRestriction restrictionFromString(const char* aStr)
+{
+  std::string l = boost::to_lower_copy(std::string(aStr));
+
+  if (l == "at") return RESTRICT_AT;
+  if (l == "above") return RESTRICT_ABOVE;
+  if (l == "below") return RESTRICT_BELOW;
+  if (l == "none") return RESTRICT_NONE;
+  
+  if (l.empty()) return RESTRICT_NONE;
+  throw sg_io_exception("unknown restriction specification:" + l, 
+    "Route restrictFromString");
+}
+
+static const char* restrictionToString(RouteRestriction aRestrict)
+{
+  switch (aRestrict) {
+  case RESTRICT_AT: return "at";
+  case RESTRICT_BELOW: return "below";
+  case RESTRICT_ABOVE: return "above";
+  case RESTRICT_NONE: return "none";
+  default:
+    throw sg_exception("invalid route restriction",
+      "Route restrictToString");
+  }
+}
+
+Waypt* Waypt::createInstance(Route* aOwner, const std::string& aTypeName)
+{
+  Waypt* r = NULL;
+  if (aTypeName == "basic") {
+    r = new BasicWaypt(aOwner);
+  } else if (aTypeName == "navaid") {
+    r = new NavaidWaypoint(aOwner);
+  } else if (aTypeName == "offset-navaid") {
+    r = new OffsetNavaidWaypoint(aOwner);
+  } else if (aTypeName == "hold") {
+    r = new Hold(aOwner);
+  } else if (aTypeName == "runway") {
+    r = new RunwayWaypt(aOwner);
+  } else if (aTypeName == "hdgToAlt") {
+    r = new HeadingToAltitude(aOwner);
+  } else if (aTypeName == "dmeIntercept") {
+    r = new DMEIntercept(aOwner);
+  } else if (aTypeName == "radialIntercept") {
+    r = new RadialIntercept(aOwner);
+  } else if (aTypeName == "vectors") {
+    r = new ATCVectors(aOwner);
+  } 
+
+  if (!r || (r->type() != aTypeName)) {
+    throw sg_exception("broken factory method for type:" + aTypeName,
+      "Waypt::createInstance");
+  }
+  
+  return r;
+}
+
+WayptRef Waypt::createFromProperties(Route* aOwner, SGPropertyNode_ptr aProp)
+{
+  if (!aProp->hasChild("type")) {
+    throw sg_io_exception("bad props node, no type provided", 
+      "Waypt::createFromProperties");
+  }
+  
+  WayptRef nd(createInstance(aOwner, aProp->getStringValue("type")));
+  nd->initFromProperties(aProp);
+  return nd;
+}
+  
+void Waypt::saveAsNode(SGPropertyNode* n) const
+{
+  n->setStringValue("type", type());
+  writeToProperties(n);
+}
+
+void Waypt::initFromProperties(SGPropertyNode_ptr aProp)
+{
+  if (aProp->hasChild("generated")) {
+    setFlag(WPT_GENERATED, aProp->getBoolValue("generated")); 
+  }
+  
+  if (aProp->hasChild("overflight")) {
+    setFlag(WPT_OVERFLIGHT, aProp->getBoolValue("overflight")); 
+  }
+  
+  if (aProp->hasChild("arrival")) {
+    setFlag(WPT_ARRIVAL, aProp->getBoolValue("arrival")); 
+  }
+  
+  if (aProp->hasChild("departure")) {
+    setFlag(WPT_DEPARTURE, aProp->getBoolValue("departure")); 
+  }
+  
+  if (aProp->hasChild("miss")) {
+    setFlag(WPT_MISS, aProp->getBoolValue("miss")); 
+  }
+  
+  if (aProp->hasChild("alt-restrict")) {
+    _altRestrict = restrictionFromString(aProp->getStringValue("alt-restrict"));
+    _altitudeFt = aProp->getDoubleValue("altitude-ft");
+  }
+  
+  if (aProp->hasChild("speed-restrict")) {
+    _speedRestrict = restrictionFromString(aProp->getStringValue("speed-restrict"));
+    _speedKts = aProp->getDoubleValue("speed-kts");
+  }
+  
+  
+}
+
+void Waypt::writeToProperties(SGPropertyNode_ptr aProp) const
+{
+  if (flag(WPT_OVERFLIGHT)) {
+    aProp->setBoolValue("overflight", true);
+  }
+
+  if (flag(WPT_DEPARTURE)) {
+    aProp->setBoolValue("departure", true);
+  }
+  
+  if (flag(WPT_ARRIVAL)) {
+    aProp->setBoolValue("arrival", true);
+  }
+  
+  if (flag(WPT_MISS)) {
+    aProp->setBoolValue("miss", true);
+  }
+  
+  if (flag(WPT_GENERATED)) {
+    aProp->setBoolValue("generated", true);
+  }
+  
+  if (_altRestrict != RESTRICT_NONE) {
+    aProp->setStringValue("alt-restrict", restrictionToString(_altRestrict));
+    aProp->setDoubleValue("altitude-ft", _altitudeFt);
+  }
+  
+  if (_speedRestrict != RESTRICT_NONE) {
+    aProp->setStringValue("speed-restrict", restrictionToString(_speedRestrict));
+    aProp->setDoubleValue("speed-kts", _speedKts);
+  }
+}
+
+void Route::dumpRouteToFile(const WayptVec& aRoute, const std::string& aName)
+{
+  SGPath p = "/Users/jmt/Desktop/" + aName + ".kml";
+  std::fstream f;
+  f.open(p.str().c_str(), fstream::out | fstream::app);
+  if (!f.is_open()) {
+    SG_LOG(SG_GENERAL, SG_WARN, "unable to open:" << p.str());
+    return;
+  }
+  
+// pre-amble
+  f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+      "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
+    "<Document>\n";
+
+  dumpRouteToLineString(aName, aRoute, f);
+  
+// post-amble
+  f << "</Document>\n" 
+    "</kml>" << endl;
+  f.close();
+}
+
+void Route::dumpRouteToLineString(const std::string& aIdent,
+  const WayptVec& aRoute, std::ostream& aStream)
+{
+  // preamble
+  aStream << "<Placemark>\n";
+  aStream << "<name>" << aIdent << "</name>\n";
+  aStream << "<LineString>\n";
+  aStream << "<tessellate>1</tessellate>\n";
+  aStream << "<coordinates>\n";
+  
+  // waypoints
+  for (unsigned int i=0; i<aRoute.size(); ++i) {
+    SGGeod pos = aRoute[i]->position();
+    aStream << pos.getLongitudeDeg() << "," << pos.getLatitudeDeg() << " " << endl;
+  }
+  
+  // postable
+  aStream << "</coordinates>\n"
+    "</LineString>\n"
+    "</Placemark>\n" << endl;
+}
+
+///////////////////////////////////////////////////////////////////////////
+
+class NavdataVisitor : public XMLVisitor {
+public:
+  NavdataVisitor(FGAirport* aApt, const SGPath& aPath);
+
+protected:
+  virtual void startXML (); 
+  virtual void endXML   ();
+  virtual void startElement (const char * name, const XMLAttributes &atts);
+  virtual void endElement (const char * name);
+  virtual void data (const char * s, int len);
+  virtual void pi (const char * target, const char * data);
+  virtual void warning (const char * message, int line, int column);
+  virtual void error (const char * message, int line, int column);
+
+private:
+  Waypt* buildWaypoint();
+  void processRunways(ArrivalDeparture* aProc, const XMLAttributes &atts);
+ 
+  void finishApproach();
+  void finishSid();
+  void finishStar();
+  
+  FGAirport* _airport;
+  SGPath _path;
+  string _text; ///< last element text value
+  
+  SID* _sid;
+  STAR* _star;
+  Approach* _approach;
+
+  WayptVec _waypoints; ///< waypoint list for current approach/sid/star
+  WayptVec _transWaypts; ///< waypoint list for current transition
+  
+  string _wayptName;
+  string _wayptType;
+  string _ident; // id of segment under construction
+  string _transIdent;
+  double _longitude, _latitude, _altitude, _speed;
+  RouteRestriction _altRestrict;
+  
+  double _holdRadial; // inbound hold radial, or -1 if radial is 'inbound'
+  double _holdTD; ///< hold time (seconds) or distance (nm), based on flag below
+  bool _holdRighthanded;
+  bool _holdDistance; // true, TD is distance in nm; false, TD is time in seconds
+  
+  double _course, _radial, _dmeDistance;
+};
+
+void Route::loadAirportProcedures(const SGPath& aPath, FGAirport* aApt)
+{
+  assert(aApt);
+  try {
+    NavdataVisitor visitor(aApt, aPath);
+    readXML(aPath.str(), visitor);
+  } catch (sg_io_exception& ex) {
+    SG_LOG(SG_GENERAL, SG_WARN, "failured parsing procedures: " << aPath.str() <<
+      "\n\t" << ex.getMessage() << "\n\tat:" << ex.getLocation().asString());
+  } catch (sg_exception& ex) {
+    SG_LOG(SG_GENERAL, SG_WARN, "failured parsing procedures: " << aPath.str() <<
+      "\n\t" << ex.getMessage());
+  }
+}
+
+NavdataVisitor::NavdataVisitor(FGAirport* aApt, const SGPath& aPath):
+  _airport(aApt),
+  _path(aPath),
+  _sid(NULL),
+  _star(NULL),
+  _approach(NULL)
+{
+}
+
+void NavdataVisitor::startXML()
+{
+}
+
+void NavdataVisitor::endXML()
+{
+}
+
+void NavdataVisitor::startElement(const char* name, const XMLAttributes &atts)
+{
+  _text.clear();
+  string tag(name);
+  if (tag == "Airport") {
+    string icao(atts.getValue("ICAOcode"));
+    if (_airport->ident() != icao) {
+      throw sg_format_exception("Airport and ICAO mismatch", icao, _path.str());
+    }
+  } else if (tag == "Sid") {
+    string ident(atts.getValue("Name"));
+    _sid = new SID(ident);
+    _waypoints.clear();
+    processRunways(_sid, atts);
+  } else if (tag == "Star") {
+    string ident(atts.getValue("Name"));
+    _star = new STAR(ident);
+    _waypoints.clear();
+    processRunways(_star, atts);
+  } else if ((tag == "Sid_Waypoint") ||
+      (tag == "App_Waypoint") ||
+      (tag == "Star_Waypoint") ||
+      (tag == "AppTr_Waypoint") ||
+      (tag == "SidTr_Waypoint") ||
+      (tag == "RwyTr_Waypoint"))
+  {
+    // reset waypoint data
+    _speed = 0.0;
+    _altRestrict = RESTRICT_NONE;
+    _altitude = 0.0;
+  } else if (tag == "Approach") {
+    _ident = atts.getValue("Name");
+    _waypoints.clear();
+    _approach = new Approach(_ident);
+  } else if ((tag == "Sid_Transition") || 
+             (tag == "App_Transition") ||
+             (tag == "Star_Transition")) {
+    _transIdent = atts.getValue("Name");
+    _transWaypts.clear();
+  } else if (tag == "RunwayTransition") {
+    _transIdent = atts.getValue("Runway");
+    _transWaypts.clear();
+  } else {
+    
+  }
+}
+
+void NavdataVisitor::processRunways(ArrivalDeparture* aProc, const XMLAttributes &atts)
+{
+  string v("All");
+  if (atts.hasAttribute("Runways")) {
+    v = atts.getValue("Runways");
+  }
+  
+  if (v == "All") {
+    for (unsigned int r=0; r<_airport->numRunways(); ++r) {
+      aProc->addRunway(_airport->getRunwayByIndex(r));
+    }
+    return;
+  }
+  
+  vector<string> rwys;
+  boost::split(rwys, v, boost::is_any_of(" ,"));
+  for (unsigned int r=0; r<rwys.size(); ++r) {
+    FGRunway* rwy = _airport->getRunwayByIdent(rwys[r]);
+    aProc->addRunway(rwy);
+  }
+}
+
+void NavdataVisitor::endElement(const char* name)
+{
+  string tag(name);
+  if ((tag == "Sid_Waypoint") ||
+      (tag == "App_Waypoint") ||
+      (tag == "Star_Waypoint"))
+  {
+    _waypoints.push_back(buildWaypoint());
+  } else if ((tag == "AppTr_Waypoint") || 
+             (tag == "SidTr_Waypoint") ||
+             (tag == "RwyTr_Waypoint") ||
+             (tag == "StarTr_Waypoint")) 
+  {
+    _transWaypts.push_back(buildWaypoint());
+  } else if (tag == "Sid_Transition") {
+    assert(_sid);
+    // SID waypoints are stored backwards, to share code with STARs
+    std::reverse(_transWaypts.begin(), _transWaypts.end());
+    Transition* t = new Transition(_transIdent, _sid, _transWaypts);
+    _sid->addTransition(t);
+  } else if (tag == "Star_Transition") {
+    assert(_star);
+    Transition* t = new Transition(_transIdent, _star, _transWaypts);
+    _star->addTransition(t);
+  } else if (tag == "App_Transition") {
+    assert(_approach);
+    Transition* t = new Transition(_transIdent, _approach, _transWaypts);
+    _approach->addTransition(t);
+  } else if (tag == "RunwayTransition") {
+    ArrivalDeparture* ad;
+    if (_sid) {
+      // SID waypoints are stored backwards, to share code with STARs
+      std::reverse(_transWaypts.begin(), _transWaypts.end());
+      ad = _sid;
+    } else {
+      ad = _star;
+    }
+    
+    Transition* t = new Transition(_transIdent, ad, _transWaypts);
+    FGRunwayRef rwy = _airport->getRunwayByIdent(_transIdent);
+    ad->addRunwayTransition(rwy, t);
+  } else if (tag == "Approach") {
+    finishApproach();
+  } else if (tag == "Sid") {
+    finishSid();
+  } else if (tag == "Star") {
+    finishStar();  
+  } else if (tag == "Longitude") {
+    _longitude = atof(_text.c_str());
+  } else if (tag == "Latitude") {
+    _latitude = atof(_text.c_str());
+  } else if (tag == "Name") {
+    _wayptName = _text;
+  } else if (tag == "Type") {
+    _wayptType = _text;
+  } else if (tag == "Speed") {
+    _speed = atoi(_text.c_str());
+  } else if (tag == "Altitude") {
+    _altitude = atof(_text.c_str());
+  } else if (tag == "AltitudeRestriction") {
+    if (_text == "at") {
+      _altRestrict = RESTRICT_AT;
+    } else if (_text == "above") {
+      _altRestrict = RESTRICT_ABOVE;
+    } else if (_text == "below") {
+      _altRestrict = RESTRICT_BELOW;
+    } else {
+      throw sg_format_exception("Unrecognized altitude restriction", _text);
+    }
+  } else if (tag == "Hld_Rad_or_Inbd") {
+    if (_text == "Inbd") {
+      _holdRadial = -1.0;
+    }
+  } else if (tag == "Hld_Time_or_Dist") {
+    _holdDistance = (_text == "Dist");
+  } else if (tag == "Hld_Rad_value") {
+    _holdRadial = atof(_text.c_str());
+  } else if (tag == "Hld_Turn") {
+    _holdRighthanded = (_text == "Right");
+  } else if (tag == "Hld_td_value") {
+    _holdTD = atof(_text.c_str());
+  } else if (tag == "Hdg_Crs_value") {
+    _course = atof(_text.c_str());
+  } else if (tag == "DMEtoIntercept") {
+    _dmeDistance = atof(_text.c_str());
+  } else if (tag == "RadialtoIntercept") {
+    _radial = atof(_text.c_str());
+  } else {
+    
+  }
+}
+
+Waypt* NavdataVisitor::buildWaypoint()
+{
+  Waypt* wp = NULL;
+  if (_wayptType == "Normal") {
+    // new LatLonWaypoint
+    SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
+    wp = new BasicWaypt(pos, _wayptName, NULL);
+  } else if (_wayptType == "Runway") {
+    string ident = _wayptName.substr(2);
+    FGRunwayRef rwy = _airport->getRunwayByIdent(ident);
+    wp = new RunwayWaypt(rwy, NULL);
+  } else if (_wayptType == "Hold") {
+    SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
+    Hold* h = new Hold(pos, _wayptName, NULL);
+    wp = h;
+    if (_holdRighthanded) {
+      h->setRightHanded();
+    } else {
+      h->setLeftHanded();
+    }
+    
+    if (_holdDistance) {
+      h->setHoldDistance(_holdTD);
+    } else {
+      h->setHoldTime(_holdTD * 60.0);
+    }
+    
+    if (_holdRadial >= 0.0) {
+      h->setHoldRadial(_holdRadial);
+    }
+  } else if (_wayptType == "Vectors") {
+    wp = new ATCVectors(NULL, _airport);
+  } else if ((_wayptType == "Intc") || (_wayptType == "VorRadialIntc")) {
+    SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
+    wp = new RadialIntercept(NULL, _wayptName, pos, _course, _radial);
+  } else if (_wayptType == "DmeIntc") {
+    SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
+    wp = new DMEIntercept(NULL, _wayptName, pos, _course, _dmeDistance);
+  } else if (_wayptType == "ConstHdgtoAlt") {
+    wp = new HeadingToAltitude(NULL, _wayptName, _course);
+  } else {
+    SG_LOG(SG_GENERAL, SG_ALERT, "implement waypoint type:" << _wayptType);
+    throw sg_format_exception("Unrecognized waypt type", _wayptType);
+  }
+  
+  assert(wp);
+  if ((_altitude > 0.0) && (_altRestrict != RESTRICT_NONE)) {
+    wp->setAltitude(_altitude,_altRestrict);
+  }
+  
+  if (_speed > 0.0) {
+    wp->setSpeed(_speed, RESTRICT_AT); // or _BELOW?
+  }
+  
+  return wp;
+}
+
+void NavdataVisitor::finishApproach()
+{
+  WayptVec::iterator it;
+  FGRunwayRef rwy;
+  
+// find the runway node
+  for (it = _waypoints.begin(); it != _waypoints.end(); ++it) {
+    FGPositionedRef navid = (*it)->source();
+    if (!navid) {
+      continue;
+    }
+    
+    if (navid->type() == FGPositioned::RUNWAY) {
+      rwy = (FGRunway*) navid.get();
+      break;
+    }
+  }
+  
+  if (!rwy) {
+    throw sg_format_exception("Malformed approach, no runway waypt", _ident);
+  }
+  
+  WayptVec primary(_waypoints.begin(), it);
+  // erase all points up to and including the runway, to leave only the
+  // missed segments
+  _waypoints.erase(_waypoints.begin(), ++it);
+  
+  _approach->setRunway(rwy);
+  _approach->setPrimaryAndMissed(primary, _waypoints);
+  _airport->addApproach(_approach);
+  _approach = NULL;
+}
+
+void NavdataVisitor::finishSid()
+{
+  // reverse order, because that's how we deal with commonality between
+  // STARs and SIDs. SID::route undoes  this
+  std::reverse(_waypoints.begin(), _waypoints.end());
+  _sid->setCommon(_waypoints);
+  _airport->addSID(_sid);
+  _sid = NULL;
+}
+
+void NavdataVisitor::finishStar()
+{
+  _star->setCommon(_waypoints);
+  _airport->addSTAR(_star);
+  _star = NULL;
+}
+
+void NavdataVisitor::data (const char * s, int len)
+{
+  _text += string(s, len);
+}
+
+
+void NavdataVisitor::pi (const char * target, const char * data) {
+  //cout << "Processing instruction " << target << ' ' << data << endl;
+}
+
+void NavdataVisitor::warning (const char * message, int line, int column) {
+  SG_LOG(SG_IO, SG_WARN, "Warning: " << message << " (" << line << ',' << column << ')');
+}
+
+void NavdataVisitor::error (const char * message, int line, int column) {
+  SG_LOG(SG_IO, SG_ALERT, "Error: " << message << " (" << line << ',' << column << ')');
+}
+
+} // of namespace flightgear

src/Navaids/route.hxx

@@ -0,0 +1,208 @@
+/**
+ * route.hxx - defines basic route and route-element classes. Route elements
+ * are specialised into waypoints and related things. Routes are any class tha
+ * owns a collection (list, tree, graph) of route elements - such as airways,
+ * procedures or a flight plan.
+ */
+ 
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#ifndef FG_ROUTE_HXX
+#define FG_ROUTE_HXX
+
+// std
+#include <vector>
+#include <map>
+#include <iosfwd>
+
+// Simgear
+#include <simgear/math/SGMath.hxx>
+#include <simgear/structure/SGReferenced.hxx>
+#include <simgear/structure/SGSharedPtr.hxx>
+#include <simgear/props/props.hxx>
+
+// forward decls
+class FGPositioned;
+class SGPath;
+class FGAirport;
+
+namespace flightgear
+{
+
+// forward decls
+class Route;
+class Waypt;
+class NavdataVisitor;
+
+typedef SGSharedPtr<Waypt> WayptRef;
+
+typedef enum {
+	WPT_MAP           = 1 << 0, ///< missed approach point
+	WPT_IAF           = 1 << 1, ///< initial approach fix
+	WPT_FAF           = 1 << 2, ///< final approach fix
+	WPT_OVERFLIGHT    = 1 << 3, ///< must overfly the point directly
+	WPT_TRANSITION    = 1 << 4, ///< transition to/from enroute structure
+	WPT_MISS          = 1 << 5, ///< segment is part of missed approach
+  /// waypoint position is dynamic, i.e moves based on other criteria,
+  /// such as altitude, inbound course, or so on.
+  WPT_DYNAMIC       = 1 << 6,
+  /// waypoint was created automatically (not manually entered/loaded)
+  /// for example waypoints from airway routing or a procedure  
+  WPT_GENERATED     = 1 << 7,
+  
+  WPT_DEPARTURE     = 1 << 8,
+  WPT_ARRIVAL       = 1 << 9
+} WayptFlag;
+
+typedef enum {
+	RESTRICT_NONE,
+	RESTRICT_AT,
+	RESTRICT_ABOVE,
+	RESTRICT_BELOW
+} RouteRestriction;
+
+/**
+ * Abstract base class for waypoints (and things that are treated similarly
+ * by navigation systems)
+ */
+class Waypt : public SGReferenced
+{
+public:
+	Route* owner() const 
+		{ return _owner; }
+  
+  /**
+   * Return true course (in degrees) and distance (in metres) from the provided
+   * position to this waypoint
+   */
+  virtual std::pair<double, double> courseAndDistanceFrom(const SGGeod& aPos) const;
+                  	
+	virtual SGGeod position() const = 0;
+	
+	/**
+	 * The Positioned associated with this element, if one exists
+	 */
+	virtual FGPositioned* source() const
+		{ return NULL; }
+	
+	virtual double altitudeFt() const 
+		{ return _altitudeFt; }
+		
+	virtual double speedKts() const
+		{ return _speedKts; }
+	
+	virtual RouteRestriction altitudeRestriction() const
+		{ return _altRestrict; }
+	
+	virtual RouteRestriction speedRestriction() const
+		{ return _speedRestrict; }
+	
+  void setAltitude(double aAlt, RouteRestriction aRestrict);
+  void setSpeed(double aSpeed, RouteRestriction aRestrict);
+  
+  /**
+   * Identifier assoicated with the waypoint. Human-readable, but
+   * possibly quite terse, and definitiely not unique.
+   */
+	virtual std::string ident() const;
+	
+	/**
+	 * Test if the specified flag is set for this element
+	 */
+	virtual bool flag(WayptFlag aFlag) const;
+	
+  void setFlag(WayptFlag aFlag, bool aV = true);
+  
+  /**
+   * Factory method
+   */
+  static WayptRef createFromProperties(Route* aOwner, SGPropertyNode_ptr aProp);
+  
+  void saveAsNode(SGPropertyNode* node) const;
+  
+  /**
+   * Test if this element and another are 'the same', i.e matching
+   * ident and lat/lon are approximately equal
+   */
+  bool matches(Waypt* aOther) const;
+
+  /**
+   * Test if this element and a position 'the same'
+   * this can be defined by either position, ident or both
+   */
+  bool matches(const SGGeod& aPos) const;
+  
+  virtual std::string type() const = 0;
+protected:
+  friend class NavdataVisitor;
+  
+	Waypt(Route* aOwner);
+  
+  /**
+   * Persistence helper - read node properties from a file
+   */
+  virtual void initFromProperties(SGPropertyNode_ptr aProp);
+  
+  /**
+   * Persistence helper - save this element to a node
+   */
+  virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
+  
+  typedef Waypt* (FactoryFunction)(Route* aOwner) ;
+  static void registerFactory(const std::string aNodeType, FactoryFunction* aFactory);
+  
+  double _altitudeFt;
+	double _speedKts;
+	RouteRestriction _altRestrict;
+	RouteRestriction _speedRestrict;
+private:
+
+  /**
+   * Create an instance of a concrete subclass, or throw an exception
+   */
+  static Waypt* createInstance(Route* aOwner, const std::string& aTypeName);
+
+	Route* _owner;
+	unsigned short _flags;
+	
+};
+
+typedef std::vector<WayptRef> WayptVec;
+  
+class Route
+{
+public:
+  /**
+   *
+   */
+  virtual std::string ident() const = 0;
+  
+  static void loadAirportProcedures(const SGPath& aPath, FGAirport* aApt);
+  
+  static void dumpRouteToFile(const WayptVec& aRoute, const std::string& aName);
+  
+  static void dumpRouteToLineString(const std::string& aIdent,
+    const WayptVec& aRoute, std::ostream& aStream);
+private:
+
+};
+
+} // of namespace flightgear
+
+#endif // of FG_ROUTE_HXX

src/Navaids/routePath.cxx

@@ -0,0 +1,346 @@
+#include <Navaids/routePath.hxx>
+
+#include <simgear/structure/exception.hxx>
+#include <simgear/magvar/magvar.hxx>
+#include <simgear/timing/sg_time.hxx>
+
+#include <Main/globals.hxx>
+#include <Airports/runways.hxx>
+#include <Navaids/waypoint.hxx>
+#include <Navaids/positioned.hxx>
+
+namespace flightgear {
+  bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
+}
+
+using namespace flightgear;
+
+// implement Point(s) known distance from a great circle
+
+static double sqr(const double x)
+{
+  return x * x;
+}
+
+double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
+{
+  double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
+  double bDist = SGGeodesy::distanceRad(a, d);
+  
+  // r=(cos(b)^2+sin(b)^2*cos(A)^2)^(1/2)
+  double r = pow(sqr(cos(bDist)) + sqr(sin(bDist)) * sqr(cos(A)), 0.5); 
+  
+  double p = atan2(sin(bDist)*cos(A), cos(bDist));
+  
+  if (sqr(cos(dist)) > sqr(r)) {
+    SG_LOG(SG_GENERAL, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
+    return -1.0;
+  }
+  
+  double dp1 = p + acos(cos(dist)/r);
+  double dp2 = p - acos(cos(dist)/r);
+  
+  double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
+  double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
+  
+  return SGMiscd::min(dp1Nm, dp2Nm);
+}
+
+RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
+  _waypts(wpts)
+{
+  _pathClimbFPM = 1200;
+  _pathDescentFPM = 800;
+  _pathIAS = 190; 
+  _pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn
+}
+
+SGGeodVec RoutePath::pathForIndex(int index) const
+{
+  if (index == 0) {
+    return SGGeodVec(); // no path for first waypoint
+  }
+  
+  if (_waypts[index]->type() == "vectors") {
+    return SGGeodVec(); // empty
+  }
+  
+  if (_waypts[index]->type() == "hold") {
+    return pathForHold((Hold*) _waypts[index].get());
+  }
+    
+  SGGeodVec r;
+  SGGeod pos;
+  if (!computedPositionForIndex(index-1, pos)) {
+    return SGGeodVec();
+  }
+  
+  r.push_back(pos);
+  if (!computedPositionForIndex(index, pos)) {
+    return SGGeodVec();
+  }
+  
+  r.push_back(pos);
+  
+  if (_waypts[index]->type() == "runway") {
+    // runways get an extra point, at the end. this is particularly
+    // important so missed approach segments draw correctly
+    FGRunway* rwy = static_cast<RunwayWaypt*>(_waypts[index].get())->runway();
+    r.push_back(rwy->end());
+  }
+  
+  return r;
+}
+
+SGGeod RoutePath::positionForIndex(int index) const
+{
+  SGGeod r;
+  bool ok = computedPositionForIndex(index, r);
+  if (!ok) {
+    return SGGeod();
+  }
+  
+  return r;
+}
+
+SGGeodVec RoutePath::pathForHold(Hold* hold) const
+{
+  int turnSteps = 16;
+  double hdg = hold->inboundRadial();
+  double turnDelta = 180.0 / turnSteps;
+  
+  SGGeodVec r;
+  double az2;
+  double stepTime = turnDelta / _pathTurnRate; // in seconds
+  double stepDist = _pathIAS * (stepTime / 3600.0) * SG_NM_TO_METER;
+  double legDist = hold->isDistance() ? 
+    hold->timeOrDistance() 
+    : _pathIAS * (hold->timeOrDistance() / 3600.0);
+  legDist *= SG_NM_TO_METER;
+  
+  if (hold->isLeftHanded()) {
+    turnDelta = -turnDelta;
+  }  
+  SGGeod pos = hold->position();
+  r.push_back(pos);
+
+  // turn+leg sides are a mirror
+  for (int j=0; j < 2; ++j) {
+  // turn
+    for (int i=0;i<turnSteps; ++i) {
+      hdg += turnDelta;
+      SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
+      r.push_back(pos);
+    }
+    
+  // leg
+    SGGeodesy::direct(pos, hdg, legDist, pos, az2);
+    r.push_back(pos);
+  } // of leg+turn duplication
+  
+  return r;
+}
+
+/**
+ * the path context holds the state of of an imaginary aircraft traversing
+ * the route, and limits the rate at which heading / altitude / position can
+ * change
+ */
+class RoutePath::PathCtx
+{
+public:
+  SGGeod pos;
+  double heading;
+};
+
+bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const
+{
+  if ((index < 0) || (index >= (int) _waypts.size())) {
+    throw sg_range_exception("waypt index out of range", 
+      "RoutePath::computedPositionForIndex");
+  }
+
+  WayptRef w = _waypts[index];
+  if (!w->flag(WPT_DYNAMIC)) {
+    r = w->position();
+    return true;
+  }
+  
+  if (w->type() == "radialIntercept") {
+    // radial intersection along track
+    SGGeod prev;
+    if (!computedPositionForIndex(index - 1, prev)) {
+      return false;
+    }
+  
+    SGGeoc prevGc = SGGeoc::fromGeod(prev);
+    SGGeoc navid = SGGeoc::fromGeod(w->position());
+    SGGeoc rGc;
+    double magVar = magVarFor(prev);
+    
+    RadialIntercept* i = (RadialIntercept*) w.get();
+    double radial = i->radialDegMagnetic() + magVar;
+    double track = i->courseDegMagnetic() + magVar;
+    bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
+    if (!ok) {
+      return false;
+    }
+    
+    r = SGGeod::fromGeoc(rGc);
+    return true;
+  } else if (w->type() == "dmeIntercept") {
+    // find the point along the DME track, from prev, that is the correct distance
+    // from the DME
+    SGGeod prev;
+    if (!computedPositionForIndex(index - 1, prev)) {
+      return false;
+    }
+    
+    DMEIntercept* di = (DMEIntercept*) w.get();
+    
+    SGGeoc prevGc = SGGeoc::fromGeod(prev);
+    SGGeoc navid = SGGeoc::fromGeod(w->position());
+    double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
+    SGGeoc rGc;
+
+    SGGeoc bPt;
+    double crs = di->courseDegMagnetic() + magVarFor(prev);
+    SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
+
+    double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
+    if (dNm < 0.0) {
+      return false;
+    }
+    
+    double az2;
+    SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2);
+    return true;
+  } else if (w->type() == "hdgToAlt") {
+    HeadingToAltitude* h = (HeadingToAltitude*) w.get();
+    double climb = h->altitudeFt() - computeAltitudeForIndex(index - 1);
+    double d = distanceForClimb(climb);
+
+    SGGeod prevPos;
+    if (!computedPositionForIndex(index - 1, prevPos)) {
+      return false;
+    }
+    
+    double hdg = h->headingDegMagnetic() + magVarFor(prevPos);
+    
+    double az2;
+    SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
+    return true;
+  } else if (w->type() == "vectors"){
+    return false;
+  } else if (w->type() == "hold") {
+    r = w->position();
+    return true;
+  }
+  
+  SG_LOG(SG_GENERAL, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type());
+  return false;
+}
+
+double RoutePath::computeAltitudeForIndex(int index) const
+{
+  if ((index < 0) || (index >= (int) _waypts.size())) {
+    throw sg_range_exception("waypt index out of range", 
+      "RoutePath::computeAltitudeForIndex");
+  }
+  
+  WayptRef w = _waypts[index];
+  if (w->altitudeRestriction() != RESTRICT_NONE) {
+    return w->altitudeFt(); // easy!
+  }
+  
+  if (w->type() == "runway") {
+    FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
+    return rwy->threshold().getElevationFt();
+  } else if ((w->type() == "hold") || (w->type() == "vectors")) {
+    // pretend we don't change altitude in holds/vectoring
+    return computeAltitudeForIndex(index - 1);
+  }
+
+  double prevAlt = computeAltitudeForIndex(index - 1);
+// find distance to previous, and hence climb/descent 
+  SGGeod pos, prevPos;
+  
+  if (!computedPositionForIndex(index, pos) ||
+      !computedPositionForIndex(index - 1, prevPos))
+  {
+    SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
+    throw sg_range_exception("unable to compute position for waypoints");
+  }
+  
+  double d = SGGeodesy::distanceNm(prevPos, pos);
+  double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes
+  
+  double deltaFt; // change in altitude in feet
+  if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) {
+    deltaFt = -_pathDescentFPM * tMinutes;
+  } else {
+    deltaFt = _pathClimbFPM * tMinutes;
+  }
+  
+  return prevAlt + deltaFt;
+}
+
+double RoutePath::computeTrackForIndex(int index) const
+{
+  if ((index < 0) || (index >= (int) _waypts.size())) {
+    throw sg_range_exception("waypt index out of range", 
+      "RoutePath::computeTrackForIndex");
+  }
+  
+  WayptRef w = _waypts[index];
+  if (w->type() == "radialIntercept") {
+    RadialIntercept* r = (RadialIntercept*) w.get();
+    return r->courseDegMagnetic();
+  } else if (w->type() == "dmeIntercept") {
+    DMEIntercept* d = (DMEIntercept*) w.get();
+    return d->courseDegMagnetic();
+  } else if (w->type() == "hdgToAlt") {
+    HeadingToAltitude* h = (HeadingToAltitude*) w.get();
+    return h->headingDegMagnetic();
+  } else if (w->type() == "hold") {
+    Hold* h = (Hold*) w.get();
+    return h->inboundRadial();
+  } else if (w->type() == "vectors") {
+    SG_LOG(SG_GENERAL, SG_WARN, "asked for track from VECTORS");
+    throw sg_range_exception("asked for track from vectors waypt");
+  } else if (w->type() == "runway") {
+    FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
+    return rwy->headingDeg();
+  }
+  
+// course based upon previous and current pos
+  SGGeod pos, prevPos;
+  
+  if (!computedPositionForIndex(index, pos) ||
+      !computedPositionForIndex(index - 1, prevPos))
+  {
+    SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
+    throw sg_range_exception("unable to compute position for waypoints");
+  }
+
+  return SGGeodesy::courseDeg(prevPos, pos);
+}
+
+double RoutePath::distanceForClimb(double climbFt) const
+{
+  double t = 0.0; // in seconds
+  if (climbFt > 0.0) {
+    t = (climbFt / _pathClimbFPM) * 60.0;
+  } else if (climbFt < 0.0) {
+    t = (climbFt / _pathDescentFPM) * 60.0;
+  }
+  
+  return _pathIAS * (t / 3600.0);
+}
+
+double RoutePath::magVarFor(const SGGeod& geod) const
+{
+  double jd = globals->get_time_params()->getJD();
+  return sgGetMagVar(geod, jd);
+}
+

src/Navaids/routePath.hxx

@@ -0,0 +1,69 @@
+/**
+ * routePath.hxx - convert a route to straight line segments, for graphical
+ * output or display.
+ */
+ 
+// Written by James Turner, started 2010.
+//
+// Copyright (C) 2010  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#ifndef FG_ROUTE_PATH_HXX
+#define FG_ROUTE_PATH_HXX
+
+#include <Navaids/route.hxx>
+
+namespace flightgear
+{
+  class Hold;
+}
+
+typedef std::vector<SGGeod> SGGeodVec;
+
+class RoutePath
+{
+public:
+  RoutePath(const flightgear::WayptVec& wpts);
+
+  SGGeodVec pathForIndex(int index) const;
+  
+  SGGeod positionForIndex(int index) const;
+  
+private:
+  class PathCtx;
+  
+  SGGeodVec pathForHold(flightgear::Hold* hold) const;
+  
+  bool computedPositionForIndex(int index, SGGeod& pos) const;
+  double computeAltitudeForIndex(int index) const;
+  double computeTrackForIndex(int index) const;
+  
+  /**
+   * Find the distance (in Nm) to climb/descend a height in feet
+   */
+  double distanceForClimb(double climbFt) const;
+  
+  double magVarFor(const SGGeod& gd) const; 
+  
+  flightgear::WayptVec _waypts;
+
+  int _pathClimbFPM; ///< climb-rate to use for pathing
+  int _pathDescentFPM; ///< descent rate to use (feet-per-minute)
+  int _pathIAS; ///< IAS (knots) to use for pathing
+  double _pathTurnRate; ///< degrees-per-second, defaults to 3, i.e 180 in a minute
+};
+
+#endif

src/Navaids/testnavs.cxx

@@ -1,3 +1,21 @@
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
 #include <simgear/misc/sg_path.hxx>
 
 #include "fixlist.hxx"

src/Navaids/waypoint.cxx

@@ -0,0 +1,471 @@
+// waypoint.cxx - waypoints that can occur in routes/procedures
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#ifdef HAVE_CONFIG_H
+# include "config.h"
+#endif
+
+#include "waypoint.hxx"
+
+#include <simgear/structure/exception.hxx>
+#include <simgear/route/waypoint.hxx>
+
+#include <Airports/simple.hxx>
+#include <Airports/runways.hxx>
+
+using std::string;
+
+namespace flightgear
+{
+
+BasicWaypt::BasicWaypt(const SGGeod& aPos, const string& aIdent, Route* aOwner) :
+  Waypt(aOwner),
+  _pos(aPos),
+  _ident(aIdent)
+{
+}
+
+BasicWaypt::BasicWaypt(const SGWayPoint& aWP, Route* aOwner) :
+  Waypt(aOwner),
+  _pos(aWP.get_target()),
+  _ident(aWP.get_id())
+{
+}
+
+BasicWaypt::BasicWaypt(Route* aOwner) :
+  Waypt(aOwner)
+{
+}
+
+void BasicWaypt::initFromProperties(SGPropertyNode_ptr aProp)
+{
+  if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) {
+    throw sg_io_exception("missing lon/lat properties", 
+      "BasicWaypt::initFromProperties");
+  }
+
+  Waypt::initFromProperties(aProp);
+
+  _pos = SGGeod::fromDeg(aProp->getDoubleValue("lon"), 
+    aProp->getDoubleValue("lat"));
+  _ident = aProp->getStringValue("ident");
+}
+
+void BasicWaypt::writeToProperties(SGPropertyNode_ptr aProp) const
+{
+  Waypt::writeToProperties(aProp);
+  
+  aProp->setStringValue("ident", _ident);
+  aProp->setDoubleValue("lon", _pos.getLongitudeDeg());
+  aProp->setDoubleValue("lat", _pos.getLatitudeDeg());
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+NavaidWaypoint::NavaidWaypoint(FGPositioned* aPos, Route* aOwner) :
+  Waypt(aOwner),
+  _navaid(aPos)
+{
+  if (aPos->type() == FGPositioned::RUNWAY) {
+      SG_LOG(SG_GENERAL, SG_WARN, "sure you don't want to be building a runway waypt here?");
+  }
+}
+
+NavaidWaypoint::NavaidWaypoint(Route* aOwner) :
+  Waypt(aOwner)
+{
+}
+
+
+SGGeod NavaidWaypoint::position() const
+{
+  return SGGeod::fromGeodFt(_navaid->geod(), _altitudeFt);
+}  
+ 
+std::string NavaidWaypoint::ident() const
+{
+  return _navaid->ident();
+}
+
+void NavaidWaypoint::initFromProperties(SGPropertyNode_ptr aProp)
+{
+  if (!aProp->hasChild("ident")) {
+    throw sg_io_exception("missing navaid value", 
+      "NavaidWaypoint::initFromProperties");
+  }
+  
+  Waypt::initFromProperties(aProp);
+  
+  std::string idn(aProp->getStringValue("ident"));
+  SGGeod p;
+  if (aProp->hasChild("lon")) {
+    p = SGGeod::fromDeg(aProp->getDoubleValue("lon"), aProp->getDoubleValue("lat"));
+  }
+  
+  // FIXME - resolve co-located DME, etc
+  // is it sufficent just to ignore DMEs, actually?
+  FGPositionedRef nav = FGPositioned::findClosestWithIdent(idn, p, NULL);
+  if (!nav) {
+    throw sg_io_exception("unknown navaid ident:" + idn, 
+      "NavaidWaypoint::initFromProperties");
+  }
+  
+  _navaid = nav;
+}
+
+void NavaidWaypoint::writeToProperties(SGPropertyNode_ptr aProp) const
+{
+  Waypt::writeToProperties(aProp);
+  
+  aProp->setStringValue("ident", _navaid->ident());
+  // write lon/lat to disambiguate
+  aProp->setDoubleValue("lon", _navaid->geod().getLongitudeDeg());
+  aProp->setDoubleValue("lat", _navaid->geod().getLatitudeDeg());
+}
+
+OffsetNavaidWaypoint::OffsetNavaidWaypoint(FGPositioned* aPos, Route* aOwner,
+  double aRadial, double aDistNm) :
+  NavaidWaypoint(aPos, aOwner),
+  _radial(aRadial),
+  _distanceNm(aDistNm)
+{
+  init();
+}
+
+OffsetNavaidWaypoint::OffsetNavaidWaypoint(Route* aOwner) :
+  NavaidWaypoint(aOwner)
+{
+}
+
+void OffsetNavaidWaypoint::init()
+{
+  SGGeod offset;
+  double az2;
+  SGGeodesy::direct(_navaid->geod(), _radial, _distanceNm * SG_NM_TO_METER, offset, az2);
+  _geod = SGGeod::fromGeodFt(offset, _altitudeFt);
+}
+
+void OffsetNavaidWaypoint::initFromProperties(SGPropertyNode_ptr aProp)
+{
+  if (!aProp->hasChild("radial-deg") || !aProp->hasChild("distance-nm")) {
+    throw sg_io_exception("missing radial/offset distance",
+      "OffsetNavaidWaypoint::initFromProperties");
+  }
+  
+  NavaidWaypoint::initFromProperties(aProp);
+  _radial = aProp->getDoubleValue("radial-deg");
+  _distanceNm = aProp->getDoubleValue("distance-nm");
+  init();
+}
+
+void OffsetNavaidWaypoint::writeToProperties(SGPropertyNode_ptr aProp) const
+{
+  NavaidWaypoint::writeToProperties(aProp);
+  aProp->setDoubleValue("radial-deg", _radial);
+  aProp->setDoubleValue("distance-nm", _distanceNm);
+}
+
+/////////////////////////////////////////////////////////////////////////////
+
+RunwayWaypt::RunwayWaypt(FGRunway* aPos, Route* aOwner) :
+  Waypt(aOwner),
+  _runway(aPos)
+{
+}
+
+RunwayWaypt::RunwayWaypt(Route* aOwner) :
+  Waypt(aOwner)
+{
+}
+
+SGGeod RunwayWaypt::position() const
+{
+  return _runway->threshold();
+}
+  
+std::string RunwayWaypt::ident() const
+{
+  return _runway->airport()->ident() + "-" + _runway->ident();
+}
+
+FGPositioned* RunwayWaypt::source() const
+{
+  return _runway;
+}
+
+void RunwayWaypt::initFromProperties(SGPropertyNode_ptr aProp)
+{
+  if (!aProp->hasChild("icao") || !aProp->hasChild("ident")) {
+    throw sg_io_exception("missing values: icao or ident", 
+      "RunwayWaypoint::initFromProperties");
+  }
+  
+  Waypt::initFromProperties(aProp);
+  std::string idn(aProp->getStringValue("ident"));
+  const FGAirport* apt = FGAirport::getByIdent(aProp->getStringValue("icao"));
+  _runway = apt->getRunwayByIdent(aProp->getStringValue("ident"));
+}
+
+void RunwayWaypt::writeToProperties(SGPropertyNode_ptr aProp) const
+{
+  Waypt::writeToProperties(aProp);
+  aProp->setStringValue("ident", _runway->ident());
+  aProp->setStringValue("icao", _runway->airport()->ident());
+}
+
+/////////////////////////////////////////////////////////////////////////////
+
+Hold::Hold(const SGGeod& aPos, const string& aIdent, Route* aOwner) :
+  BasicWaypt(aPos, aIdent, aOwner),
+  _righthanded(true),
+  _isDistance(false)
+{
+  setFlag(WPT_DYNAMIC);
+}
+
+Hold::Hold(Route* aOwner) :
+  BasicWaypt(aOwner),
+  _righthanded(true),
+  _isDistance(false)
+{
+}
+
+void Hold::setHoldRadial(double aInboundRadial)
+{
+  _bearing = aInboundRadial;
+}
+
+void Hold::setHoldDistance(double aDistanceNm)
+{
+  _isDistance = true;
+  _holdTD = aDistanceNm;
+}
+
+void Hold::setHoldTime(double aTimeSec)
+{
+  _isDistance = false;
+  _holdTD = aTimeSec;
+}
+
+void Hold::setRightHanded()
+{
+  _righthanded = true;
+}
+
+void Hold::setLeftHanded()
+{
+  _righthanded = false;
+}
+  
+void Hold::initFromProperties(SGPropertyNode_ptr aProp)
+{
+  BasicWaypt::initFromProperties(aProp);
+  
+  if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) {
+    throw sg_io_exception("missing lon/lat properties", 
+      "Hold::initFromProperties");
+  }
+
+  _righthanded = aProp->getBoolValue("right-handed");
+  _isDistance = aProp->getBoolValue("is-distance");
+  _bearing = aProp->getDoubleValue("inbound-radial-deg");
+  _holdTD = aProp->getDoubleValue("td");
+}
+
+void Hold::writeToProperties(SGPropertyNode_ptr aProp) const
+{
+  BasicWaypt::writeToProperties(aProp);
+
+  aProp->setBoolValue("right-handed", _righthanded);
+  aProp->setBoolValue("is-distance", _isDistance);
+  aProp->setDoubleValue("inbound-radial-deg", _bearing);
+  aProp->setDoubleValue("td", _holdTD);
+}
+
+/////////////////////////////////////////////////////////////////////////////
+
+HeadingToAltitude::HeadingToAltitude(Route* aOwner, const string& aIdent, 
+  double aMagHdg) :
+  Waypt(aOwner),
+  _ident(aIdent),
+  _magHeading(aMagHdg)
+{
+  setFlag(WPT_DYNAMIC);
+}
+
+HeadingToAltitude::HeadingToAltitude(Route* aOwner) :
+  Waypt(aOwner)
+{
+}
+
+void HeadingToAltitude::initFromProperties(SGPropertyNode_ptr aProp)
+{
+  if (!aProp->hasChild("heading-deg")) {
+    throw sg_io_exception("missing heading/alt properties", 
+      "HeadingToAltitude::initFromProperties");
+  }
+
+  Waypt::initFromProperties(aProp);
+  _magHeading = aProp->getDoubleValue("heading-deg");
+  _ident = aProp->getStringValue("ident");
+}
+
+void HeadingToAltitude::writeToProperties(SGPropertyNode_ptr aProp) const
+{
+  Waypt::writeToProperties(aProp);
+  aProp->setStringValue("ident", _ident);
+  aProp->setDoubleValue("heading-deg", _magHeading);
+}
+
+/////////////////////////////////////////////////////////////////////////////
+
+DMEIntercept::DMEIntercept(Route* aOwner, const string& aIdent, const SGGeod& aPos,
+    double aCourseDeg, double aDistanceNm) :
+  Waypt(aOwner),
+  _ident(aIdent),
+  _pos(aPos),
+  _magCourse(aCourseDeg),
+  _dmeDistanceNm(aDistanceNm)
+{
+  setFlag(WPT_DYNAMIC);
+}
+
+DMEIntercept::DMEIntercept(Route* aOwner) :
+  Waypt(aOwner)
+{
+}
+
+void DMEIntercept::initFromProperties(SGPropertyNode_ptr aProp)
+{
+  if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) {
+    throw sg_io_exception("missing lon/lat properties", 
+      "DMEIntercept::initFromProperties");
+  }
+
+  Waypt::initFromProperties(aProp);
+  _pos = SGGeod::fromDeg(aProp->getDoubleValue("lon"), aProp->getDoubleValue("lat"));
+  _ident = aProp->getStringValue("ident");
+// check it's a real DME?
+  _magCourse = aProp->getDoubleValue("course-deg");
+  _dmeDistanceNm = aProp->getDoubleValue("dme-distance-nm");
+  
+}
+
+void DMEIntercept::writeToProperties(SGPropertyNode_ptr aProp) const
+{
+  Waypt::writeToProperties(aProp);
+  
+  aProp->setStringValue("ident", _ident);
+  aProp->setDoubleValue("lon", _pos.getLongitudeDeg());
+  aProp->setDoubleValue("lat", _pos.getLatitudeDeg());
+  aProp->setDoubleValue("course-deg", _magCourse);
+  aProp->setDoubleValue("dme-distance-nm", _dmeDistanceNm);
+}
+
+/////////////////////////////////////////////////////////////////////////////
+
+RadialIntercept::RadialIntercept(Route* aOwner, const string& aIdent, const SGGeod& aPos,
+    double aCourseDeg, double aRadial) :
+  Waypt(aOwner),
+  _ident(aIdent),
+  _pos(aPos),
+  _magCourse(aCourseDeg),
+  _radial(aRadial)
+{
+  setFlag(WPT_DYNAMIC);
+}
+
+RadialIntercept::RadialIntercept(Route* aOwner) :
+  Waypt(aOwner)
+{
+}
+  
+void RadialIntercept::initFromProperties(SGPropertyNode_ptr aProp)
+{
+  if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) {
+    throw sg_io_exception("missing lon/lat properties", 
+      "RadialIntercept::initFromProperties");
+  }
+
+  Waypt::initFromProperties(aProp);
+  _pos = SGGeod::fromDeg(aProp->getDoubleValue("lon"), aProp->getDoubleValue("lat"));
+  _ident = aProp->getStringValue("ident");
+// check it's a real VOR?
+  _magCourse = aProp->getDoubleValue("course-deg");
+  _radial = aProp->getDoubleValue("radial-deg");
+  
+}
+
+void RadialIntercept::writeToProperties(SGPropertyNode_ptr aProp) const
+{
+  Waypt::writeToProperties(aProp);
+  
+  aProp->setStringValue("ident", _ident);
+  aProp->setDoubleValue("lon", _pos.getLongitudeDeg());
+  aProp->setDoubleValue("lat", _pos.getLatitudeDeg());
+  aProp->setDoubleValue("course-deg", _magCourse);
+  aProp->setDoubleValue("radial-deg", _radial);
+}
+
+/////////////////////////////////////////////////////////////////////////////
+
+ATCVectors::ATCVectors(Route* aOwner, FGAirport* aFacility) :
+  Waypt(aOwner),
+  _facility(aFacility)
+{
+  setFlag(WPT_DYNAMIC);
+}
+
+ATCVectors::~ATCVectors()
+{
+}
+
+ATCVectors::ATCVectors(Route* aOwner) :
+  Waypt(aOwner)
+{
+}
+
+SGGeod ATCVectors::position() const
+{
+  return _facility->geod();
+}
+    
+string ATCVectors::ident() const
+{
+  return "VECTORS-" + _facility->ident();
+}
+
+void ATCVectors::initFromProperties(SGPropertyNode_ptr aProp)
+{  
+  if (!aProp->hasChild("icao")) {
+    throw sg_io_exception("missing icao propertie", 
+      "ATCVectors::initFromProperties");
+  }
+
+  Waypt::initFromProperties(aProp);
+  _facility = FGAirport::getByIdent(aProp->getStringValue("icao"));
+}
+
+void ATCVectors::writeToProperties(SGPropertyNode_ptr aProp) const
+{
+  Waypt::writeToProperties(aProp);
+  aProp->setStringValue("icao", _facility->ident());
+}
+
+} // of namespace

src/Navaids/waypoint.hxx

@@ -0,0 +1,314 @@
+// waypoint.hxx - waypoints that can occur in routes/procedures
+// Written by James Turner, started 2009.
+//
+// Copyright (C) 2009  Curtis L. Olson
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+#ifndef FG_WAYPOINT_HXX
+#define FG_WAYPOINT_HXX
+
+#include <Navaids/route.hxx>
+#include <Navaids/positioned.hxx>
+
+class FGAirport;
+typedef SGSharedPtr<FGAirport> FGAirportRef;
+class SGWayPoint;
+class FGRunway;
+
+namespace flightgear
+{
+
+
+class BasicWaypt : public Waypt
+{
+public:
+  
+  BasicWaypt(const SGGeod& aPos, const std::string& aIdent, Route* aOwner);
+  
+  BasicWaypt(const SGWayPoint& aWP, Route* aOwner);
+  
+  BasicWaypt(Route* aOwner);
+  
+  virtual SGGeod position() const
+    { return _pos; }
+  
+  virtual std::string ident() const
+    { return _ident; }
+
+protected:
+  virtual void initFromProperties(SGPropertyNode_ptr aProp);
+  virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
+
+  virtual std::string type() const
+    { return "basic"; } 
+
+  SGGeod _pos;
+  std::string _ident;
+  
+};
+
+/**
+ * Waypoint based upon a navaid. In practice this means any Positioned
+ * element, excluding runways (see below)
+ */
+class NavaidWaypoint : public Waypt
+{
+public:
+  NavaidWaypoint(FGPositioned* aPos, Route* aOwner);
+  
+  NavaidWaypoint(Route* aOwner);
+  
+  virtual SGGeod position() const;
+  
+  virtual FGPositioned* source() const
+    { return _navaid; }
+    
+  virtual std::string ident() const;
+protected:	
+  virtual void initFromProperties(SGPropertyNode_ptr aProp);
+  virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
+
+  virtual std::string type() const
+    { return "navaid"; }
+    
+  FGPositionedRef _navaid;
+};
+
+class OffsetNavaidWaypoint : public NavaidWaypoint
+{
+public:	
+  OffsetNavaidWaypoint(FGPositioned* aPos, Route* aOwner, double aRadial, double aDistNm);
+
+  OffsetNavaidWaypoint(Route* aOwner);
+  
+  virtual SGGeod position() const
+    { return _geod; }
+
+protected:
+  virtual void initFromProperties(SGPropertyNode_ptr aProp);
+  virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
+  
+  virtual std::string type() const
+    { return "offset-navaid"; }
+    
+private:
+  void init();
+  
+  SGGeod _geod;
+  double _radial; // true, degrees
+  double _distanceNm;
+};
+
+/**
+ * Waypoint based upon a runway. 
+ * Runways are handled specially in various places, so it's cleaner
+ * to be able to distuinguish them from other navaid waypoints
+ */
+class RunwayWaypt : public Waypt
+{
+public:
+  RunwayWaypt(FGRunway* aPos, Route* aOwner);
+  
+  RunwayWaypt(Route* aOwner);
+  
+  virtual SGGeod position() const;
+  
+  virtual FGPositioned* source() const;
+    
+  virtual std::string ident() const;
+
+  FGRunway* runway() const
+    { return _runway; }
+
+protected:	
+  virtual std::string type() const
+    { return "runway"; }
+  
+  virtual void initFromProperties(SGPropertyNode_ptr aProp);
+  virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
+
+private:
+  FGRunway* _runway;
+};
+
+class Hold : public BasicWaypt
+{
+public:
+  Hold(const SGGeod& aPos, const std::string& aIdent, Route* aOwner);
+  
+  Hold(Route* aOwner);
+  
+  void setHoldRadial(double aInboundRadial);
+  void setHoldDistance(double aDistanceNm);
+  void setHoldTime(double aTimeSec);
+  
+  void setRightHanded();
+  void setLeftHanded();
+  
+  double inboundRadial() const
+  { return _bearing; }
+  
+  bool isLeftHanded() const
+  { return !_righthanded; }
+  
+  bool isDistance() const
+  { return _isDistance; }
+  
+  double timeOrDistance() const
+  { return _holdTD;}
+  
+protected:
+  virtual void initFromProperties(SGPropertyNode_ptr aProp);
+  virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
+  
+  virtual std::string type() const
+    { return "hold"; }
+    
+private:
+  double _bearing;
+  bool _righthanded;
+  bool _isDistance;
+  double _holdTD;
+};
+
+class HeadingToAltitude : public Waypt
+{
+public:
+  HeadingToAltitude(Route* aOwner, const std::string& aIdent, double aMagHdg);
+  
+  HeadingToAltitude(Route* aOwner);
+  
+  virtual void initFromProperties(SGPropertyNode_ptr aProp);
+  virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
+  
+  virtual std::string type() const
+    { return "hdgToAlt"; }
+
+  virtual SGGeod position() const
+    { return SGGeod(); }
+    
+  virtual std::string ident() const
+    { return _ident; }
+    
+  double headingDegMagnetic() const
+    { return _magHeading; }
+  
+private:
+  std::string _ident;
+  double _magHeading;
+};
+
+class DMEIntercept : public Waypt
+{
+public:
+  DMEIntercept(Route* aOwner, const std::string& aIdent, const SGGeod& aPos,
+    double aCourseDeg, double aDistanceNm);
+  
+  DMEIntercept(Route* aOwner);
+  
+  virtual void initFromProperties(SGPropertyNode_ptr aProp);
+  virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
+  
+  virtual std::string type() const
+    { return "dmeIntercept"; }
+
+  virtual SGGeod position() const
+    { return _pos; }
+    
+  virtual std::string ident() const
+    { return _ident; }
+  
+  double courseDegMagnetic() const
+    { return _magCourse; }
+    
+  double dmeDistanceNm() const
+    { return _dmeDistanceNm; }
+    
+private:
+  std::string _ident;
+  SGGeod _pos;
+  double _magCourse;
+  double _dmeDistanceNm;
+};
+
+class RadialIntercept : public Waypt
+{
+public:
+  RadialIntercept(Route* aOwner, const std::string& aIdent, const SGGeod& aPos,
+    double aCourseDeg, double aRadialDeg);
+  
+  RadialIntercept(Route* aOwner);
+  
+  virtual void initFromProperties(SGPropertyNode_ptr aProp);
+  virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
+  
+  virtual std::string type() const
+    { return "radialIntercept"; }
+
+  virtual SGGeod position() const
+    { return _pos; }
+    
+  virtual std::string ident() const
+    { return _ident; }
+  
+  double courseDegMagnetic() const
+    { return _magCourse; }
+    
+  double radialDegMagnetic() const
+    { return _radial; }
+    
+private:
+  std::string _ident;
+  SGGeod _pos;
+  double _magCourse;
+  double _radial;
+};
+
+
+/** 
+ * Represent ATC radar vectored segment. Common at the end of published
+ * missed approach procedures, and from STAR arrival points to final approach
+ */
+class ATCVectors : public Waypt
+{
+public:
+  ATCVectors(Route* aOwner, FGAirport* aFacility);
+  virtual ~ATCVectors();
+  
+  ATCVectors(Route* aOwner);
+  
+  virtual void initFromProperties(SGPropertyNode_ptr aProp);
+  virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
+  
+  virtual std::string type() const
+    { return "vectors"; }
+
+  virtual SGGeod position() const;
+    
+  virtual std::string ident() const;
+  
+private:
+  /**
+   * ATC facility. Using an airport here is incorrect, since often arrivals
+   * facilities will be shared between several nearby airports, but it
+   * suffices until we have a proper facility representation
+   */
+  FGAirportRef _facility;
+};  
+  
+} // of namespace flighgear
+
+#endif // of FG_WAYPOINT_HXX

src/Scripting/NasalSys.cxx

@@ -33,6 +33,7 @@
 #include <Main/util.hxx>
 #include <Scenery/scenery.hxx>
 #include <Navaids/navrecord.hxx>
+#include <Navaids/procedure.hxx>
 
 #include "NasalSys.hxx"
 
@@ -613,6 +614,28 @@ static naRef f_airportinfo(naContext c, naRef me, int argc, naRef* args)
           HASHSET("ils_frequency_mhz", 17, naNum(rwy->ILS()->get_freq() / 100.0));
         }
         
+        std::vector<flightgear::SID*> sids(rwy->getSIDs());
+        naRef sidVec = naNewVector(c);
+        
+        for (unsigned int s=0; s < sids.size(); ++s) {
+          naRef procId = naStr_fromdata(naNewString(c),
+                    const_cast<char *>(sids[s]->ident().c_str()),
+                    sids[s]->ident().length());
+          naVec_append(sidVec, procId);
+        }
+        HASHSET("sids", 4, sidVec); 
+        
+        std::vector<flightgear::STAR*> stars(rwy->getSTARs());
+        naRef starVec = naNewVector(c);
+      
+        for (unsigned int s=0; s < stars.size(); ++s) {
+          naRef procId = naStr_fromdata(naNewString(c),
+                    const_cast<char *>(stars[s]->ident().c_str()),
+                    stars[s]->ident().length());
+          naVec_append(starVec, procId);
+        }
+        HASHSET("stars", 5, starVec); 
+
 #undef HASHSET
         naHash_set(rwys, rwyid, rwydata);
     }