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(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
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;
}
/**
* Calculates the location (loc_out) you would have, after being at
* a certain start location (loc) with a certain Bearing and going straight
* forward for a certain Distance.
* @param loc Current location
* @param Bearing Current bearing
* @param Distance Distance to predict
* @param loc_out Future location
*/
GeoPoint
FindLatitudeLongitude(GeoPoint loc, Angle Bearing,
fixed Distance)
{
assert(!negative(Distance));
if (!positive(Distance))
return loc;
GeoPoint loc_out;
Distance *= fixed_inv_earth_r;
fixed sinDistance, cosDistance;
sin_cos(Distance, &sinDistance, &cosDistance);
fixed sinBearing, cosBearing;
Bearing.sin_cos(sinBearing, cosBearing);
fixed sinLatitude, cosLatitude;
loc.Latitude.sin_cos(sinLatitude, cosLatitude);
loc_out.Latitude = Angle::radians(earth_asin(sinLatitude * cosDistance + cosLatitude
* sinDistance * cosBearing));
fixed result;
if (cosLatitude == fixed_zero)
result = loc.Longitude.value_radians();
else {
result = loc.Longitude.value_radians() +
earth_asin(sinBearing * sinDistance / cosLatitude);
}
loc_out.Longitude = Angle::radians(result);
loc_out.normalize(); // ensure longitude is within -180:180
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return loc_out;
}
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;
}
static inline
fixed earth_distance_function(const fixed a) {
if (!positive(a))
return fixed_zero;
#ifdef FIXED_MATH
// static const fixed fixed_shrink(fixed_two/(1<<(EXPAND_BITS*2)));
// acos(1-x) = 2*asin(sqrt(x/2))
// acos(1-2*x) = 2*asin(sqrt(x))
// = 2*atan2(sqrt(x), sqrt(fixed_one-x));
return fixed_two*earth_asin(sqrt(a)/(1<<fixed::accurate_cordic_shift));
#else
return acos(fixed_one-fixed_two*a);
#endif
}
GeoPoint
FindLatitudeLongitude(const GeoPoint loc, const Angle bearing,
fixed distance)
{
assert(!negative(distance));
if (!positive(distance))
return loc;
GeoPoint loc_out;
distance *= fixed_inv_earth_r;
const auto scd = sin_cos(distance);
const fixed sin_distance = scd.first, cos_distance = scd.second;
const auto scb = bearing.SinCos();
const fixed sin_bearing = scb.first, cos_bearing = scb.second;
const auto scl = loc.latitude.SinCos();
const fixed sin_latitude = scl.first, cos_latitude = scl.second;
loc_out.latitude = Angle::Radians(earth_asin(
sin_latitude * cos_distance + cos_latitude * sin_distance * cos_bearing));
fixed result = loc.longitude.Radians();
if (cos_latitude != fixed_zero)
result += earth_asin(sin_bearing * sin_distance / cos_latitude);
loc_out.longitude = Angle::Radians(result);
loc_out.Normalize(); // ensure longitude is within -180:180
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return loc_out;
}
