fly/flightgear
1
0
Fork 0
Code Activity

Relocate implementation of geocRadialIntersection

Browse source
This commit is contained in:
James Turner 2015-12-10 15:06:33 -06:00
parent d7a680e848
commit 545b347a16
2 changed files with 68 additions and 66 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

67
src/Instrumentation/rnav_waypt_controller.cxx
View file

@ -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:

67
src/Navaids/routePath.cxx
View file

@ -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;