bool
AATPoint::SetRange(const fixed p, const bool force_if_current)
{
if (target_locked)
return false;
switch (GetActiveState()) {
case CURRENT_ACTIVE:
if (!HasEntered() || force_if_current) {
target_location = GetLocationMin().Interpolate(GetLocationMax(),p);
return true;
}
return false;
case AFTER_ACTIVE:
if (GetActiveState() == AFTER_ACTIVE) {
target_location = GetLocationMin().Interpolate(GetLocationMax(),p);
return true;
}
return false;
default:
return false;
}
}
const GeoPoint&
AATPoint::GetLocationRemaining() const
{
if (!IsPast())
return target_location;
if (HasSampled())
return GetLocationMax();
return GetLocationMin();
}
const GeoPoint&
AATPoint::GetLocationRemaining() const
{
if (GetActiveState() != BEFORE_ACTIVE)
return target_location;
if (HasSampled())
return GetLocationMax();
return GetLocationMin();
}
bool
AATPoint::CheckTargetInside(const AircraftState& state)
{
/* target must be moved if d(p_last,t)+d(t,p_next) <
d(p_last,state)+d(state,p_next) */
if (!IsCloseToTarget(state))
return false;
if (positive(DoubleLegDistance(state.location)
- DoubleLegDistance(GetLocationMax())))
// no improvement available
return false;
target_location = state.location;
return true;
}
RangeAndRadial
AATPoint::GetTargetRangeRadial(fixed oldrange) const
{
const auto fprev = GetPrevious()->GetLocationRemaining();
const auto floc = GetLocation();
const auto radialraw = (floc.Bearing(GetTargetLocation()) -
fprev.Bearing(floc)).AsBearing();
auto radial = radialraw.AsDelta();
auto d = floc.Distance(GetTargetLocation());
if (radial < -Angle::QuarterCircle() || radial > Angle::QuarterCircle())
d = -d;
const auto radius = negative(d)
? floc.Distance(GetLocationMin())
: floc.Distance(GetLocationMax());
const auto range = Clamp(d / radius, fixed(-1), fixed(1));
if (oldrange == fixed(0) && range == fixed(0))
radial = Angle::Zero();
return RangeAndRadial{ range, radial };
}
void
AATPoint::SetTarget(RangeAndRadial rar, const FlatProjection &proj)
{
const auto fprev =
proj.ProjectFloat(GetPrevious()->GetLocationRemaining());
const auto floc = proj.ProjectFloat(GetLocation());
const FlatLine flb (fprev,floc);
const FlatLine fradius(floc,
proj.ProjectFloat(negative(rar.range)
? GetLocationMin()
: GetLocationMax()));
const auto radius = fradius.d() * fabs(rar.range);
const auto angle = rar.radial - flb.angle();
const FlatPoint ftarget1(radius * angle.cos(),
radius * -(angle).sin());
const auto ftarget2 = floc + ftarget1;
const auto targetG = proj.Unproject(ftarget2);
SetTarget(targetG, true);
}
gcc_pure
GeoPoint InterpolateLocationMinMax(fixed p) const {
return GetLocationMin().Interpolate(GetLocationMax(), p);
}