Ejemplo n.º 1
0
GeoPoint
FindLatitudeLongitudeS(const GeoPoint &loc, const Angle bearing,
                       double distance)
{
  assert(loc.IsValid());
  assert(distance >= 0);

  if (distance <= 0)
    return loc;

  const Angle distance_angle = FAISphere::EarthDistanceToAngle(distance);

  const auto scd = distance_angle.SinCos();
  const auto sin_distance = scd.first, cos_distance = scd.second;

  const auto scb = bearing.SinCos();
  const auto sin_bearing = scb.first, cos_bearing = scb.second;

  const auto scl = loc.latitude.SinCos();
  const auto sin_latitude = scl.first, cos_latitude = scl.second;

  GeoPoint loc_out;
  loc_out.latitude = Angle::asin(sin_latitude * cos_distance
                                 + cos_latitude * sin_distance * cos_bearing);

  loc_out.longitude = loc.longitude +
    Angle::FromXY(cos_distance - sin_latitude * loc_out.latitude.sin(),
                  sin_bearing * sin_distance * cos_latitude);

  loc_out.Normalize(); // ensure longitude is within -180:180

  return loc_out;
}
Ejemplo n.º 2
0
static void
DrawLandableRunway(Canvas &canvas, const PixelPoint &pt,
                   const Angle angle, double radius, double width)
{
    if (radius <= 0)
        return;

    const auto sc = angle.SinCos();
    const auto x = sc.first, y = sc.second;
    int lx = iround(2 * x * radius) & ~0x1;  // make it a even number
    int ly = iround(2 * y * radius) & ~0x1;
    int wx = iround(-y * width);
    int wy = iround(x * width);

    BulkPixelPoint runway[4];
    runway[0].x = pt.x        - (lx / 2) + (wx / 2);
    runway[0].y = pt.y        + (ly / 2) - (wy / 2);
    runway[1].x = runway[0].x            - wx;
    runway[1].y = runway[0].y            + wy;
    runway[2].x = runway[1].x + lx;
    runway[2].y = runway[1].y - ly;
    runway[3].x = runway[2].x            + wx;
    runway[3].y = runway[2].y            - wy;
    canvas.DrawTriangleFan(runway, ARRAY_SIZE(runway));
}
Ejemplo n.º 3
0
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 Angle cross_track_distance =
    EarthASin(SmallMult(sindist_AD, (crs_AD - crs_AB).sin()));

  if (loc4) {
    const auto sc = cross_track_distance.SinCos();
    const fixed sinXTD = sc.first, cosXTD = sc.second;

    const Angle along_track_distance =
      EarthASin(sqrt(sqr(sindist_AD) - sqr(sinXTD)) / cosXTD);

    *loc4 = IntermediatePoint(loc1, loc2, along_track_distance, dist_AB);
  }

#ifdef INSTRUMENT_TASK
  count_distbearing++;
#endif

  return AngleToEarthDistance(cross_track_distance);
}
Ejemplo n.º 4
0
void
FlatPoint::Rotate(const Angle angle)
{
  const fixed _x = x;
  const fixed _y = y;
  const auto sc = angle.SinCos();
  const fixed sa = sc.first, ca = sc.second;
  x = _x * ca - _y * sa;
  y = _x * sa + _y * ca;
}
Ejemplo n.º 5
0
  void Rotate(const Angle alpha) {
    const auto sc = alpha.SinCos();
    const fixed sin = sc.first, cos = sc.second;
#ifdef FIXED_MATH
    long s = sin.as_glfixed();
    long c = cos.as_glfixed();
#else
    long s = sin * (1<<16);
    long c = cos * (1<<16);
#endif
    Rotatex(s, c);
  }
Ejemplo n.º 6
0
GeoPoint
FindLatitudeLongitude(const GeoPoint &loc, const Angle bearing,
                      fixed distance)
{
  assert(loc.IsValid());

  assert(!negative(distance));
  if (!positive(distance))
    return loc;

  GeoPoint loc_out;

  const Angle distance_angle = EarthDistanceToAngle(distance);

  const auto scd = distance_angle.SinCos();
  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 = EarthASin(SmallMult(sin_latitude, cos_distance)
                               + SmallMult(cos_latitude, sin_distance,
                                           cos_bearing));

  loc_out.longitude = loc.longitude +
    Angle::FromXY(cos_distance - SmallMult(sin_latitude,
                                           loc_out.latitude.sin()),
                  SmallMult(sin_bearing, sin_distance, cos_latitude));

  loc_out.Normalize(); // ensure longitude is within -180:180

#ifdef INSTRUMENT_TASK
  count_distbearing++;
#endif

  return loc_out;
}
Ejemplo n.º 7
0
FlatPoint
FlatEllipse::Parametric(const fixed t) const
{
  const Angle at = (Angle::FullCircle() * t + theta_initial).AsDelta();

  const auto sc = at.SinCos();
  fixed sat = sc.first, cat = sc.second;

  FlatPoint res(a * cat, b * sat);
  res.Rotate(theta);
  res.Add(p);
  return res;
}
Ejemplo n.º 8
0
bool
FlatEllipse::IntersectExtended(const FlatPoint &pe, FlatPoint &i1,
                                FlatPoint &i2) const
{
  const FlatLine l_f1p(f1, pe);
  const FlatLine l_pf2(pe, f2);
  const Angle ang = l_f1p.angle();

  const fixed d = l_pf2.d() + std::max(a, b); // max line length

  const auto sc = ang.SinCos();
  fixed san = sc.first, can = sc.second;

  FlatLine e_l(pe, FlatPoint(pe.x + d * can, pe.y + d * san));
  // e_l is the line extended from p in direction of f1-p 
  
  return Intersect(e_l, i1, i2);
}
Ejemplo n.º 9
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)
{
  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
}
Ejemplo n.º 10
0
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
}
Ejemplo n.º 11
0
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(0);

  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(0);

  // 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 Angle cross_track_distance =
    EarthASin(SmallMult(sindist_AD, (crs_AD - crs_AB).sin()));

  const auto sc = cross_track_distance.SinCos();
  const fixed sinXTD = sc.first, cosXTD = sc.second;

  // along track distance
  const Angle along_track_distance =
    EarthASin(sqrt(sqr(sindist_AD) - sqr(sinXTD)) / cosXTD);

#ifdef INSTRUMENT_TASK
  count_distbearing++;
#endif

  return AngleToEarthDistance(along_track_distance);
}
Ejemplo n.º 12
0
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;
}
Ejemplo n.º 13
0
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();
  }
}
Ejemplo n.º 14
0
 Vector(Angle bearing, fixed norm) {
   auto sc = bearing.SinCos();
   x = sc.second * norm;
   y = sc.first * norm;
 }
Ejemplo n.º 15
0
 ZZX(const fixed mag, const Angle ang) {
   const auto sc = ang.SinCos();
   gps_east = sc.first * mag;
   gps_north = sc.second * mag;
 }