Convert waypoint::CourseAndDistance users to use SGGeodesy helper functions.

src/AIModel/AIAircraft.cxx

@@ -644,16 +644,10 @@ bool FGAIAircraft::leadPointReached(FGAIFlightPlan::waypoint* curr) {
 
 
 bool FGAIAircraft::aiTrafficVisible() {
-    double userLatitude  = fgGetDouble("/position/latitude-deg");
+  SGGeod userPos(SGGeod::fromDeg(fgGetDouble("/position/longitude-deg"), 
-    double userLongitude = fgGetDouble("/position/longitude-deg");
+    fgGetDouble("/position/latitude-deg")));
-    double course, distance;
+  
-
+  return (SGGeodesy::distanceNm(userPos, pos) <= TRAFFICTOAIDISTTODIE);
-    SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0);
-    SGWayPoint user (userLongitude, userLatitude, 0);
-
-    user.CourseAndDistance(current, &course, &distance);
-
-    return ((distance * SG_METER_TO_NM) <= TRAFFICTOAIDISTTODIE);
 }
 
 

src/AIModel/AIFlightPlanCreate.cxx

@@ -286,7 +286,7 @@ void FGAIFlightPlan::createTakeoffTaxi(FGAIAircraft *ac, bool firstFlight,
 		char buffer[10];
 		snprintf (buffer, 10, "%d", node);
 		FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node);
-    waypoint* wpt = createOnGround(ac, buffer, tn->geod(), apt->getElevation(), ac->getPerformance()->vTaxi());
+    waypoint* wpt = createOnGround(ac, buffer, tn->getGeod(), apt->getElevation(), ac->getPerformance()->vTaxi());
     wpt->routeIndex = route;
 		waypoints.push_back(wpt);
   }
@@ -358,7 +358,7 @@ void FGAIFlightPlan::createLandingTaxi(FGAIAircraft *ac, FGAirport *apt,
     char buffer[10];
     snprintf (buffer, 10, "%d", node);
     FGTaxiNode *tn = gn->findNode(node);
-    waypoint* wpt = createOnGround(ac, buffer, tn->geod(), apt->getElevation(), ac->getPerformance()->vTaxi());
+    waypoint* wpt = createOnGround(ac, buffer, tn->getGeod(), apt->getElevation(), ac->getPerformance()->vTaxi());
     wpt->routeIndex = route;
     waypoints.push_back(wpt);
   }

src/AIModel/AIFlightPlanCreateCruise.cxx

@@ -51,37 +51,30 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
   intVec nodes;
   int tmpNode, prevNode;
 
-
+  SGGeoc dep(SGGeoc::fromDegM(deplon, deplat, 100.0));
-  SGWayPoint first (deplon,
+  SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0));
-		    deplat,
+  
-		    100);
+  SGVec3d a = SGVec3d::fromGeoc(dep);
-  SGWayPoint sec (arrlon,
+  SGVec3d b = SGVec3d::fromGeoc(arr);
-		  arrlat,
-		  100);
-  double course, distance;
-  first.CourseAndDistance(sec, &course, &distance);
-  distance *= SG_METER_TO_NM;
-
-  SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(deplon, deplat, 1));
-  SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1));
   SGVec3d _cross = cross(b, a);
 
   double angle = sgACos(dot(a, b));
   tmpNode = 0;
   for (double ang = 0.0; ang < angle; ang += 0.05)
-    {
+  {
       sgdVec3 newPos;
       sgdMat4 matrix;
       //cerr << "Angle = " << ang << endl;
       sgdMakeRotMat4(matrix, ang, _cross.sg());
       for(int j = 0; j < 3; j++)
-	{
+      {
-	  newPos[j] =0.0;
+        newPos[j] =0.0;
-	  for (int k = 0; k<3; k++)
+        for (int k = 0; k<3; k++)
-	    {
+        {
-	      newPos[j] += matrix[j][k]*a[k];
+          newPos[j] += matrix[j][k]*a[k];
-	    }
+        }
-	}
+      }
+      
       //cerr << "1"<< endl;
       SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
 
@@ -91,17 +84,12 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
       prevNode = tmpNode;
       tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
 
-      double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude  ();
+      FGNode* node = globals->get_airwaynet()->findNode(tmpNode);
-      double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
+      SGGeoc nodePos(SGGeoc::fromGeod(node->getPosition()));
-      SGWayPoint curr(midlat,
+    
-      	              midlon,
+      if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geoc, nodePos) < 25000)) {
-      	              100);
+        nodes.push_back(tmpNode);
-      SGWayPoint node(nodelat,
+      }
-      	              nodelon,
-      	              100);
-      curr.CourseAndDistance(node, &course, &distance);
-      if ((distance < 25000) && (tmpNode != prevNode))
-      	nodes.push_back(tmpNode);
     }
 
     intVecIterator i = nodes.begin();

