IntersectingAirspaceVisitorAdapter(const GeoPoint &_loc,
const GeoPoint &_end,
const FlatProjection &_projection,
AirspaceIntersectionVisitor &_visitor)
:start(_loc), end(_end), projection(&_projection),
ray(projection->ProjectInteger(start), projection->ProjectInteger(end)),
visitor(&_visitor) {}
GeoPoint
RoutePolars::MSLIntercept(const int index, const AGeoPoint& p,
const FlatProjection &proj) const
{
const unsigned safe_index = ((unsigned)index) % ROUTEPOLAR_POINTS;
const FlatGeoPoint fp = proj.ProjectInteger(p);
const fixed d = p.altitude * polar_glide.GetPoint(safe_index).inv_gradient;
const fixed scale = proj.GetApproximateScale();
const int steps = int(d / scale) + 1;
int dx, dy;
RoutePolar::IndexToDXDY(safe_index, dx, dy);
dx = (dx * steps) >> 7;
dy = (dy * steps) >> 7;
const FlatGeoPoint dp(fp.longitude + dx, fp.latitude + dy);
return proj.Unproject(dp);
}
SearchPoint::SearchPoint(const FlatGeoPoint &floc, const FlatProjection &tp)
:location(tp.Unproject(floc)), flat_location(floc)
#ifndef NDEBUG
, projected(true)
#endif
{
}
SearchPoint::SearchPoint(const GeoPoint &loc, const FlatProjection &tp)
:location(loc), flat_location(tp.ProjectInteger(loc))
#ifndef NDEBUG
, projected(true)
#endif
{
}
void
Waypoint::Project(const FlatProjection &projection)
{
flat_location = projection.ProjectInteger(location);
#ifndef NDEBUG
flat_location_initialised = true;
#endif
}
void
SearchPoint::Project(const FlatProjection &tp)
{
flat_location = tp.ProjectInteger(location);
#ifndef NDEBUG
projected = true;
#endif
}
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 FlatProjection &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);
}
/**
* 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};
}
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;
}
/**
* Check if the two wave lines match, and attempt to merge them.
*
* TODO: this is clumsy and unoptimised code; please improve!
*
* @return true if the lines have been merged into #i
*/
static bool
MergeLines(WaveInfo &i, const WaveInfo &new_wave, double new_length,
FlatLine new_line, const FlatProjection &projection)
{
Angle delta_angle = (new_wave.normal - i.normal).AsDelta();
if (delta_angle > Angle::QuarterCircle())
delta_angle -= Angle::HalfCircle();
else if (delta_angle < -Angle::QuarterCircle())
delta_angle += Angle::HalfCircle();
/* basic direction check */
constexpr Angle max_delta_angle = Angle::Degrees(20);
if (delta_angle > max_delta_angle || delta_angle < -max_delta_angle)
return false;
/* basic range check */
const double max_distance = std::max(new_length, i.GetLength());
if (i.a.DistanceS(new_wave.a) > max_distance &&
i.b.DistanceS(new_wave.a) > max_distance &&
i.a.DistanceS(new_wave.b) > max_distance &&
i.a.DistanceS(new_wave.b) > max_distance)
return false;
FlatLine other_line(projection.ProjectFloat(i.a),
projection.ProjectFloat(i.b));
if (other_line.a.x > other_line.b.x)
/* rearrange a and b to get well-defined order */
std::swap(other_line.a, other_line.b);
if (!MergeLines(other_line, new_line))
return false;
i.a = projection.Unproject(other_line.a);
i.b = projection.Unproject(other_line.b);
Angle bearing = i.a.Bearing(i.b);
i.normal = (bearing + Angle::QuarterCircle()).AsBearing();
i.time = new_wave.time;
return true;
}
FlatGeoPoint
RoutePolars::ReachIntercept(const int index, const AGeoPoint& origin,
const RasterMap* map,
const FlatProjection &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);
}
inline FlatGeoPoint
RoutePolars::MSLIntercept(const int index, const FlatGeoPoint &fp,
double altitude,
const FlatProjection &proj) const
{
const unsigned safe_index = ((unsigned)index) % ROUTEPOLAR_POINTS;
const auto d = altitude * polar_glide.GetPoint(safe_index).inv_gradient;
const auto scale = proj.GetApproximateScale();
const int steps = int(d / scale) + 1;
FlatGeoPoint dp = RoutePolar::IndexToDXDY(safe_index);
dp.x = (dp.x * steps) >> 7;
dp.y = (dp.y * steps) >> 7;
return fp + dp;
}
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;
}
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);
}
void
FlatTriangleFanTree::AcceptInRange(const FlatBoundingBox &bb,
const FlatProjection &projection,
TriangleFanVisitor &visitor) const
{
if (!bb.Overlaps(bb_children))
return;
if (bb.Overlaps(bounding_box)) {
visitor.StartFan();
for (const auto &v : vs)
visitor.AddPoint(projection.Unproject(v));
visitor.EndFan();
}
for (const auto &child : children)
child.AcceptInRange(bb, projection, visitor);
}
