Relocate implementation of geocRadialIntersection

diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx
index d783a206b11a4be70336feb9fe0797e852cfdbc6..560cf9e12c312ebcd2741aa876770f235e9f2c99 100644 (file)
--- a/src/Instrumentation/rnav_waypt_controller.cxx
+++ b/src/Instrumentation/rnav_waypt_controller.cxx
@@ -32,71 +32,8 @@ extern double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const S
 namespace flightgear
 {
 
-// 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 sinLat1 = sin(a.getLatitudeRad());
-  double cosLat1 = cos(a.getLatitudeRad());
-  double sinDst12 = sin(dst12);
-  double cosDst12 = cos(dst12);
-  
-  double crs12 = SGGeodesy::courseRad(a, b),
-    crs21 = SGGeodesy::courseRad(b, a);
-    
-
-  // normalise to -pi .. pi range
-  double ang1 = SGMiscd::normalizeAngle(crs13-crs12);
-  double ang2 = SGMiscd::normalizeAngle(crs21-crs23);
-    
-  if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) {
-    SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: infinity of intersections");
-    return false;
-  }
-  
-  if ((sin(ang1)*sin(ang2))<0.0) {
-   // SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: intersection ambiguous:"
-    //       << ang1 << " " << ang2 << " sin1 " << sin(ang1) << " sin2 " << sin(ang2));
-    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;
-}
+// declared in routePath.cxx
+bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
 
 /**
  * Helper function Cross track error:

diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx
index 721c6d26a48c680ec61455aaf89ae9c947356fe9..cae2fd7110129dbba2ae49208296df1d7f0d3042 100644 (file)
--- a/src/Navaids/routePath.cxx
+++ b/src/Navaids/routePath.cxx
@@ -16,7 +16,72 @@
 #include <Navaids/positioned.hxx>
 
 namespace flightgear {
-  bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
+
+  // 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 sinLat1 = sin(a.getLatitudeRad());
+    double cosLat1 = cos(a.getLatitudeRad());
+    double sinDst12 = sin(dst12);
+    double cosDst12 = cos(dst12);
+
+    double crs12 = SGGeodesy::courseRad(a, b),
+      crs21 = SGGeodesy::courseRad(b, a);
+
+
+    // normalise to -pi .. pi range
+    double ang1 = SGMiscd::normalizeAngle(crs13-crs12);
+    double ang2 = SGMiscd::normalizeAngle(crs21-crs23);
+
+    if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) {
+      SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: infinity of intersections");
+      return false;
+    }
+
+    if ((sin(ang1)*sin(ang2))<0.0) {
+     // SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: intersection ambiguous:"
+      //       << ang1 << " " << ang2 << " sin1 " << sin(ang1) << " sin2 " << sin(ang2));
+      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;
+  }
 }
 
 using namespace flightgear;