AirspaceIntersectionVector
AirspaceCircle::Intersects(const GeoPoint &start, const GeoPoint &end,
const TaskProjection &projection) const
{
const fixed f_radius = projection.ProjectRangeFloat(m_center, m_radius);
const FlatPoint f_center = projection.ProjectFloat(m_center);
const FlatPoint f_start = projection.ProjectFloat(start);
const FlatPoint f_end = projection.ProjectFloat(end);
const FlatLine line(f_start, f_end);
FlatPoint f_p1, f_p2;
if (!line.intersect_circle(f_radius, f_center, f_p1, f_p2))
return AirspaceIntersectionVector();
const fixed mag = line.dsq();
if (!positive(mag))
return AirspaceIntersectionVector();
const fixed inv_mag = fixed(1) / mag;
const fixed t1 = FlatLine(f_start, f_p1).dot(line);
const fixed t2 = (f_p1 == f_p2) ?
fixed(-1) : FlatLine(f_start, f_p2).dot(line);
const bool in_range = (t1 < mag) || (t2 < mag);
// if at least one point is within range, capture both points
AirspaceIntersectSort sorter(start, *this);
if ((t1 >= fixed(0)) && in_range)
sorter.add(t1 * inv_mag, projection.Unproject(f_p1));
if ((t2 >= fixed(0)) && in_range)
sorter.add(t2 * inv_mag, projection.Unproject(f_p2));
return sorter.all();
}
SearchPoint::SearchPoint(const FlatGeoPoint &floc, const TaskProjection &tp)
:location(tp.Unproject(floc)), flat_location(floc)
#ifndef NDEBUG
, projected(true)
#endif
{
}
void
AATPoint::SetTarget(const fixed range, const fixed radial,
const TaskProjection &proj)
{
fixed oldrange = fixed_zero;
fixed oldradial = fixed_zero;
GetTargetRangeRadial(oldrange, oldradial);
const FlatPoint fprev =
proj.ProjectFloat(GetPrevious()->GetLocationRemaining());
const FlatPoint floc = proj.ProjectFloat(GetLocation());
const FlatLine flb (fprev,floc);
const FlatLine fradius (floc,proj.ProjectFloat(GetLocationMin()));
const fixed bearing = fixed_minus_one * flb.angle().Degrees();
const fixed radius = fradius.d();
fixed swapquadrants = fixed_zero;
if (positive(range) != positive(oldrange))
swapquadrants = fixed(180);
const FlatPoint ftarget1 (fabs(range) * radius *
cos((bearing + radial + swapquadrants)
/ fixed(360) * fixed_two_pi),
fabs(range) * radius *
sin( fixed_minus_one * (bearing + radial + swapquadrants)
/ fixed(360) * fixed_two_pi));
const FlatPoint ftarget2 = floc + ftarget1;
const GeoPoint targetG = proj.Unproject(ftarget2);
SetTarget(targetG, true);
}
GeoPoint
AirspacePolygon::ClosestPoint(const GeoPoint &loc,
const TaskProjection &projection) const
{
const FlatGeoPoint p = projection.ProjectInteger(loc);
const FlatGeoPoint pb = m_border.NearestPoint(p);
return projection.Unproject(pb);
}
bool
RoutePolars::CheckClearance(const RouteLink &e, const RasterMap* map,
const TaskProjection &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, (int)e.first.altitude, dest,
(int)e.second.altitude, (int)CalcVHeight(e),
(int)climb_ceiling, (int)GetSafetyHeight(),
int_x, int_h))
return true;
inp = RoutePoint(proj.ProjectInteger(int_x), RoughAltitude(int_h));
return false;
}
GeoPoint
RoutePolars::MSLIntercept(const int index, const AGeoPoint& p,
const TaskProjection& 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);
}
void
FlatTriangleFanTree::AcceptInRange(const FlatBoundingBox &bb,
const TaskProjection &task_proj,
TriangleFanVisitor &visitor) const
{
if (!bb.Overlaps(bb_children))
return;
if (bb.Overlaps(bounding_box)) {
visitor.StartFan();
for (auto it = vs.cbegin(), end = vs.cend(); it != end; ++it)
visitor.AddPoint(task_proj.Unproject(*it));
visitor.EndFan();
}
for (auto it = children.cbegin(), end = children.cend(); it != end; ++it)
it->AcceptInRange(bb, task_proj, visitor);
}
void
FlatTriangleFanTree::AcceptInRange(const FlatBoundingBox &bb,
const TaskProjection &task_proj,
TriangleFanVisitor &visitor) const
{
if (!bb.Overlaps(bb_children))
return;
if (bb.Overlaps(bounding_box)) {
visitor.StartFan();
for (const auto &v : vs)
visitor.AddPoint(task_proj.Unproject(v));
visitor.EndFan();
}
for (const auto &child : children)
child.AcceptInRange(bb, task_proj, visitor);
}
AirspaceIntersectionVector
AirspacePolygon::Intersects(const GeoPoint &start, const GeoPoint &end,
const TaskProjection &projection) const
{
const FlatRay ray(projection.ProjectInteger(start),
projection.ProjectInteger(end));
AirspaceIntersectSort sorter(start, *this);
for (auto it = m_border.begin(); it + 1 != m_border.end(); ++it) {
const FlatRay r_seg(it->GetFlatLocation(), (it + 1)->GetFlatLocation());
fixed t = ray.DistinctIntersection(r_seg);
if (!negative(t))
sorter.add(t, projection.Unproject(ray.Parametric(t)));
}
return sorter.all();
}
void
ThermalLocator::Update(const fixed t_0,
const GeoPoint &location_0,
const SpeedVector wind,
ThermalLocatorInfo &therm)
{
if (n_points < TLOCATOR_NMIN) {
therm.estimate_valid = false;
return; // nothing to do.
}
GeoPoint dloc = FindLatitudeLongitude(location_0, wind.bearing, wind.norm);
TaskProjection projection;
projection.Reset(location_0);
projection.Update();
// drift points
Drift(t_0, projection, location_0 - dloc);
FlatPoint av = glider_average();
// find thermal center relative to glider's average position
FlatPoint f0(fixed(0), fixed(0));
fixed acc = fixed(0);
for (unsigned i = 0; i < n_points; ++i) {
f0 += (points[i].loc_drift-av)*points[i].lift_weight;
acc += points[i].lift_weight;
}
// if sufficient data, estimate location
if (!positive(acc)) {
therm.estimate_valid = false;
return;
}
f0 = f0 * (fixed(1)/acc) + av;
therm.estimate_location = projection.Unproject(f0);
therm.estimate_valid = true;
}
void
AATPoint::SetTarget(RangeAndRadial rar, const TaskProjection &proj)
{
const FlatPoint fprev =
proj.ProjectFloat(GetPrevious()->GetLocationRemaining());
const FlatPoint floc = proj.ProjectFloat(GetLocation());
const FlatLine flb (fprev,floc);
const FlatLine fradius(floc,
proj.ProjectFloat(negative(rar.range)
? GetLocationMin()
: GetLocationMax()));
const fixed radius = fradius.d() * fabs(rar.range);
const Angle angle = rar.radial - flb.angle();
const FlatPoint ftarget1(radius * angle.cos(),
radius * -(angle).sin());
const FlatPoint ftarget2 = floc + ftarget1;
const GeoPoint targetG = proj.Unproject(ftarget2);
SetTarget(targetG, true);
}
