void
RoutePolars::Initialise(const GlideSettings &settings, const GlidePolar &polar,
const SpeedVector &wind,
const int _height_min_working)
{
polar_glide.Initialise(settings, polar, wind, true);
polar_cruise.Initialise(settings, polar, wind, false);
inv_mc = MC_CEILING_PENALTY_FACTOR * polar.GetInvMC();
height_min_working = std::max(0, _height_min_working - GetSafetyHeight());
}
FlatGeoPoint
RoutePolars::ReachIntercept(const int index, const AGeoPoint& origin,
const RasterMap* map,
const TaskProjection& proj) const
{
const bool valid = map && map->IsDefined();
const RoughAltitude altitude = origin.altitude - GetSafetyHeight();
const AGeoPoint m_origin((GeoPoint)origin, altitude);
const GeoPoint dest = MSLIntercept(index, m_origin, proj);
const GeoPoint p = valid ?
map->Intersection(m_origin, (short)altitude, (short)altitude, dest) : dest;
return proj.ProjectInteger(p);
}
GeoPoint
RoutePolars::Intersection(const AGeoPoint &origin,
const AGeoPoint &destination,
const RasterMap *map, const FlatProjection &proj) const
{
if (map == nullptr || !map->IsDefined())
return GeoPoint::Invalid();
RouteLink e(RoutePoint(proj.ProjectInteger(destination),
destination.altitude),
RoutePoint(proj.ProjectInteger(origin), origin.altitude), proj);
if (e.d <= 0)
return GeoPoint::Invalid();
return map->Intersection(origin,
origin.altitude - GetSafetyHeight(),
CalcVHeight(e), destination,
height_min_working);
}
bool
RoutePolars::Intersection(const AGeoPoint& origin, const AGeoPoint& destination,
const RasterMap* map, const TaskProjection& proj,
GeoPoint& intx) const
{
if (map == nullptr || !map->IsDefined())
return false;
RouteLink e(RoutePoint(proj.ProjectInteger(destination),
destination.altitude),
RoutePoint(proj.ProjectInteger(origin), origin.altitude), proj);
if (!positive(e.d))
return false;
const RoughAltitude vh = CalcVHeight(e);
intx = map->Intersection(origin, (short)(origin.altitude - GetSafetyHeight()),
(short)vh, destination);
return !(intx == destination);
}
FlatGeoPoint
RoutePolars::ReachIntercept(const int index, const AFlatGeoPoint &flat_origin,
const GeoPoint &origin,
const RasterMap* map,
const FlatProjection &proj) const
{
const bool valid = map && map->IsDefined();
const int altitude = flat_origin.altitude - GetSafetyHeight();
const FlatGeoPoint flat_dest = MSLIntercept(index, flat_origin,
altitude, proj);
if (!valid)
return flat_dest;
const GeoPoint dest = proj.Unproject(flat_dest);
const GeoPoint p = map->Intersection(origin, altitude,
altitude, dest, height_min_working);
if (!p.IsValid())
return flat_dest;
FlatGeoPoint fp = proj.ProjectInteger(p);
/* when there's an obstacle very nearby and our intersection is
right next to our origin, the intersection may be deformed due to
terrain raster rounding errors; the following code applies
clipping to avoid degenerate polygons */
FlatGeoPoint delta1 = flat_dest - (FlatGeoPoint)flat_origin;
FlatGeoPoint delta2 = fp - (FlatGeoPoint)flat_origin;
if (delta1.x * delta2.x < 0)
/* intersection is on the wrong horizontal side */
fp.x = flat_origin.x;
if (delta1.y * delta2.y < 0)
/* intersection is on the wrong vertical side */
fp.x = flat_origin.y;
return fp;
}
static void
CheckLeg(const TaskWaypoint &tp, const AircraftState &aircraft,
const TaskStats &stats)
{
const GeoPoint destination = tp.GetWaypoint().location;
const fixed safety_height = GetSafetyHeight(tp);
const fixed min_arrival_alt = tp.GetWaypoint().elevation + safety_height;
const GeoVector vector = aircraft.location.DistanceBearing(destination);
const fixed ld = glide_polar.GetBestLD();
const fixed height_above_min = aircraft.altitude - min_arrival_alt;
const fixed height_consumption = vector.distance / ld;
const ElementStat &leg = stats.current_leg;
const GlideResult &solution_remaining = leg.solution_remaining;
ok1(leg.vector_remaining.IsValid());
ok1(equals(leg.vector_remaining.distance, vector.distance));
ok1(equals(leg.vector_remaining.bearing, vector.bearing));
ok1(solution_remaining.IsOk());
ok1(solution_remaining.vector.IsValid());
ok1(equals(solution_remaining.vector.distance, vector.distance));
ok1(equals(solution_remaining.vector.bearing, vector.bearing));
ok1(equals(solution_remaining.height_glide, height_consumption));
ok1(equals(solution_remaining.altitude_difference,
height_above_min - height_consumption));
ok1(equals(solution_remaining.GetRequiredAltitudeWithDrift(),
min_arrival_alt + height_consumption));
if (height_above_min >= height_consumption) {
/* straight glide */
ok1(equals(solution_remaining.height_climb, 0));
} else if (positive(glide_polar.GetMC())) {
/* climb required */
ok1(equals(solution_remaining.height_climb,
height_consumption - height_above_min));
} else {
/* climb required, but not possible (MC=0) */
ok1(equals(solution_remaining.height_climb, 0));
}
}
bool
RoutePolars::CheckClearance(const RouteLink &e, const RasterMap* map,
const FlatProjection &proj, RoutePoint& inp) const
{
if (!config.IsTerrainEnabled())
return true;
GeoPoint int_x;
int int_h;
GeoPoint start = proj.Unproject(e.first);
GeoPoint dest = proj.Unproject(e.second);
assert(map);
if (!map->FirstIntersection(start, e.first.altitude, dest,
e.second.altitude, CalcVHeight(e),
climb_ceiling, GetSafetyHeight(),
int_x, int_h))
return true;
inp = RoutePoint(proj.ProjectInteger(int_x), int_h);
return false;
}
