/** * 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) { assert(loc1.IsValid()); assert(loc2.IsValid()); 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 Angle dlon = loc2.longitude - loc1.longitude; if (distance) { const fixed s1 = (loc2.latitude - loc1.latitude).accurate_half_sin(); const fixed s2 = dlon.accurate_half_sin(); const fixed a = sqr(s1) + SmallMult(cos_lat1, cos_lat2) * sqr(s2); Angle distance2 = EarthDistance(a); assert(!negative(distance2.Native())); *distance = distance2; } if (bearing) { // speedup for fixed since this is one call const auto sc = dlon.SinCos(); const fixed sin_dlon = sc.first, cos_dlon = sc.second; const fixed y = SmallMult(sin_dlon, cos_lat2); const fixed x = SmallMult(cos_lat1, sin_lat2) - SmallMult(sin_lat1, cos_lat2, cos_dlon); *bearing = (x == fixed(0) && y == fixed(0)) ? Angle::Zero() : Angle::FromXY(x, y).AsBearing(); } #ifdef INSTRUMENT_TASK count_distbearing++; #endif }
void DistanceBearingS(const GeoPoint &loc1, const GeoPoint &loc2, Angle *distance, Angle *bearing) { assert(loc1.IsValid()); assert(loc2.IsValid()); const auto sc1 = loc1.latitude.SinCos(); auto sin_lat1 = sc1.first, cos_lat1 = sc1.second; const auto sc2 = loc2.latitude.SinCos(); auto sin_lat2 = sc2.first, cos_lat2 = sc2.second; const Angle dlon = loc2.longitude - loc1.longitude; if (distance) { const auto s1 = (loc2.latitude - loc1.latitude).accurate_half_sin(); const auto s2 = dlon.accurate_half_sin(); const auto a = Square(s1) + cos_lat1 * cos_lat2 * Square(s2); Angle distance2 = EarthDistance(a); assert(!distance2.IsNegative()); *distance = distance2; } if (bearing) { // speedup for fixed since this is one call const auto sc = dlon.SinCos(); const auto sin_dlon = sc.first, cos_dlon = sc.second; const auto y = sin_dlon * cos_lat2; const auto x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon; *bearing = (x == fixed(0) && y == fixed(0)) ? Angle::Zero() : Angle::FromXY(x, y).AsBearing(); } #ifdef INSTRUMENT_TASK count_distbearing++; #endif }
void DistanceBearingS(const GeoPoint &loc1, const GeoPoint &loc2, Angle *distance, Angle *bearing) { assert(loc1.IsValid()); assert(loc2.IsValid()); const auto sc1 = loc1.latitude.SinCos(); auto sin_lat1 = sc1.first, cos_lat1 = sc1.second; const auto sc2 = loc2.latitude.SinCos(); auto sin_lat2 = sc2.first, cos_lat2 = sc2.second; const Angle dlon = loc2.longitude - loc1.longitude; if (distance) { const auto s1 = (loc2.latitude - loc1.latitude).accurate_half_sin(); const auto s2 = dlon.accurate_half_sin(); const auto a = Square(s1) + cos_lat1 * cos_lat2 * Square(s2); Angle distance2 = EarthDistance(a); assert(!distance2.IsNegative()); *distance = distance2; } if (bearing) { const auto sc = dlon.SinCos(); const auto sin_dlon = sc.first, cos_dlon = sc.second; const auto y = sin_dlon * cos_lat2; const auto x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon; *bearing = (x == 0 && y == 0) ? Angle::Zero() : Angle::FromXY(x, y).AsBearing(); } }