src/Airports/gnnode.cxx

@@ -44,6 +44,26 @@ bool sortByLength(FGTaxiSegment *a, FGTaxiSegment *b) {
  *************************************************************************/
 
 
+void FGTaxiNode::setLatitude (double val)
+{
+  geod.setLatitudeDeg(val);
+}
+
+void FGTaxiNode::setLongitude(double val)
+{
+  geod.setLongitudeDeg(val);
+}
+
+void FGTaxiNode::setLatitude (const string& val)
+{
+  geod.setLatitudeDeg(processPosition(val));
+}
+
+void FGTaxiNode::setLongitude(const string& val)
+{
+  geod.setLongitudeDeg(processPosition(val));
+}
+  
 void FGTaxiNode::sortEndSegments(bool byLength)
 {
   if (byLength)

src/Airports/gnnode.hxx

@@ -22,22 +22,17 @@
 #include <simgear/compiler.h>
 #include <simgear/math/sg_geodesy.hxx>
 
-using std::string;
-using std::vector;
-
 class FGTaxiSegment;
-typedef vector<FGTaxiSegment*>  FGTaxiSegmentVector;
+typedef std::vector<FGTaxiSegment*>  FGTaxiSegmentVector;
 typedef FGTaxiSegmentVector::iterator FGTaxiSegmentVectorIterator;
 
 bool sortByHeadingDiff(FGTaxiSegment *a, FGTaxiSegment *b);
 bool sortByLength     (FGTaxiSegment *a, FGTaxiSegment *b);
-double processPosition(const string& pos);
 
 class FGTaxiNode 
 {
 private:
-  double lat;
+  SGGeod geod;
-  double lon;
   int index;
 
   bool isOnRunway;
@@ -51,8 +46,6 @@ private:
 
 public:
   FGTaxiNode() :
-      lat (0.0),
-      lon (0.0),
       index(0),
       isOnRunway(false),
       holdType(0),
@@ -63,8 +56,7 @@ public:
 };
 
   FGTaxiNode(const FGTaxiNode &other) :
-      lat(other.lat),
+      geod(other.geod),
-      lon(other.lon),
       index(other.index),
       isOnRunway(other.isOnRunway),
       holdType(other.holdType),
@@ -77,8 +69,7 @@ public:
 
 FGTaxiNode &operator =(const FGTaxiNode &other)
 {
-   lat          = other.lat;
+   geod          = other.geod;
-   lon          = other.lon;
    index        = other.index;
    isOnRunway   = other.isOnRunway;
    holdType     = other.holdType;
@@ -90,10 +81,10 @@ FGTaxiNode &operator =(const FGTaxiNode &other)
 };
 
   void setIndex(int idx)                  { index = idx;                 };
-  void setLatitude (double val)           { lat = val;                   };
+  void setLatitude (double val);
-  void setLongitude(double val)           { lon = val;                   };
+  void setLongitude(double val);
-  void setLatitude (const string& val)    { lat = processPosition(val);  };
+  void setLatitude (const std::string& val);
-  void setLongitude(const string& val)    { lon = processPosition(val);  };
+  void setLongitude(const std::string& val);
   void addSegment(FGTaxiSegment *segment) { next.push_back(segment);     };
   void setHoldPointType(int val)          { holdType = val;              };
   void setOnRunway(bool val)              { isOnRunway = val;            };
@@ -106,10 +97,10 @@ FGTaxiNode &operator =(const FGTaxiNode &other)
   FGTaxiSegment *getPreviousSegment() { return previousSeg;  };
 
   double getPathScore() { return pathScore; };
-  double getLatitude() { return lat;};
+  double getLatitude() { return geod.getLatitudeDeg();};
-  double getLongitude(){ return lon;};
+  double getLongitude(){ return geod.getLongitudeDeg();};
 
-  SGGeod geod() const { return SGGeod::fromDeg(lon, lat); }
+  const SGGeod& getGeod() const { return geod; }
 
   int getIndex() { return index; };
   int getHoldPointType() { return holdType; };
@@ -124,7 +115,7 @@ FGTaxiNode &operator =(const FGTaxiNode &other)
 
 };
 
-typedef vector<FGTaxiNode*> FGTaxiNodeVector;
+typedef std::vector<FGTaxiNode*> FGTaxiNodeVector;
 typedef FGTaxiNodeVector::iterator FGTaxiNodeVectorIterator;
 
 #endif

src/Airports/groundnetwork.cxx

@@ -79,14 +79,7 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
 // doing this.
 void FGTaxiSegment::setTrackDistance()
 {
-  //double course;
+  length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
-  SGWayPoint first  (start->getLongitude(),
-		     start->getLatitude(),
-		     0);
-  SGWayPoint second (end->getLongitude(),
-		     end->getLatitude(),
-		     0);
-  first.CourseAndDistance(second, &course, &length);
 }
 
 
@@ -301,35 +294,26 @@ void FGGroundNetwork::init()
 
 int FGGroundNetwork::findNearestNode(const SGGeod& aGeod)
 {
-  return findNearestNode(aGeod.getLatitudeDeg(), aGeod.getLongitudeDeg());
+  double minDist = HUGE_VAL;
+  int index = -1;
+  
+  for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); itr++)
+  {
+    double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
+    if (d < minDist)
+    {
+      minDist = d;
+      index = (*itr)->getIndex();
+      //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
+    }
+  }
+  
+  return index;
 }
 
 int FGGroundNetwork::findNearestNode(double lat, double lon)
 {
-  double minDist = HUGE_VAL;
+  return findNearestNode(SGGeod::fromDeg(lon, lat));
-  double dist;
-  int index;
-  SGWayPoint first  (lon,
-		     lat,
-		     0);
-  
-  for (FGTaxiNodeVectorIterator 
-	 itr = nodes.begin();
-       itr != nodes.end(); itr++)
-    {
-      double course;
-      SGWayPoint second ((*itr)->getLongitude(),
-			 (*itr)->getLatitude(),
-			 0);
-      first.CourseAndDistance(second, &course, &dist);
-      if (dist < minDist)
-	{
-	  minDist = dist;
-	  index = (*itr)->getIndex();
-	  //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
-	}
-    }
-  return index;
 }
 
 FGTaxiNode *FGGroundNetwork::findNode(int idx)
