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