fixed
DoubleDistance(GeoPoint loc1, GeoPoint loc2, GeoPoint loc3)
{
const fixed cloc1Latitude = loc1.Latitude.cos();
const fixed cloc2Latitude = loc2.Latitude.cos();
const fixed cloc3Latitude = loc3.Latitude.cos();
const fixed s21 = (loc2.Latitude - loc1.Latitude).accurate_half_sin();
const fixed sl21 = (loc2.Longitude - loc1.Longitude).accurate_half_sin();
const fixed s32 = (loc3.Latitude - loc2.Latitude).accurate_half_sin();
const fixed sl32 = (loc3.Longitude - loc2.Longitude).accurate_half_sin();
const fixed a12 = sqr(s21) + cloc1Latitude * cloc2Latitude * sqr(sl21);
const fixed a23 = sqr(s32) + cloc2Latitude * cloc3Latitude * sqr(sl32);
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
return fixed_double_earth_r *
(earth_distance_function(a12) + earth_distance_function(a23));
}
/**
* Calculates the distance and bearing of two locations
* @param loc1 Location 1
* @param loc2 Location 2
* @param Distance Pointer to the distance variable
* @param Bearing Pointer to the bearing variable
*/
static void
DistanceBearingS(const GeoPoint loc1, const GeoPoint loc2,
Angle *Distance, Angle *Bearing)
{
fixed cos_lat1, sin_lat1;
loc1.Latitude.sin_cos(sin_lat1, cos_lat1);
fixed cos_lat2, sin_lat2;
loc2.Latitude.sin_cos(sin_lat2, cos_lat2);
const fixed dlon = (loc2.Longitude - loc1.Longitude).value_radians();
if (Distance) {
const fixed s1 = (loc2.Latitude - loc1.Latitude).accurate_half_sin();
const fixed s2 = accurate_half_sin(dlon);
const fixed a = sqr(s1) + cos_lat1 * cos_lat2 * sqr(s2);
fixed distance2 = earth_distance_function(a);
assert(!negative(distance2));
*Distance = Angle::radians(distance2);
}
if (Bearing) {
fixed sin_dlon, cos_dlon;
// speedup for fixed since this is one call
sin_cos(dlon, &sin_dlon, &cos_dlon);
const fixed y = sin_dlon * cos_lat2;
const fixed x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon;
*Bearing = (x == fixed_zero && y == fixed_zero)
? Angle::native(fixed_zero)
: Angle::radians(atan2(y, x)).as_bearing();
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
}
/**
* Calculates the distance and bearing of two locations
* @param loc1 Location 1
* @param loc2 Location 2
* @param Distance Pointer to the distance variable
* @param Bearing Pointer to the bearing variable
*/
static void
DistanceBearingS(const GeoPoint loc1, const GeoPoint loc2,
Angle *distance, Angle *bearing)
{
const auto sc1 = loc1.latitude.SinCos();
fixed sin_lat1 = sc1.first, cos_lat1 = sc1.second;
const auto sc2 = loc2.latitude.SinCos();
fixed sin_lat2 = sc2.first, cos_lat2 = sc2.second;
const fixed dlon = (loc2.longitude - loc1.longitude).Radians();
if (distance) {
const fixed s1 = (loc2.latitude - loc1.latitude).accurate_half_sin();
const fixed s2 = accurate_half_sin(dlon);
const fixed a = sqr(s1) + cos_lat1 * cos_lat2 * sqr(s2);
fixed distance2 = earth_distance_function(a);
assert(!negative(distance2));
*distance = Angle::Radians(distance2);
}
if (bearing) {
// speedup for fixed since this is one call
const auto sc = sin_cos(dlon);
const fixed sin_dlon = sc.first, cos_dlon = sc.second;
const fixed y = sin_dlon * cos_lat2;
const fixed x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon;
*bearing = (x == fixed_zero && y == fixed_zero)
? Angle::Zero()
: Angle::Radians(atan2(y, x)).AsBearing();
}
#ifdef INSTRUMENT_TASK
count_distbearing++;
#endif
}