C++ GeoPoint::IsValid方法代码示例

fixed
DoubleDistance(const GeoPoint &loc1, const GeoPoint &loc2,
               const GeoPoint &loc3)
{
  assert(loc1.IsValid());
  assert(loc2.IsValid());
  assert(loc3.IsValid());

  const fixed cos_loc1_lat = loc1.latitude.cos();
  const fixed cos_loc2_lat = loc2.latitude.cos();
  const fixed cos_loc3_lat = 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)
    + SmallMult(cos_loc1_lat, cos_loc2_lat) * sqr(sl21);
  const fixed a23 = sqr(s32)
    + SmallMult(cos_loc2_lat, cos_loc3_lat) * sqr(sl32);

#ifdef INSTRUMENT_TASK
  count_distbearing++;
#endif

  return (2 * REARTH) *
    (EarthDistance(a12) + EarthDistance(a23)).Radians();
}

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;
}

inline double
OrderedTask::ScanDistanceMin(const GeoPoint &location, bool full)
{
  if (!full && location.IsValid() && last_min_location.IsValid() &&
      DistanceIsSignificant(location, last_min_location)) {
    const TaskWaypoint *active = GetActiveTaskPoint();
    if (active != nullptr) {
      const GeoPoint &target = active->GetWaypoint().location;
      const unsigned last_distance =
        (unsigned)last_min_location.Distance(target);
      const unsigned cur_distance =
        (unsigned)location.Distance(target);

      /* do the full scan only if the distance to the active task
         point has changed by more than 5%, otherwise we don't expect
         any relevant changes */
      if (last_distance < 2000 || cur_distance < 2000 ||
          last_distance * 20 >= cur_distance * 21 ||
          cur_distance * 20 >= last_distance * 21)
        full = true;
    }
  }

  if (full) {
    RunDijsktraMin(location);
    last_min_location = location;
  }

  return task_points.front()->ScanDistanceMin();
}

inline GeoVector
TaskLeg::GetRemainingVector(const GeoPoint &ref) const
{
  switch (destination.GetActiveState()) {
  case OrderedTaskPoint::AFTER_ACTIVE:
    // this leg totally included
    return GetPlannedVector();

  case OrderedTaskPoint::CURRENT_ACTIVE: {
    // this leg partially included

    if (!ref.IsValid())
      /* if we don't have a GPS fix yet, we fall back to the "planned"
         vector unless this task leg has already been achieved */
      return destination.HasEntered()
        ? GeoVector::Zero()
        : GetPlannedVector();

    return memo_remaining.calc(ref, destination.GetLocationRemaining());
  }

  case OrderedTaskPoint::BEFORE_ACTIVE:
    // this leg not included
    return GeoVector::Zero();
  }

  gcc_unreachable();
  assert(false);
  return GeoVector::Invalid();
}

static void
WriteEvent(JSON::ObjectWriter &object, const char *name,
           const BrokenDateTime &time, const GeoPoint &location)
{
  if (time.IsPlausible() || location.IsValid())
    object.WriteElement(name, WriteEventAttributes, time, location);
}

GlideResult
TaskSolution::GlideSolutionRemaining(const GeoPoint &location,
                                     const GeoPoint &target,
                                     const fixed target_elevation,
                                     const fixed altitude,
                                     const SpeedVector &wind,
                                     const GlideSettings &settings,
                                     const GlidePolar &polar)
{
  assert(location.IsValid());
  assert(target.IsValid());

  GlideState gs(location.DistanceBearing(target),
                target_elevation, altitude, wind);

  return MacCready::Solve(settings, polar, gs);
}

inline GeoVector
TaskLeg::GetTravelledVector(const GeoPoint &ref) const
{
  switch (destination.GetActiveState()) {
  case OrderedTaskPoint::BEFORE_ACTIVE:
    if (!GetOrigin())
      return GeoVector::Zero();

    // this leg totally included
    return memo_travelled.calc(GetOrigin()->GetLocationTravelled(),
                               destination.GetLocationTravelled());

  case OrderedTaskPoint::CURRENT_ACTIVE:
    // this leg partially included
    if (!GetOrigin())
      return GeoVector(0,
                       ref.IsValid()
                       ? ref.Bearing(destination.GetLocationRemaining())
                       : Angle::Zero());

    if (destination.HasEntered())
      return memo_travelled.calc(GetOrigin()->GetLocationTravelled(),
                                 destination.GetLocationTravelled());
    else if (!ref.IsValid())
      return GeoVector::Zero();
    else
      return memo_travelled.calc(GetOrigin()->GetLocationTravelled(), ref);

  case OrderedTaskPoint::AFTER_ACTIVE:
    if (!GetOrigin())
      return GeoVector::Zero();

    // this leg may be partially included
    if (GetOrigin()->HasEntered())
      return memo_travelled.calc(GetOrigin()->GetLocationTravelled(),
                                 ref.IsValid()
                                 ? ref
                                 : destination.GetLocationTravelled());

    return GeoVector::Zero();
  }

  gcc_unreachable();
  assert(false);
  return GeoVector::Invalid();
}

