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