TESTS = $(check_PROGRAMS)
SGMathTest_SOURCES = SGMathTest.cxx
-SGMathTest_LDADD = libsgmath.a -lsgstructure $(base_LIBS)
+SGMathTest_LDADD = \
+ libsgmath.a \
+ $(top_builddir)/simgear/debug/libsgdebug.a \
+ $(top_builddir)/simgear/structure/libsgstructure.a \
+ $(base_LIBS)
+
SGGeometryTest_SOURCES = SGGeometryTest.cxx
-SGGeometryTest_LDADD = libsgmath.a -lsgstructure $(base_LIBS)
+SGGeometryTest_LDADD = \
+ libsgmath.a \
+ $(top_builddir)/simgear/debug/libsgdebug.a \
+ $(top_builddir)/simgear/structure/libsgstructure.a \
+ $(base_LIBS)
+
lib_LIBRARIES = libsgmath.a
include_HEADERS = \
#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
{
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;
+}
static double courseRad(const SGGeoc& from, const SGGeoc& to);
static double distanceRad(const SGGeoc& from, const SGGeoc& to);
static double distanceM(const SGGeoc& from, const SGGeoc& to);
+
+ /**
+ * compute the intersection of two (true) radials (in degrees), or return false
+ * if no intersection culd be computed.
+ */
+ static bool radialIntersection(const SGGeoc& a, double aRadial,
+ const SGGeoc& b, double bRadial, SGGeoc& result);
+
+ static bool radialIntersection(const SGGeod& a, double aRadial,
+ const SGGeod& b, double bRadial, SGGeod& result);
};
#endif
if (!equivalent(cart0, cart1))
return false;
+ // test course / advance routines
+ // uses examples from Williams aviation formulary
+ SGGeoc lax = SGGeoc::fromRadM(-2.066470, 0.592539, 10.0);
+ SGGeoc jfk = SGGeoc::fromRadM(-1.287762, 0.709186, 10.0);
+
+ double distNm = SGGeodesy::distanceRad(lax, jfk) * SG_RAD_TO_NM;
+ std::cout << "distance is " << distNm << std::endl;
+ if (0.5 < fabs(distNm - 2144)) // 2144 nm
+ return false;
+
+ double crsDeg = SGGeodesy::courseRad(lax, jfk) * SG_RADIANS_TO_DEGREES;
+ std::cout << "course is " << crsDeg << std::endl;
+ if (0.5 < fabs(crsDeg - 66)) // 66 degrees
+ return false;
+
+ SGGeoc adv;
+ SGGeodesy::advanceRadM(lax, crsDeg * SG_DEGREES_TO_RADIANS, 100 * SG_NM_TO_METER, adv);
+ std::cout << "lon:" << adv.getLongitudeRad() << ", lat:" << adv.getLatitudeRad() << std::endl;
+
+ if (0.01 < fabs(adv.getLongitudeRad() - (-2.034206)) ||
+ 0.01 < fabs(adv.getLatitudeRad() - 0.604180))
+ return false;
+
return true;
}