/*
$GPRMB,<1>,,,,<5>,,,,,<10>,<11>,,<13>*hh<CR><LF>

<1>  Position Valid (A = valid, V = invalid)
<5>  Destination waypoint identifier, three digits
     (leading zeros will be transmitted)
<10> Range from present position to distination waypoint, format XXXX.X,
     nautical miles (leading zeros will be transmitted)
<11> Bearing from present position to destination waypoint, format XXX.X,
     degrees true (leading zeros will be transmitted)
<13> Arrival flag <A = arrival, V = not arrival)
*/
static bool
FormatGPRMB(char *buffer, size_t buffer_size, const GeoPoint& here,
            const AGeoPoint &destination)
{
  if (!here.IsValid() || !destination.IsValid())
    return false;

  const GeoVector vector(here, destination);
  const bool has_arrived = vector.distance < 1000; // < 1km ?

  snprintf(buffer, buffer_size, "GPRMB,%c,,,,,,,,,%06.1f,%04.1f,%c",
           here.IsValid() ? 'A' : 'V',
           (double)Units::ToUserUnit(vector.distance, Unit::NAUTICAL_MILES),
           (double)vector.bearing.Degrees(),
           has_arrived ? 'A' : 'V');

  return true;
}

fixed
UnorderedTask::ScanDistanceRemaining(const GeoPoint &location)
{
  TaskPoint *tp = GetActiveTaskPoint();
  if (tp == nullptr || !location.IsValid())
    return fixed(0);

  return tp->Distance(location);
}

inline fixed
TaskLeg::GetScoredDistance(const GeoPoint &ref) const
{
  if (!GetOrigin())
    return fixed(0);

  switch (destination.GetActiveState()) {
  case OrderedTaskPoint::BEFORE_ACTIVE:
    // this leg totally included
    return std::max(fixed(0),
                    GetOrigin()->GetLocationScored().Distance(destination.GetLocationScored())
                    - GetOrigin()->ScoreAdjustment()-destination.ScoreAdjustment());

  case OrderedTaskPoint::CURRENT_ACTIVE:
    // this leg partially included
    if (destination.HasEntered()) {
      return std::max(fixed(0),
                      GetOrigin()->GetLocationScored().Distance(destination.GetLocationScored())
                      - GetOrigin()->ScoreAdjustment()-destination.ScoreAdjustment());
    } else if (ref.IsValid())
      return std::max(fixed(0),
                      ref.ProjectedDistance(GetOrigin()->GetLocationScored(),
                                            destination.GetLocationScored())
                      -GetOrigin()->ScoreAdjustment());
    else
      return fixed(0);

  case OrderedTaskPoint::AFTER_ACTIVE:
    // this leg may be partially included
    if (GetOrigin()->HasEntered() && ref.IsValid()) {
      return std::max(fixed(0),
                      memo_travelled.calc(GetOrigin()->GetLocationScored(),
                                          ref).distance
                      -GetOrigin()->ScoreAdjustment());
    }

    return fixed(0);
  }

  gcc_unreachable();
  assert(false);
  return fixed(0);
}

void
ChartProjection::Set(const PixelRect &rc, const OrderedTask &task,
                     const GeoPoint &fallback_loc)
{
  GeoPoint center = task.GetTaskCenter();
  if (!center.IsValid())
    center = fallback_loc;

  const fixed radius = std::max(fixed(10000), task.GetTaskRadius());
  Set(rc, center, radius);
}

void
FlatProjection::SetCenter(const GeoPoint &_center)
{
    assert(_center.IsValid());

    center = _center;

    cos = center.latitude.fastcosine() * fixed_scale;
    r_cos = 1. / cos;
    approx_scale = Unproject(FlatGeoPoint(0,-1)).DistanceS(Unproject(FlatGeoPoint(0,1))) / 2;
}

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 Push(lua_State *L, GeoPoint value) {
    if (value.IsValid()) {
      lua_newtable(L);

      lua_newtable(L);
      SetField(L, -2, "__tostring", l_GeoPoint_tostring);
      lua_setmetatable(L, -2);

      SetField(L, -2, "longitude", value.longitude);
      SetField(L, -2, "latitude", value.latitude);
    } else
      lua_pushnil(L);
  }

static void
WriteEventAttributes(TextWriter &writer,
                     const BrokenDateTime &time, const GeoPoint &location)
{
  JSON::ObjectWriter object(writer);

  if (time.IsPlausible()) {
    NarrowString<64> buffer;
    FormatISO8601(buffer.buffer(), time);
    object.WriteElement("time", JSON::WriteString, buffer);
  }

  if (location.IsValid())
    JSON::WriteGeoPointAttributes(object, location);
}