X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2FSGGeodesy.cxx;h=d86cf0e30e303b9f83b026aa0cfd7ecfe3cc711d;hb=7bdb530440d1dadc991f305edb1b70ec85f27451;hp=05d76f0ed408e0b8577a44f05e25bb7c4d11ef91;hpb=3e023b77dde9526d7efe8af00fa57c2143893983;p=simgear.git

diff --git a/simgear/math/SGGeodesy.cxx b/simgear/math/SGGeodesy.cxx
index 05d76f0e..d86cf0e3 100644
--- a/simgear/math/SGGeodesy.cxx
+++ b/simgear/math/SGGeodesy.cxx
@@ -22,6 +22,8 @@
 #include <cmath>
 
 #include <simgear/structure/exception.hxx>
+#include <simgear/debug/logstream.hxx>
+
 #include "SGMath.hxx"
 
 // These are hard numbers from the WGS84 standard.  DON'T MODIFY
@@ -57,7 +59,7 @@ const double SGGeodesy::POLRAD = _POLRAD;
 #define E2 fabs(1 - _SQUASH*_SQUASH)
 static double a = _EQURAD;
 static double ra2 = 1/(_EQURAD*_EQURAD);
-static double e = sqrt(E2);
+//static double e = sqrt(E2);
 static double e2 = E2;
 static double e4 = E2*E2;
 
@@ -79,11 +81,30 @@ SGGeodesy::SGCartToGeod(const SGVec3<double>& cart, SGGeod& geod)
   double Y = cart(1);
   double Z = cart(2);
   double XXpYY = X*X+Y*Y;
+  if( XXpYY + Z*Z < 25 ) {
+    // This function fails near the geocenter region, so catch that special case here.
+    // Define the innermost sphere of small radius as earth center and return the 
+    // coordinates 0/0/-EQURAD. It may be any other place on geoide's surface,
+    // the Northpole, Hawaii or Wentorf. This one was easy to code ;-)
+    geod.setLongitudeRad( 0.0 );
+    geod.setLongitudeRad( 0.0 );
+    geod.setElevationM( -EQURAD );
+    return;
+  }
+    
   double sqrtXXpYY = sqrt(XXpYY);
   double p = XXpYY*ra2;
   double q = Z*Z*(1-e2)*ra2;
   double r = 1/6.0*(p+q-e4);
   double s = e4*p*q/(4*r*r*r);
+/* 
+  s*(2+s) is negative for s = [-2..0]
+  slightly negative values for s due to floating point rounding errors
+  cause nan for sqrt(s*(2+s))
+  We can probably clamp the resulting parable to positive numbers
+*/
+  if( s >= -2.0 && s <= 0.0 )
+    s = 0.0;
   double t = pow(1+s+sqrt(s*(2+s)), 1/3.0);
   double u = r*(1+t+1/t);
   double v = sqrt(u*u+e4*q);
@@ -315,7 +336,7 @@ static int _geo_inverse_wgs_84( double lat1, double lon1, double lat2,
     double sinphi2 = sin(phi2), cosphi2 = cos(phi2);
 	
     if( (fabs(lat1-lat2) < testv && 
-	 ( fabs(lon1-lon2) < testv) || fabs(lat1-90.0) < testv ) )
+	 ( fabs(lon1-lon2) < testv)) || (fabs(lat1-90.0) < testv ) )
     {	
 	// TWO STATIONS ARE IDENTICAL : SET DISTANCE & AZIMUTHS TO ZERO */
 	*az1 = 0.0; *az2 = 0.0; *s = 0.0;
@@ -490,15 +511,16 @@ SGGeodesy::advanceRadM(const SGGeoc& geoc, double course, double distance,
     result.setLongitudeRad(geoc.getLongitudeRad());
   } else {
     double tmp = SGMiscd::clip(sin(course) * sinDistance / cosLat, -1, 1);
-    double lon = SGMiscd::normalizeAngle(geoc.getLongitudeRad() - asin( tmp ));
-    result.setLongitudeRad(lon);
+    double lon = SGMiscd::normalizeAngle(-geoc.getLongitudeRad() - asin( tmp ));
+    result.setLongitudeRad(-lon);
   }
 }
 
 double
 SGGeodesy::courseRad(const SGGeoc& from, const SGGeoc& to)
 {
-  double diffLon = to.getLongitudeRad() - from.getLongitudeRad();
+  //double diffLon = to.getLongitudeRad() - from.getLongitudeRad();
+  double diffLon = from.getLongitudeRad() - to.getLongitudeRad();
 
   double sinLatFrom = sin(from.getLatitudeRad());
   double cosLatFrom = cos(from.getLatitudeRad());
@@ -521,7 +543,7 @@ SGGeodesy::courseRad(const SGGeoc& from, const SGGeoc& to)
 }
 
 double
-SGGeodesy::distanceM(const SGGeoc& from, const SGGeoc& to)
+SGGeodesy::distanceRad(const SGGeoc& from, const SGGeoc& to)
 {
   // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
   //            cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
@@ -531,5 +553,85 @@ SGGeodesy::distanceM(const SGGeoc& from, const SGGeoc& to)
   double tmp2 = sin(0.5*(from.getLongitudeRad() - to.getLongitudeRad()));
   double square = tmp1*tmp1 + cosLatFrom*cosLatTo*tmp2*tmp2;
   double s = SGMiscd::min(sqrt(SGMiscd::max(square, 0)), 1);
-  return 2 * asin(s) * SG_RAD_TO_NM * SG_NM_TO_METER;
+  return 2 * asin(s);
+}
+
+
+double
+SGGeodesy::distanceM(const SGGeoc& from, const SGGeoc& to)
+{
+  return distanceRad(from, to) * SG_RAD_TO_NM * SG_NM_TO_METER;
+}
+
+bool 
+SGGeodesy::radialIntersection(const SGGeoc& a, double r1, 
+    const SGGeoc& b, double r2, SGGeoc& result)
+{
+  // implementation of
+  // http://williams.best.vwh.net/avform.htm#Intersection
+
+  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 crs12 = SGGeodesy::courseRad(a, b),
+         crs21 = SGGeodesy::courseRad(b, a);
+
+  double sinLat1 = sin(a.getLatitudeRad());
+  double cosLat1 = cos(a.getLatitudeRad());
+  double sinDst12 = sin(dst12);
+  double cosDst12 = cos(dst12);
+  
+  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, "SGGeodesy::radialIntersection: infinity of intersections");
+    return false;
+  }
+
+  if ((sin(ang1)*sin(ang2))<0.0) {
+    SG_LOG(SG_GENERAL, SG_WARN, "SGGeodesy::radialIntersection: 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));
+  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());
+  return true;
+}
+    
+bool
+SGGeodesy::radialIntersection(const SGGeod& a, double aRadial, 
+    const SGGeod& b, double bRadial, SGGeod& result)
+{
+  SGGeoc r;
+  bool ok = radialIntersection(SGGeoc::fromGeod(a), aRadial, 
+    SGGeoc::fromGeod(b), bRadial, r);
+  if (!ok) {
+    return false;
+  }
+  
+  result = SGGeod::fromGeoc(r);
+  return true;
 }