Пример #1
0
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));
}
Пример #2
0
/**
 * 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
}
Пример #3
0
/**
 * 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
}