fixed
CrossTrackError(GeoPoint loc1, GeoPoint loc2, GeoPoint loc3, GeoPoint *loc4)
{
Angle dist_AD; Angle crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
Angle dist_AB; Angle crs_AB;
DistanceBearingS(loc1, loc2, &dist_AB, &crs_AB);
// The "along track distance", ATD, the distance from A along the
// course towards B to the point abeam D
const fixed sindist_AD = dist_AD.sin();
// cross track distance
const fixed XTD(earth_asin(sindist_AD * (crs_AD - crs_AB).sin()));
if (loc4) {
fixed sinXTD, cosXTD;
sin_cos(XTD, &sinXTD, &cosXTD);
// along track distance
const fixed ATD(earth_asin(sqrt(sindist_AD * sindist_AD - sinXTD * sinXTD)
/ cosXTD));
*loc4 = IntermediatePoint(loc1, loc2, ATD, dist_AB.value_radians());
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
// units
return XTD * fixed_earth_r;
}
GeoPoint
Middle(const GeoPoint &a, const GeoPoint &b)
{
// TODO: optimize this naive approach
const fixed distance = Distance(a, b);
return IntermediatePoint(a, b, Half(distance));
}
fixed
CrossTrackError(const GeoPoint &loc1, const GeoPoint &loc2,
const GeoPoint &loc3, GeoPoint *loc4)
{
Angle dist_AD, crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
Angle dist_AB, crs_AB;
DistanceBearingS(loc1, loc2, &dist_AB, &crs_AB);
// The "along track distance", ATD, the distance from A along the
// course towards B to the point abeam D
const fixed sindist_AD = dist_AD.sin();
// cross track distance
const Angle cross_track_distance =
EarthASin(SmallMult(sindist_AD, (crs_AD - crs_AB).sin()));
if (loc4) {
const auto sc = cross_track_distance.SinCos();
const fixed sinXTD = sc.first, cosXTD = sc.second;
const Angle along_track_distance =
EarthASin(sqrt(sqr(sindist_AD) - sqr(sinXTD)) / cosXTD);
*loc4 = IntermediatePoint(loc1, loc2, along_track_distance, dist_AB);
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return AngleToEarthDistance(cross_track_distance);
}
fixed
CrossTrackError(const GeoPoint loc1, const GeoPoint loc2,
const GeoPoint loc3, GeoPoint *loc4)
{
Angle dist_AD, crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
Angle dist_AB, crs_AB;
DistanceBearingS(loc1, loc2, &dist_AB, &crs_AB);
// The "along track distance", ATD, the distance from A along the
// course towards B to the point abeam D
const fixed sindist_AD = dist_AD.sin();
// cross track distance
const fixed cross_track_distance =
earth_asin(sindist_AD * (crs_AD - crs_AB).sin());
if (loc4) {
const auto sc = sin_cos(cross_track_distance);
const fixed sinXTD = sc.first, cosXTD = sc.second;
const fixed along_track_distance =
earth_asin(sqrt(sindist_AD * sindist_AD - sinXTD * sinXTD) / cosXTD);
*loc4 = IntermediatePoint(loc1, loc2, along_track_distance, dist_AB.Radians());
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return cross_track_distance * fixed_earth_r;
}
GeoPoint
IntermediatePoint(const GeoPoint loc1, const GeoPoint loc2, const fixed dthis)
{
const fixed dtotal = ::Distance(loc1, loc2);
if (dthis >= dtotal)
return loc2;
return IntermediatePoint(loc1, loc2, dthis * fixed_inv_earth_r,
dtotal * fixed_inv_earth_r);
}
GeoPoint
IntermediatePoint(const GeoPoint &loc1, const GeoPoint &loc2,
const fixed dthis)
{
const fixed dtotal = ::Distance(loc1, loc2);
if (dthis >= dtotal)
return loc2;
return IntermediatePoint(loc1, loc2,
EarthDistanceToAngle(dthis),
EarthDistanceToAngle(dtotal));
}
// finds cross track error in meters and closest point p4 between p3 and
// desired track p1-p2.
// very slow function!
double CrossTrackError(double lon1, double lat1,
double lon2, double lat2,
double lon3, double lat3,
double *lon4, double *lat4) {
double dist_AD, crs_AD;
DistanceBearing(lat1, lon1, lat3, lon3, &dist_AD, &crs_AD);
dist_AD/= (RAD_TO_DEG * 111194.9267); crs_AD*= DEG_TO_RAD;
double dist_AB, crs_AB;
DistanceBearing(lat1, lon1, lat2, lon2, &dist_AB, &crs_AB);
dist_AB/= (RAD_TO_DEG * 111194.9267); crs_AB*= DEG_TO_RAD;
lat1 *= DEG_TO_RAD;
lat2 *= DEG_TO_RAD;
lat3 *= DEG_TO_RAD;
lon1 *= DEG_TO_RAD;
lon2 *= DEG_TO_RAD;
lon3 *= DEG_TO_RAD;
double XTD; // cross track distance
double ATD; // along track distance
// The "along track distance", ATD, the distance from A along the
// course towards B to the point abeam D
double sindist_AD = sin(dist_AD);
XTD = asin(sindist_AD*sin(crs_AD-crs_AB));
double sinXTD = sin(XTD);
ATD = asin(sqrt( sindist_AD*sindist_AD - sinXTD*sinXTD )/cos(XTD));
if (lon4 && lat4) {
IntermediatePoint(lon1, lat1, lon2, lat2, ATD, dist_AB,
lon4, lat4);
}
// units
XTD *= (RAD_TO_DEG * 111194.9267);
return XTD;
}
double CrossTrackError(GEOPOINT loc1, GEOPOINT loc2, GEOPOINT loc3,
GEOPOINT *loc4)
{
double dist_AD, crs_AD;
DistanceBearing(loc1, loc3, &dist_AD, &crs_AD);
dist_AD/= (RAD_TO_DEG * 111194.9267); crs_AD*= DEG_TO_RAD;
double dist_AB, crs_AB;
DistanceBearing(loc1, loc2, &dist_AB, &crs_AB);
dist_AB/= (RAD_TO_DEG * 111194.9267); crs_AB*= DEG_TO_RAD;
loc1.Latitude *= DEG_TO_RAD;
loc2.Latitude *= DEG_TO_RAD;
loc3.Latitude *= DEG_TO_RAD;
loc1.Longitude *= DEG_TO_RAD;
loc2.Longitude *= DEG_TO_RAD;
loc3.Longitude *= DEG_TO_RAD;
double XTD; // cross track distance
double ATD; // along track distance
// The "along track distance", ATD, the distance from A along the
// course towards B to the point abeam D
double sindist_AD = sin(dist_AD);
XTD = asin(sindist_AD*sin(crs_AD-crs_AB));
double sinXTD = sin(XTD);
ATD = asin(sqrt( sindist_AD*sindist_AD - sinXTD*sinXTD )/cos(XTD));
if (loc4) {
IntermediatePoint(loc1, loc2, ATD, dist_AB, loc4);
}
// units
XTD *= (RAD_TO_DEG * 111194.9267);
return XTD;
}