@@ -609,24 +593,21 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
   double mindist = HUGE_VAL;
   if (activeTraffic.size()) 
     {
-      double course, dist, bearing, minbearing;
+      double course, dist, bearing, minbearing, az2;
-      SGWayPoint curr  (lon,
+      SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
-			lat,
-			alt);
       //TrafficVector iterator closest;
       closest = current;
       for (TrafficVectorIterator i = activeTraffic.begin(); 
    	   i != activeTraffic.end(); i++)
    	{
-   	  if (i != current) {
+   	  if (i == current) {
-   	    //SGWayPoint curr  (lon,
+        continue;
-  	    //	      lat,
+      }
-  	    //	      alt);
+      
-   	    SGWayPoint other    (i->getLongitude  (),
+        SGGeod other(SGGeod::fromDegM(i->getLongitude(),
-   				 i->getLatitude (),
+          i->getLatitude(), i->getAltitude()));
-   				 i->getAltitude  ());
+        SGGeodesy::inverse(other, curr, course, az2, dist);
-   	    other.CourseAndDistance(curr, &course, &dist);
+        bearing = fabs(heading-course);
-   	    bearing = fabs(heading-course);
    	    if (bearing > 180)
    	      bearing = 360-bearing;
    	    if ((dist < mindist) && (bearing < 60.0))
@@ -635,7 +616,6 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
    		closest = i;
    		minbearing = bearing;
    	      }
-   	  }
    	}
       //Check traffic at the tower controller
       if (towerController->hasActiveTraffic())
