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 }