#include <cmath>
+#include <simgear/sg_inlines.h>
#include <simgear/structure/exception.hxx>
+#include <simgear/debug/logstream.hxx>
+
#include "SGMath.hxx"
// These are hard numbers from the WGS84 standard. DON'T MODIFY
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.setLatitudeRad( 0.0 );
+ geod.setElevationM( -EQURAD );
+ return;
+ }
+
double sqrtXXpYY = sqrt(XXpYY);
double p = XXpYY*ra2;
double q = Z*Z*(1-e2)*ra2;
return ret == 0;
}
+SGGeod
+SGGeodesy::direct(const SGGeod& p1, double course1, double distance)
+{
+ double lat2, lon2, course2;
+ int ret = _geo_direct_wgs_84(p1.getLatitudeDeg(), p1.getLongitudeDeg(),
+ course1, distance, &lat2, &lon2, &course2);
+ if (ret != 0) {
+ throw sg_exception("_geo_direct_wgs_84 failed");
+ }
+
+ SGGeod p2;
+ p2.setLatitudeDeg(lat2);
+ p2.setLongitudeDeg(lon2);
+ p2.setElevationM(0);
+ return p2;
+}
+
+
// given lat1, lon1, lat2, lon2, calculate starting and ending
// az1, az2 and distance (s). Lat, lon, and azimuth are in degrees.
// distance in meters
} else if( fabs(cosphi1) < testv ) {
// initial point is polar
int k = _geo_inverse_wgs_84( lat2,lon2,lat1,lon1, az1,az2,s );
- k = k; // avoid compiler error since return result is unused
+ SG_UNUSED(k);
+
b = *az1; *az1 = *az2; *az2 = b;
return 0;
} else if( fabs(cosphi2) < testv ) {
double _lon1 = lon1 + 180.0f;
int k = _geo_inverse_wgs_84( lat1, lon1, lat1, _lon1,
az1, az2, s );
- k = k; // avoid compiler error since return result is unused
+ SG_UNUSED(k);
+
*s /= 2.0;
*az2 = *az1 + 180.0;
if( *az2 > 360.0 ) *az2 -= 360.0;
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());
}
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))
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;
}