const Airspaces::AirspaceVector
Airspaces::find_inside(const AIRCRAFT_STATE &state,
const AirspacePredicate &condition) const
{
Airspace bb_target(state.Location, task_projection);
AirspaceVector vectors;
airspace_tree.find_within_range(bb_target, 0, std::back_inserter(vectors));
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
for (AirspaceVector::iterator v = vectors.begin(); v != vectors.end();) {
#ifdef INSTRUMENT_TASK
count_intersections++;
#endif
if (!condition(*v->get_airspace()) || !(*v).inside(state))
vectors.erase(v);
else
++v;
}
return vectors;
}
const Airspaces::AirspaceVector
Airspaces::scan_nearest(const GeoPoint &location,
const AirspacePredicate &condition) const
{
Airspace bb_target(location, task_projection);
std::pair<AirspaceTree::const_iterator, double>
found = airspace_tree.find_nearest(bb_target);
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
AirspaceVector res;
if (found.first != airspace_tree.end()) {
// also should do scan_range with range = 0 since there
// could be more than one with zero dist
if (found.second == 0) {
return scan_range(location, fixed_zero, condition);
} else {
if (condition(*found.first->get_airspace()))
res.push_back(*found.first);
}
}
return res;
}
const Airspaces::AirspaceVector
Airspaces::scan_range(const GeoPoint &location,
const fixed range,
const AirspacePredicate &condition) const
{
Airspace bb_target(location, task_projection);
int mrange = task_projection.project_range(location, range);
std::deque< Airspace > vectors;
airspace_tree.find_within_range(bb_target, -mrange, std::back_inserter(vectors));
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
AirspaceVector res;
for (std::deque<Airspace>::iterator v = vectors.begin(); v != vectors.end(); ++v) {
if (!condition(*v->get_airspace()))
continue;
if (fixed((*v).distance(bb_target)) > range)
continue;
if ((*v).inside(location) || positive(range))
res.push_back(*v);
}
return res;
}
const Airspaces::AirspaceVector
Airspaces::ScanNearest(const GeoPoint &location,
const AirspacePredicate &condition) const
{
if (empty())
// nothing to do
return AirspaceVector();
Airspace bb_target(location, task_projection);
std::pair<AirspaceTree::const_iterator, AirspaceTree::distance_type>
found = airspace_tree.find_nearest(bb_target);
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
AirspaceVector res;
if (found.first != airspace_tree.end()) {
// also should do scan_range with range = 0 since there
// could be more than one with zero dist
if (found.second.is_zero())
return ScanRange(location, fixed_zero, condition);
if (condition(*found.first->GetAirspace()))
res.push_back(*found.first);
}
return res;
}
const Airspaces::AirspaceVector
Airspaces::ScanRange(const GeoPoint &location, fixed range,
const AirspacePredicate &condition) const
{
if (empty())
// nothing to do
return AirspaceVector();
Airspace bb_target(location, task_projection);
int projected_range = task_projection.ProjectRangeInteger(location, range);
std::deque< Airspace > vectors;
airspace_tree.find_within_range(bb_target, -projected_range,
std::back_inserter(vectors));
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
AirspaceVector res;
for (const auto &v : vectors) {
if (!condition(*v.GetAirspace()))
continue;
if (fixed(v.Distance(bb_target)) > range)
continue;
if (v.IsInside(location) || positive(range))
res.push_back(v);
}
return res;
}
const Airspaces::AirspaceVector
Airspaces::FindInside(const AircraftState &state,
const AirspacePredicate &condition) const
{
Airspace bb_target(state.location, task_projection);
AirspaceVector vectors;
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
std::function<void(const Airspace &)> visitor =
[&state, &condition, &vectors](const Airspace &v){
#ifdef INSTRUMENT_TASK
count_intersections++;
#endif
if (condition(v.GetAirspace()) &&
v.IsInside(state))
vectors.push_back(v);
};
airspace_tree.visit_within_range(bb_target, 0, visitor);
return vectors;
}
const Airspaces::AirspaceVector
Airspaces::ScanRange(const GeoPoint &location, fixed range,
const AirspacePredicate &condition) const
{
if (IsEmpty())
// nothing to do
return AirspaceVector();
Airspace bb_target(location, task_projection);
int projected_range = task_projection.ProjectRangeInteger(location, range);
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
AirspaceVector res;
std::function<void(const Airspace &)> visitor =
[&location, range, &condition, &bb_target, &res](const Airspace &v){
if (condition(v.GetAirspace()) &&
fixed(v.Distance(bb_target)) <= range &&
(v.IsInside(location) || positive(range)))
res.push_back(v);
};
airspace_tree.visit_within_range(bb_target, -projected_range, visitor);
return res;
}
TracePointVector
Trace::find_within_range(const GeoPoint &loc, const fixed range,
const unsigned mintime, const fixed resolution) const
{
if (empty())
return TracePointVector();
TracePoint bb_target(loc);
bb_target.project(task_projection);
const unsigned mrange = task_projection.project_range(loc, range);
// TracePointVector vectors;
TracePointSet tset;
TracePointSetFilterInserter filter(tset, mintime);
trace_tree.find_within_range(bb_target, mrange, filter);
/*
// std::inserter(tset, tset.begin()));
if (mintime>0) {
TracePointSet::iterator tit = tset.lower_bound(bb_target);
tset.erase(tset.begin(), tit);
}
*/
if (positive(resolution)) {
const unsigned rrange = task_projection.project_range(loc, resolution);
TracePointList tlist(tset.begin(), tset.end());
thin_trace(tlist, rrange * rrange);
return TracePointVector(tlist.begin(), tlist.end());
} else {
return TracePointVector(tset.begin(), tset.end());
}
}
void
Airspaces::VisitInside(const GeoPoint &loc,
AirspaceVisitor& visitor) const
{
if (empty()) return; // nothing to do
Airspace bb_target(loc, task_projection);
AirspaceVector vectors;
airspace_tree.find_within_range(bb_target, 0, std::back_inserter(vectors));
for (auto &v : vectors)
if (v.IsInside(loc))
visitor.Visit(v);
}
void
Airspaces::visit_within_range(const GeoPoint &loc,
const fixed range,
AirspaceVisitor& visitor,
const AirspacePredicate &predicate) const
{
Airspace bb_target(loc, task_projection);
int mrange = task_projection.project_range(loc, range);
AirspacePredicateVisitorAdapter adapter(predicate, visitor);
airspace_tree.visit_within_range(bb_target, -mrange, adapter);
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
}
void
Airspaces::visit_inside(const GeoPoint &loc,
AirspaceVisitor& visitor) const
{
if (empty()) return; // nothing to do
Airspace bb_target(loc, task_projection);
AirspaceVector vectors;
airspace_tree.find_within_range(bb_target, 0, std::back_inserter(vectors));
for (auto v = vectors.begin(); v != vectors.end(); ++v) {
if ((*v).inside(loc))
visitor.Visit(*v);
}
}
void
Airspaces::VisitInside(const GeoPoint &loc, AirspaceVisitor &visitor) const
{
if (IsEmpty())
// nothing to do
return;
Airspace bb_target(loc, task_projection);
std::function<void(const Airspace &)> visitor2 =
[&loc, &visitor](const Airspace &v){
if (v.IsInside(loc))
visitor.Visit(v);
};
airspace_tree.visit_within_range(bb_target, 0, visitor2);
}
void
Airspaces::visit_intersecting(const GeoPoint &loc,
const GeoVector &vec,
AirspaceIntersectionVisitor& visitor) const
{
FlatRay ray(task_projection.project(loc),
task_projection.project(vec.end_point(loc)));
GeoPoint c = vec.mid_point(loc);
Airspace bb_target(c, task_projection);
int mrange = task_projection.project_range(c, vec.Distance / 2);
IntersectingAirspaceVisitorAdapter adapter(loc, vec, ray, visitor);
airspace_tree.visit_within_range(bb_target, -mrange, adapter);
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
}
void
Airspaces::VisitIntersecting(const GeoPoint &loc, const GeoPoint &end,
AirspaceIntersectionVisitor& visitor) const
{
if (empty()) return; // nothing to do
FlatRay ray(task_projection.project(loc), task_projection.project(end));
const GeoPoint c = loc.Middle(end);
Airspace bb_target(c, task_projection);
int mrange = task_projection.project_range(c, loc.Distance(end) / 2);
IntersectingAirspaceVisitorAdapter adapter(loc, end, ray, visitor);
airspace_tree.visit_within_range(bb_target, -mrange, adapter);
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
}
void
Airspaces::VisitIntersecting(const GeoPoint &loc, const GeoPoint &end,
AirspaceIntersectionVisitor& visitor) const
{
if (empty())
// nothing to do
return;
const GeoPoint c = loc.Middle(end);
Airspace bb_target(c, task_projection);
int projected_range = task_projection.ProjectRangeInteger(c, loc.Distance(end) / 2);
IntersectingAirspaceVisitorAdapter adapter(loc, end, task_projection, visitor);
airspace_tree.visit_within_range(bb_target, -projected_range, adapter);
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
}
void
Airspaces::VisitWithinRange(const GeoPoint &location, fixed range,
AirspaceVisitor& visitor,
const AirspacePredicate &predicate) const
{
if (empty())
// nothing to do
return;
Airspace bb_target(location, task_projection);
int projected_range = task_projection.ProjectRangeInteger(location, range);
AirspacePredicateVisitorAdapter adapter(predicate, visitor);
airspace_tree.visit_within_range(bb_target, -projected_range, adapter);
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
}
void
Waypoints::VisitWithinRange(const GeoPoint &loc, const fixed range,
WaypointVisitor& visitor) const
{
if (IsEmpty())
return; // nothing to do
Waypoint bb_target(loc);
bb_target.Project(task_projection);
const unsigned mrange = task_projection.ProjectRangeInteger(loc, range);
WaypointEnvelopeVisitor wve(&visitor);
waypoint_tree.VisitWithinRange(bb_target, mrange, wve);
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
}
const Waypoint*
Waypoints::GetNearest(const GeoPoint &loc, fixed range) const
{
if (IsEmpty())
return NULL;
Waypoint bb_target(loc);
bb_target.Project(task_projection);
const unsigned mrange = task_projection.ProjectRangeInteger(loc, range);
const auto found = waypoint_tree.FindNearest(bb_target, mrange);
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
if (found.first == waypoint_tree.end())
return NULL;
return &*found.first;
}
const Waypoint*
Waypoints::get_nearest_landable(const GeoPoint &loc, fixed range) const
{
if (empty())
return NULL;
Waypoint bb_target(loc);
bb_target.Project(task_projection);
const unsigned mrange = task_projection.project_range(loc, range);
std::pair<WaypointTree::const_iterator, WaypointTree::distance_type> found =
waypoint_tree.FindNearestIf(bb_target, mrange, LandablePredicate());
#ifdef INSTRUMENT_TASK
n_queries++;
#endif
if (found.first == waypoint_tree.end())
return NULL;
return &*found.first;
}