@@ -644,16 +624,12 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
 	       i != towerController->getActiveTraffic().end(); i++)
 	    {
 	      //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
-	      //SGWayPoint curr  (lon,
+	      SGGeod other(SGGeod::fromDegM(i->getLongitude(),
-	      //		  lat,
+          i->getLatitude(), i->getAltitude()));
-	      //		  alt);
+        SGGeodesy::inverse(other, curr, course, az2, dist);
-	      SGWayPoint other    (i->getLongitude  (),
+        bearing = fabs(heading-course);
-				   i->getLatitude (),
-				   i->getAltitude  ());
-	      other.CourseAndDistance(curr, &course, &dist);
-	      bearing = fabs(heading-course);
 	      if (bearing > 180)
-		bearing = 360-bearing;
+          bearing = 360-bearing;
 	      if ((dist < mindist) && (bearing < 60.0))
 		{
 		  mindist = dist;
@@ -666,10 +642,9 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
       // Finally, check UserPosition
       double userLatitude  = fgGetDouble("/position/latitude-deg");
       double userLongitude = fgGetDouble("/position/longitude-deg");
-      SGWayPoint user    (userLongitude,
+      SGGeod user(SGGeod::fromDeg(userLatitude,userLongitude));
-			  userLatitude,
+      SGGeodesy::inverse(user, curr, course, az2, dist);
-			  alt); // Alt is not really important here. 
+      
-      user.CourseAndDistance(curr, &course, &dist);
       bearing = fabs(heading-course);
       if (bearing > 180)
 	bearing = 360-bearing;
@@ -750,10 +725,8 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat,
   }
   current = i;
   current->setHoldPosition(false);
-  SGWayPoint curr  (lon,
+  SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
-		    lat,
+  
-		    alt);
-  double course, dist, bearing, minbearing;
   for (i = activeTraffic.begin(); 
        i != activeTraffic.end(); i++)
     {
@@ -762,20 +735,16 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat,
   	  int node = current->crosses(this, *i);
   	  if (node != -1)
   	    {
+          FGTaxiNode* taxiNode = findNode(node);
+          
   	      // Determine whether it's save to continue or not. 
   	      // If we have a crossing route, there are two possibilities:
   	      // 1) This is an interestion
   	      // 2) This is oncoming two-way traffic, using the same taxiway.
   	      //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
-  	      SGWayPoint nodePos(findNode(node)->getLongitude  (),
+  	     
-  				 findNode(node)->getLatitude   (),
+        SGGeod other(SGGeod::fromDegM(i->getLongitude(), i->getLatitude(), i->getAltitude()));
-  				 alt);
+        bool needsToWait;
-
-  	      SGWayPoint other    (i->getLongitude  (),
-  				   i->getLatitude (),
-  				   i->getAltitude  ());
-  	      //other.CourseAndDistance(curr, &course, &dist);
-  	      bool needsToWait;
 	      bool opposing;
   	      if (current->isOpposing(this, *i, node))
   		{
@@ -793,8 +762,7 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat,
   	      else 
   		{
 		  opposing = false;
-  		  other.CourseAndDistance(nodePos, &course, &dist);
+  		  if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
-  		  if (dist > 200) // 2.0*i->getRadius())
   		    {
   		      needsToWait = false; 
   		      //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue " 
@@ -806,8 +774,9 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat,
   		      //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
   		    }
   		}
-  	      curr.CourseAndDistance(nodePos, &course, &dist);
+      
-  	      if (!(i->hasHoldPosition()))
+      double dist = SGGeodesy::distanceM(curr, taxiNode->getGeod());
+      if (!(i->hasHoldPosition()))
   		{
 		  
   		  if ((dist < 200) && //2.5*current->getRadius()) && 

src/Navaids/awynet.cxx

@@ -47,11 +47,18 @@ FGNode::FGNode()
 {
 }
 
+FGNode::FGNode(double lt, double ln, int idx, std::string id) :
+  ident(id),
+  geod(SGGeod::fromDeg(ln, lt)),
+  index(idx)
+{
+}
+
 bool FGNode::matches(string id, double lt, double ln)
 {
   if ((ident == id) &&
-      (fabs(lt - lat) < 1.0) &&
+      (fabs(lt - geod.getLatitudeDeg()) < 1.0) &&
-      (fabs(ln - lon) < 1.0))
+      (fabs(ln - geod.getLongitudeDeg()) < 1.0))
     return true;
   else
     return false;
@@ -92,15 +99,7 @@ void FGAirway::setEnd(node_map *nodes)
 // doing this.
 void FGAirway::setTrackDistance()
 {
-  double course;
+  length = SGGeodesy::distanceM(start->getPosition(), end->getPosition());
-  SGWayPoint first  (start->getLongitude(),
-		     start->getLatitude(),
-		     0);
-  SGWayPoint second (end->getLongitude(),
-		     end->getLatitude(),
-		     0);
-  first.CourseAndDistance(second, &course, &length);
-
 }
 
 /***************************************************************************

src/Navaids/awynet.hxx

@@ -50,24 +50,21 @@ class FGNode
 {
 private:
   std::string ident;
-  double lat;
+  SGGeod geod;
-  double lon;
   int index;
   FGAirwayPointerVector next; // a vector to all the segments leaving from this node
 
 public:
   FGNode();
-  FGNode(double lt, double ln, int idx, std::string id) { lat = lt; lon = ln; index = idx; ident = id;};
+  FGNode(double lt, double ln, int idx, std::string id);
 
   void setIndex(int idx)                  { index = idx;};
-  void setLatitude (double val)           { lat = val;};
-  void setLongitude(double val)           { lon = val;};
-  //void setLatitude (const std::string& val)           { lat = processPosition(val);  };
-  //void setLongitude(const std::string& val)           { lon = processPosition(val);  };
   void addAirway(FGAirway *segment) { next.push_back(segment); };
 
-  double getLatitude() { return lat;};
+  double getLatitude() { return geod.getLatitudeDeg();};
-  double getLongitude(){ return lon;};
+  double getLongitude(){ return geod.getLongitudeDeg();};
+
+  const SGGeod& getPosition() {return geod;}
 
   int getIndex() { return index; };
   std::string getIdent() { return ident; };