gcc_pure
static Angle
CalcIntermediateAngle(const SeeYouTurnpointInformation &turnpoint_infos,
const GeoPoint &location,
const GeoPoint &start,
const GeoPoint &previous,
const GeoPoint &next)
{
switch (turnpoint_infos.style) {
case SeeYouTurnpointInformation::FIXED:
return turnpoint_infos.angle12.Reciprocal();
case SeeYouTurnpointInformation::SYMMETRICAL:
break;
case SeeYouTurnpointInformation::TO_NEXT_POINT:
return next.Bearing(location);
case SeeYouTurnpointInformation::TO_PREVIOUS_POINT:
return previous.Bearing(location);
case SeeYouTurnpointInformation::TO_START_POINT:
return start.Bearing(location);
}
/* SYMMETRICAL is the fallback when the file contained an
invalid/unknown style */
const Angle ap = previous.Bearing(location);
const Angle an = next.Bearing(location);
return ap.HalfAngle(an).Reciprocal();
}
bool
FAITrianglePointValidator::IsFAIAngle(const GeoPoint &p0, const GeoPoint &p1,
const GeoPoint &p2, bool right)
{
const Angle a01 = p0.Bearing(p1);
const Angle a21 = p2.Bearing(p1);
const Angle diff = (a01 - a21).AsDelta();
if (right)
return (diff > min_fai_angle) && (diff < max_fai_angle);
else
return diff < -min_fai_angle && diff > -max_fai_angle;
}
/**
* Perform fast check to exclude point as from further consideration
* based on min/max possible values for any FAI triangle
* @param p0 point 1 of angle
* @param p1 point 2 of angle
* @param p2 point 3 of angle
* @param right. = 1 if angle is for right triangle, -1 if left triangle.
* @returns False if angle from three points is out of possible range for
* an FAI triangle.
*/
bool
IsFAIAngle(const GeoPoint &p0, const GeoPoint &p1, const GeoPoint &p2,
const fixed right)
{
const Angle a01 = p0.Bearing(p1);
const Angle a21 = p2.Bearing(p1);
const fixed diff = (a01 - a21).AsDelta().Degrees();
if (positive(right))
return (diff > min_fai_angle) && (diff < max_fai_angle);
else
return (diff < fixed(-1) * min_fai_angle) && (diff > fixed(-1)
* max_fai_angle);
}
bool
AirspaceFilterData::Match(const GeoPoint &location,
const FlatProjection &projection,
const AbstractAirspace &as) const
{
if (cls != AirspaceClass::AIRSPACECLASSCOUNT && as.GetType() != cls)
return false;
if (name_prefix != nullptr && !as.MatchNamePrefix(name_prefix))
return false;
if (!direction.IsNegative()) {
const auto closest = as.ClosestPoint(location, projection);
const auto bearing = location.Bearing(closest);
auto direction_error = (bearing - direction).AsDelta().AbsoluteDegrees();
if (direction_error > fixed(18))
return false;
}
if (!negative(distance)) {
const auto closest = as.ClosestPoint(location, projection);
const auto distance = location.Distance(closest);
if (distance > distance)
return false;
}
return true;
}
void
DeviceBlackboard::SetSimulatorLocation(const GeoPoint &location)
{
ScopeLock protect(mutex);
NMEAInfo &basic = simulator_data;
simulator.Touch(basic);
basic.track = location.Bearing(basic.location).Reciprocal();
basic.location = location;
ScheduleMerge();
}
void
AATPoint::GetTargetRangeRadial(fixed &range, fixed &radial) const
{
const fixed oldrange = range;
const GeoPoint fprev = GetPrevious()->GetLocationRemaining();
const GeoPoint floc = GetLocation();
const Angle radialraw = (floc.Bearing(GetTargetLocation()) -
fprev.Bearing(floc)).AsBearing();
const fixed d = floc.Distance(GetTargetLocation());
const fixed radius = floc.Distance(GetLocationMin());
const fixed rangeraw = min(fixed_one, d / radius);
radial = radialraw.AsDelta().Degrees();
const fixed rangesign = (fabs(radial) > fixed(90)) ?
fixed_minus_one : fixed_one;
range = rangeraw * rangesign;
if ((oldrange == fixed_zero) && (range == fixed_zero))
radial = fixed_zero;
}
RangeAndRadial
AATPoint::GetTargetRangeRadial(fixed oldrange) const
{
const GeoPoint fprev = GetPrevious()->GetLocationRemaining();
const GeoPoint floc = GetLocation();
const Angle radialraw = (floc.Bearing(GetTargetLocation()) -
fprev.Bearing(floc)).AsBearing();
Angle radial = radialraw.AsDelta();
fixed d = floc.Distance(GetTargetLocation());
if (radial < -Angle::QuarterCircle() || radial > Angle::QuarterCircle())
d = -d;
const fixed radius = negative(d)
? floc.Distance(GetLocationMin())
: floc.Distance(GetLocationMax());
const fixed range = Clamp(d / radius, fixed(-1), fixed(1));
if (oldrange == fixed(0) && range == fixed(0))
radial = Angle::Zero();
return RangeAndRadial{ range, radial };
}
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();
}
/**
* Convert a #LeastSquares to a #WaveInfo. Returns
* WaveInfo::Undefined() if there is no valid result in the
* #LeastSquares instance.
*/
gcc_pure
static WaveInfo
GetWaveInfo(const LeastSquares &ls, const FlatProjection &projection,
double time)
{
if (!ls.HasResult())
return WaveInfo::Undefined();
const FlatPoint flat_location(ls.GetMiddleX(), ls.GetAverageY());
const GeoPoint location(projection.Unproject(flat_location));
const GeoPoint a(projection.Unproject(FlatPoint(ls.GetMinX(),
ls.GetYAtMinX())));
const GeoPoint b(projection.Unproject(FlatPoint(ls.GetMaxX(),
ls.GetYAtMaxX())));
Angle bearing = a.Bearing(b);
Angle normal = (bearing + Angle::QuarterCircle()).AsBearing();
return {location, a, b, normal, time};
}
void
AppendArc(const GeoPoint start, const GeoPoint end)
{
// Determine start bearing and radius
const GeoVector v = center.DistanceBearing(start);
Angle start_bearing = v.bearing;
const fixed radius = v.distance;
// 5 or -5, depending on direction
const auto _step = ArcStepWidth(radius);
const Angle step = Angle::Degrees(rotation * _step);
const fixed threshold = _step * fixed(1.5);
// Determine end bearing
Angle end_bearing = center.Bearing(end);
if (rotation > 0) {
while (end_bearing < start_bearing)
end_bearing += Angle::FullCircle();
} else if (rotation < 0) {
while (end_bearing > start_bearing)
end_bearing -= Angle::FullCircle();
}
// Add first polygon point
points.push_back(start);
// Add intermediate polygon points
while ((end_bearing - start_bearing).AbsoluteDegrees() > threshold) {
start_bearing += step;
points.push_back(FindLatitudeLongitude(center, start_bearing, radius));
}
// Add last polygon point
points.push_back(end);
}
GeoVector
TaskLeg::GetTravelledVector(const GeoPoint &ref) const
{
switch (destination.GetActiveState()) {
case OrderedTaskPoint::BEFORE_ACTIVE:
if (!GetOrigin())
return GeoVector(fixed(0));
// this leg totally included
return memo_travelled.calc(GetOrigin()->GetLocationTravelled(),
destination.GetLocationTravelled());
case OrderedTaskPoint::CURRENT_ACTIVE:
// this leg partially included
if (!GetOrigin())
return GeoVector(fixed(0),
ref.Bearing(destination.GetLocationRemaining()));
if (destination.HasEntered())
return memo_travelled.calc(GetOrigin()->GetLocationTravelled(),
destination.GetLocationTravelled());
else
return memo_travelled.calc(GetOrigin()->GetLocationTravelled(), ref);
case OrderedTaskPoint::AFTER_ACTIVE:
if (!GetOrigin())
return GeoVector(fixed(0));
// this leg may be partially included
if (GetOrigin()->HasEntered())
return memo_travelled.calc(GetOrigin()->GetLocationTravelled(), ref);
default:
return GeoVector(fixed(0));
};
}
