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;
}
fixed
ProjectedDistance(GeoPoint loc1, GeoPoint loc2, GeoPoint loc3)
{
Angle dist_AD; Angle crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
if (!positive(dist_AD.value_native()))
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return fixed_zero;
Angle dist_AB; Angle crs_AB;
DistanceBearingS(loc1, loc2, &dist_AB, &crs_AB);
if (!positive(dist_AB.value_native()))
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return fixed_zero;
// 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();
const fixed XTD(earth_asin(sindist_AD * (crs_AD - crs_AB).sin())); // cross track distance
fixed sinXTD, cosXTD;
sin_cos(XTD, &sinXTD, &cosXTD);
// along track distance
const fixed ATD(earth_asin(sqrt(sindist_AD * sindist_AD - sinXTD * sinXTD) / cosXTD));
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return ATD * fixed_earth_r;
}
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;
}
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);
}
double
ProjectedDistanceS(const GeoPoint &loc1, const GeoPoint &loc2,
const GeoPoint &loc3)
{
Angle dist_AD, crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
if (!dist_AD.IsPositive())
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return 0;
Angle dist_AB, crs_AB;
DistanceBearingS(loc1, loc2, &dist_AB, &crs_AB);
if (!dist_AB.IsPositive())
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return 0;
// The "along track distance", along_track_distance, the distance from A along the
// course towards B to the point abeam D
const auto sindist_AD = dist_AD.sin();
const auto cross_track_distance =
Angle::asin(sindist_AD * (crs_AD - crs_AB).sin());
const auto sc = cross_track_distance.SinCos();
const auto sinXTD = sc.first, cosXTD = sc.second;
// along track distance
const Angle along_track_distance =
Angle::asin(Cathetus(sindist_AD, sinXTD) / cosXTD);
return FAISphere::AngleToEarthDistance(along_track_distance);
}
void
DistanceBearing(GeoPoint loc1, GeoPoint loc2, fixed *Distance, Angle *Bearing)
{
if (Distance != NULL) {
Angle distance_angle;
DistanceBearingS(loc1, loc2, &distance_angle, Bearing);
*Distance = distance_angle.value_radians() * fixed_earth_r;
} else
DistanceBearingS(loc1, loc2, NULL, Bearing);
}
void
DistanceBearing(const GeoPoint &loc1, const GeoPoint &loc2,
fixed *distance, Angle *bearing)
{
if (distance != nullptr) {
Angle distance_angle;
DistanceBearingS(loc1, loc2, &distance_angle, bearing);
*distance = AngleToEarthDistance(distance_angle);
} else
DistanceBearingS(loc1, loc2, nullptr, bearing);
}
void
DistanceBearing(const GeoPoint loc1, const GeoPoint loc2,
fixed *distance, Angle *bearing)
{
if (distance != NULL) {
Angle distance_angle;
DistanceBearingS(loc1, loc2, &distance_angle, bearing);
*distance = distance_angle.Radians() * fixed_earth_r;
} else
DistanceBearingS(loc1, loc2, NULL, bearing);
}
void
DistanceBearingS(const GeoPoint &loc1, const GeoPoint &loc2,
double *distance, Angle *bearing)
{
if (distance != nullptr) {
Angle distance_angle;
DistanceBearingS(loc1, loc2, &distance_angle, bearing);
*distance = FAISphere::AngleToEarthDistance(distance_angle);
} else
DistanceBearingS(loc1, loc2, (Angle *)nullptr, bearing);
}
fixed
ProjectedDistance(const GeoPoint &loc1, const GeoPoint &loc2,
const GeoPoint &loc3)
{
Angle dist_AD, crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
if (!positive(dist_AD.Native()))
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return fixed(0);
Angle dist_AB, crs_AB;
DistanceBearingS(loc1, loc2, &dist_AB, &crs_AB);
if (!positive(dist_AB.Native()))
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return fixed(0);
// The "along track distance", along_track_distance, the distance from A along the
// course towards B to the point abeam D
const fixed sindist_AD = dist_AD.sin();
const Angle cross_track_distance =
EarthASin(SmallMult(sindist_AD, (crs_AD - crs_AB).sin()));
const auto sc = cross_track_distance.SinCos();
const fixed sinXTD = sc.first, cosXTD = sc.second;
// along track distance
const Angle along_track_distance =
EarthASin(sqrt(sqr(sindist_AD) - sqr(sinXTD)) / cosXTD);
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return AngleToEarthDistance(along_track_distance);
}
fixed
ProjectedDistance(const GeoPoint loc1, const GeoPoint loc2, const GeoPoint loc3)
{
Angle dist_AD, crs_AD;
DistanceBearingS(loc1, loc3, &dist_AD, &crs_AD);
if (!positive(dist_AD.Native()))
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return fixed_zero;
Angle dist_AB, crs_AB;
DistanceBearingS(loc1, loc2, &dist_AB, &crs_AB);
if (!positive(dist_AB.Native()))
/* workaround: new sine implementation may return small non-zero
values for sin(0) */
return fixed_zero;
// The "along track distance", along_track_distance, the distance from A along the
// course towards B to the point abeam D
const fixed sindist_AD = dist_AD.sin();
const fixed cross_track_distance =
earth_asin(sindist_AD * (crs_AD - crs_AB).sin());
const auto sc = sin_cos(cross_track_distance);
const fixed sinXTD = sc.first, cosXTD = sc.second;
// along track distance
const fixed along_track_distance =
earth_asin(sqrt(sindist_AD * sindist_AD - sinXTD * sinXTD) / cosXTD);
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return along_track_distance * fixed_earth_r;
}
