void MapItemListBuilder::AddArrivalAltitudes( const ProtectedRoutePlanner &route_planner, const RasterTerrain *terrain, fixed safety_height) { if (list.full()) return; // Calculate terrain elevation if possible short elevation; if (terrain != NULL) elevation = terrain->GetTerrainHeight(location); else elevation = RasterBuffer::TERRAIN_INVALID; // Calculate target altitude RoughAltitude safety_elevation(safety_height); if (!RasterBuffer::IsInvalid(elevation)) safety_elevation += RoughAltitude(elevation); // Save destination point incl. elevation and safety height const AGeoPoint destination(location, safety_elevation); // Calculate arrival altitudes ReachResult reach; ProtectedRoutePlanner::Lease leased_route_planner(route_planner); if (!leased_route_planner->FindPositiveArrival(destination, reach)) return; reach.Subtract(RoughAltitude(safety_height)); list.append(new ArrivalAltitudeMapItem(RoughAltitude(elevation), reach)); }
int main(int argc, char** argv) { const char hc_path[] = "tmp/terrain"; const char *map_path; if ((argc<2) || !strlen(argv[0])) { map_path = hc_path; } else { map_path = argv[0]; } TCHAR jp2_path[4096]; _tcscpy(jp2_path, PathName(map_path)); _tcscat(jp2_path, _T(DIR_SEPARATOR_S) _T("terrain.jp2")); TCHAR j2w_path[4096]; _tcscpy(j2w_path, PathName(map_path)); _tcscat(j2w_path, _T(DIR_SEPARATOR_S) _T("terrain.j2w")); NullOperationEnvironment operation; RasterMap map(jp2_path, j2w_path, NULL, operation); do { map.SetViewCenter(map.GetMapCenter(), fixed(100000)); } while (map.IsDirty()); plan_tests(16*3); test_troute(map, fixed(0), fixed(0.1), RoughAltitude(10000)); test_troute(map, fixed(0), fixed(0), RoughAltitude(10000)); test_troute(map, fixed(5.0), fixed(1), RoughAltitude(10000)); return exit_status(); }
static void test_reach(const RasterMap& map, fixed mwind, fixed mc) { GlideSettings settings; settings.SetDefaults(); GlidePolar polar(mc); SpeedVector wind(Angle::Degrees(0), mwind); TerrainRoute route; route.UpdatePolar(settings, polar, polar, wind); route.SetTerrain(&map); GeoPoint origin(map.GetMapCenter()); fixed pd = map.PixelDistance(origin, 1); printf("# pixel size %g\n", (double)pd); bool retval= true; short horigin = map.GetHeight(origin)+1000; AGeoPoint aorigin(origin, RoughAltitude(horigin)); RoutePlannerConfig config; config.SetDefaults(); retval = route.SolveReach(aorigin, config, RoughAltitude::Max()); ok(retval, "reach solve", 0); PrintHelper::print_reach_tree(route); GeoPoint dest(origin.longitude-Angle::Degrees(0.02), origin.latitude-Angle::Degrees(0.02)); { Directory::Create(_T("output/results")); std::ofstream fout("output/results/terrain.txt"); unsigned nx = 100; unsigned ny = 100; for (unsigned i=0; i< nx; ++i) { for (unsigned j=0; j< ny; ++j) { fixed fx = (fixed)i / (nx - 1) * 2 - fixed(1); fixed fy = (fixed)j / (ny - 1) * 2 - fixed(1); GeoPoint x(origin.longitude + Angle::Degrees(fixed(0.6) * fx), origin.latitude + Angle::Degrees(fixed(0.6) * fy)); short h = map.GetInterpolatedHeight(x); AGeoPoint adest(x, RoughAltitude(h)); ReachResult reach; route.FindPositiveArrival(adest, reach); if ((i % 5 == 0) && (j % 5 == 0)) { AGeoPoint ao2(x, RoughAltitude(h + 1000)); route.SolveReach(ao2, config, RoughAltitude::Max()); } fout << x.longitude.Degrees() << " " << x.latitude.Degrees() << " " << h << " " << (int)reach.terrain << "\n"; } fout << "\n"; } fout << "\n"; } }
bool ReachFan::FindPositiveArrival(const AGeoPoint dest, const RoutePolars &rpolars, ReachResult &result_r) const { if (root.IsEmpty()) return false; const FlatGeoPoint d(task_proj.ProjectInteger(dest)); const ReachFanParms parms(rpolars, task_proj, (int)terrain_base); result_r.Clear(); // first calculate direct (terrain-independent height) result_r.direct = root.DirectArrival(d, parms); if (root.IsDummy()) /* terrain reach is not available, stop here */ return true; // if can't reach even with no terrain, exit early if (std::min(root.GetHeight(), result_r.direct) < dest.altitude) { result_r.terrain = result_r.direct; result_r.terrain_valid = ReachResult::Validity::UNREACHABLE; return true; } // now calculate turning solution result_r.terrain = dest.altitude - RoughAltitude(1); result_r.terrain_valid = root.FindPositiveArrival(d, parms, result_r.terrain) ? ReachResult::Validity::VALID : ReachResult::Validity::UNREACHABLE; return true; }
bool FlatTriangleFanTree::CheckGap(const AFlatGeoPoint &n, const RouteLink &e_1, const RouteLink &e_2, ReachFanParms &parms) { const bool side = (e_1.d > e_2.d); const RouteLink &e_long = (side ? e_1 : e_2); const RouteLink &e_short = (side ? e_2 : e_1); if (e_short.d >= e_long.d) return false; const FlatGeoPoint &p_long = (side ? e_1.first : e_2.first); // return true if this gap was caught (applicable) whether or not it generated // a change const fixed f0 = e_short.d * e_long.inv_d; const RoughAltitude h_loss = parms.rpolars.CalcGlideArrival(n, p_long, parms.task_proj) - n.altitude; const FlatGeoPoint dp(p_long - n); // scan from n-p_long to perpendicular to n-p_long int index_left, index_right; if (!side) { index_left = e_long.polar_index - REACH_SWEEP; index_right = e_long.polar_index - REACH_BUFFER; } else { index_left = e_long.polar_index + REACH_BUFFER; index_right = e_long.polar_index + REACH_SWEEP; } children.emplace_back(depth + 1); FlatTriangleFanTree &child = children.back(); for (fixed f = f0; f < fixed(0.9); f += fixed(0.1)) { // find corner point const FlatGeoPoint px = (dp * f + n); // position x is length (n to p_short) along (n to p_long) const RoughAltitude h = n.altitude + RoughAltitude(f * h_loss); // altitude calculated from pure glide from n to x const AFlatGeoPoint x(px, h); child.FillReach(x, index_left, index_right, parms); // prune child if empty or single spike if (child.vs.size() > 3) { parms.vertex_counter += child.vs.size(); parms.fan_counter++; return true; } child.vs.clear(); } // don't need the child children.pop_back(); return false; }
RoughAltitude RoutePolars::CalcGlideArrival(const AFlatGeoPoint& origin, const FlatGeoPoint& dest, const TaskProjection& proj) const { const RouteLink e(RoutePoint(dest, RoughAltitude(0)), origin, proj); return origin.altitude - CalcVHeight(e); }
void visit_abstract(const AbstractAirspace &as) { assert(!m_intersections.empty()); GeoPoint point = m_intersections[0].first; RouteLink l = rpolar.GenerateIntermediate(link.first, RoutePoint(proj.project(point), link.second.altitude), proj); if (l.second.altitude < RoughAltitude(as.GetBase().altitude) || l.second.altitude > RoughAltitude(as.GetTop().altitude)) return; if (negative(min_distance) || (l.d < min_distance)) { min_distance = l.d; nearest = std::make_pair(&as, l.second); } }
int main(int argc, char** argv) { static const char hc_path[] = "tmp/map.xcm"; const char *map_path; if ((argc<2) || !strlen(argv[0])) { map_path = hc_path; } else { map_path = argv[0]; } ZZIP_DIR *dir = zzip_dir_open(map_path, nullptr); if (dir == nullptr) { fprintf(stderr, "Failed to open %s\n", map_path); return EXIT_FAILURE; } RasterMap map; NullOperationEnvironment operation; if (!LoadTerrainOverview(dir, map.GetTileCache(), operation)) { fprintf(stderr, "failed to load map\n"); zzip_dir_close(dir); return EXIT_FAILURE; } SharedMutex mutex; do { UpdateTerrainTiles(dir, map.GetTileCache(), mutex, map.GetProjection(), map.GetMapCenter(), fixed(100000)); } while (map.IsDirty()); zzip_dir_close(dir); plan_tests(16*3); test_troute(map, fixed(0), fixed(0.1), RoughAltitude(10000)); test_troute(map, fixed(0), fixed(0), RoughAltitude(10000)); test_troute(map, fixed(5.0), fixed(1), RoughAltitude(10000)); return exit_status(); }
void FlatTriangleFanTree::FillGaps(const AFlatGeoPoint &origin, ReachFanParms &parms) { // worth checking for gaps? if (vs.size() > 2 && parms.rpolars.IsTurningReachEnabled()) { // now check gaps const RoutePoint o(origin, RoughAltitude(0)); RouteLink e_last(RoutePoint(*vs.begin(), RoughAltitude(0)), o, parms.task_proj); for (auto x_last = vs.cbegin(), end = vs.cend(), x = x_last + 1; x != end; x_last = x++) { if (TooClose(*x, origin) || TooClose(*x_last, origin)) continue; const RouteLink e(RoutePoint(*x, RoughAltitude(0)), o, parms.task_proj); // check if children need to be added CheckGap(origin, e_last, e, parms); e_last = e; } } }
gcc_pure NearestAirspace NearestAirspace::FindHorizontal(const MoreData &basic, const ProtectedAirspaceWarningManager &airspace_warnings, const Airspaces &airspace_database) { if (!basic.location_available) /* can't check for airspaces without a GPS fix */ return NearestAirspace(); /* find the nearest airspace */ //consider only active airspaces const WrapAirspacePredicate<ActiveAirspacePredicate> active_predicate(&airspace_warnings); const WrapAirspacePredicate<OutsideAirspacePredicate> outside_predicate(AGeoPoint(basic.location, RoughAltitude(0))); const AndAirspacePredicate outside_and_active_predicate(active_predicate, outside_predicate); const Airspace *airspace; //if altitude is available, filter airspaces in same height as airplane if (basic.NavAltitudeAvailable()) { /* check altitude; hard-coded margin of 50m (for now) */ WrapAirspacePredicate<AirspacePredicateHeightRange> height_range_predicate(RoughAltitude(basic.nav_altitude-fixed(50)), RoughAltitude(basic.nav_altitude+fixed(50))); AndAirspacePredicate and_predicate(outside_and_active_predicate, height_range_predicate); airspace = airspace_database.FindNearest(basic.location, and_predicate); } else //only filter outside and active airspace = airspace_database.FindNearest(basic.location, outside_and_active_predicate); if (airspace == nullptr) return NearestAirspace(); const AbstractAirspace &as = airspace->GetAirspace(); /* calculate distance to the nearest point */ const GeoPoint closest = as.ClosestPoint(basic.location, airspace_database.GetProjection()); return NearestAirspace(as, basic.location.Distance(closest)); }
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; }
unsigned RoutePolars::CalcTime(const RouteLink& link) const { const RoughAltitude dh = link.second.altitude - link.first.altitude; if (dh.IsNegative() && !positive(inv_mc)) // impossible, can't climb return UINT_MAX; // dh/d = gradient const fixed rho = dh.IsPositive() ? std::min(fixed(1), (dh * link.inv_d * polar_glide.GetPoint(link.polar_index).inv_gradient)) : fixed(0); if ((rho < fixed(1)) && !polar_cruise.GetPoint(link.polar_index).valid) // impossible, can't cruise return UINT_MAX; if (positive(rho) && !polar_glide.GetPoint(link.polar_index).valid) // impossible, can't glide return UINT_MAX; const int t_cruise = (int)( link.d * (rho * polar_glide.GetPoint(link.polar_index).slowness + (fixed(1) - rho) * polar_cruise.GetPoint(link.polar_index).slowness)); if (link.second.altitude > cruise_altitude) { // penalise any climbs required above cruise altitude const RoughAltitude h_penalty = std::max( RoughAltitude(0), link.second.altitude - std::max(cruise_altitude, link.first.altitude)); return t_cruise + (int)(h_penalty * inv_mc); } return t_cruise; }
static bool test_route(const unsigned n_airspaces, const RasterMap& map) { Airspaces airspaces; setup_airspaces(airspaces, map.GetMapCenter(), n_airspaces); { std::ofstream fout("results/terrain.txt"); unsigned nx = 100; unsigned ny = 100; GeoPoint origin(map.GetMapCenter()); for (unsigned i = 0; i < nx; ++i) { for (unsigned j = 0; j < ny; ++j) { fixed fx = (fixed)i / (nx - 1) * fixed(2.0) - fixed_one; fixed fy = (fixed)j / (ny - 1) * fixed(2.0) - fixed_one; GeoPoint x(origin.longitude + Angle::Degrees(fixed(0.2) + fixed(0.7) * fx), origin.latitude + Angle::Degrees(fixed(0.9) * fy)); short h = map.GetInterpolatedHeight(x); fout << x.longitude.Degrees() << " " << x.latitude.Degrees() << " " << h << "\n"; } fout << "\n"; } fout << "\n"; } { // local scope, see what happens when we go out of scope GeoPoint p_start(Angle::Degrees(fixed(-0.3)), Angle::Degrees(fixed(0.0))); p_start += map.GetMapCenter(); GeoPoint p_dest(Angle::Degrees(fixed(0.8)), Angle::Degrees(fixed(-0.7))); p_dest += map.GetMapCenter(); AGeoPoint loc_start(p_start, RoughAltitude(map.GetHeight(p_start) + 100)); AGeoPoint loc_end(p_dest, RoughAltitude(map.GetHeight(p_dest) + 100)); AircraftState state; GlidePolar glide_polar(fixed(0.1)); AirspaceAircraftPerformanceGlide perf(glide_polar); GeoVector vec(loc_start, loc_end); fixed range = fixed(10000) + vec.distance / 2; state.location = loc_start; state.altitude = loc_start.altitude; { Airspaces as_route(airspaces, false); // dummy // real one, see if items changed as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), range); int size_1 = as_route.size(); if (verbose) printf("# route airspace size %d\n", size_1); as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), fixed_one); int size_2 = as_route.size(); if (verbose) printf("# route airspace size %d\n", size_2); ok(size_2 < size_1, "shrink as", 0); // go back as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_end), range); int size_3 = as_route.size(); if (verbose) printf("# route airspace size %d\n", size_3); ok(size_3 >= size_2, "grow as", 0); // and again as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), range); int size_4 = as_route.size(); if (verbose) printf("# route airspace size %d\n", size_4); ok(size_4 >= size_3, "grow as", 0); scan_airspaces(state, as_route, perf, true, loc_end); } // try the solver SpeedVector wind(Angle::Degrees(fixed(0)), fixed(0.0)); GlidePolar polar(fixed_one); GlideSettings settings; settings.SetDefaults(); AirspaceRoute route(airspaces); route.UpdatePolar(settings, polar, polar, wind); route.SetTerrain(&map); RoutePlannerConfig config; config.mode = RoutePlannerConfig::Mode::BOTH; bool sol = false; for (int i = 0; i < NUM_SOL; i++) { loc_end.latitude += Angle::Degrees(fixed(0.1)); loc_end.altitude = map.GetHeight(loc_end) + 100; route.Synchronise(airspaces, loc_start, loc_end); if (route.Solve(loc_start, loc_end, config)) { sol = true; if (verbose) { PrintHelper::print_route(route); } } else { if (verbose) { printf("# fail\n"); } sol = false; } char buffer[80]; sprintf(buffer, "route %d solution", i); ok(sol, buffer, 0); } } return true; }
gcc_constexpr_method RoughAltitude operator+(const RoughAltitude other) const { return RoughAltitude(value + other.value); }
/** * Create a representation of the largest possible value. */ gcc_constexpr_function static RoughAltitude Max() { return RoughAltitude((short)SHRT_MAX); }
/** * Create a representation of the largest possible value. */ constexpr static RoughAltitude Max() { return RoughAltitude((short)SHRT_MAX); }
constexpr RoughAltitude operator-(const RoughAltitude other) const { return RoughAltitude(value - other.value); }
RoughAltitude GetSafetyHeight() const { return RoughAltitude(config.safety_height_terrain); }
static void test_troute(const RasterMap& map, fixed mwind, fixed mc, RoughAltitude ceiling) { GlideSettings settings; settings.SetDefaults(); GlidePolar polar(mc); SpeedVector wind(Angle::Degrees(0), mwind); TerrainRoute route; route.UpdatePolar(settings, polar, polar, wind); route.SetTerrain(&map); GeoPoint origin(map.GetMapCenter()); fixed pd = map.PixelDistance(origin, 1); printf("# pixel size %g\n", (double)pd); bool retval= true; { Directory::Create(_T("output/results")); std::ofstream fout ("output/results/terrain.txt"); unsigned nx = 100; unsigned ny = 100; for (unsigned i=0; i< nx; ++i) { for (unsigned j=0; j< ny; ++j) { fixed fx = (fixed)i / (nx - 1) * 2 - fixed(1); fixed fy = (fixed)j / (ny - 1) * 2 - fixed(1); GeoPoint x(origin.longitude + Angle::Degrees(fixed(0.6) * fx), origin.latitude + Angle::Degrees(fixed(0.4) * fy)); short h = map.GetInterpolatedHeight(x); fout << x.longitude.Degrees() << " " << x.latitude.Degrees() << " " << h << "\n"; } fout << "\n"; } fout << "\n"; } RoutePlannerConfig config; config.mode = RoutePlannerConfig::Mode::BOTH; unsigned i=0; for (fixed ang = fixed(0); ang < fixed_two_pi; ang += fixed(M_PI / 8)) { GeoPoint dest = GeoVector(fixed(40000.0), Angle::Radians(ang)).EndPoint(origin); short hdest = map.GetHeight(dest)+100; retval = route.Solve(AGeoPoint(origin, RoughAltitude(map.GetHeight(origin) + 100)), AGeoPoint(dest, RoughAltitude(positive(mc) ? hdest : std::max(hdest, (short)3200))), config, ceiling); char buffer[80]; sprintf(buffer,"terrain route solve, dir=%g, wind=%g, mc=%g ceiling=%d", (double)ang, (double)mwind, (double)mc, (int)ceiling); ok(retval, buffer, 0); PrintHelper::print_route(route); i++; } // polar.SetMC(fixed(0)); // route.UpdatePolar(polar, wind); }
bool AirspacePredicateHeightRange::check_height(const AbstractAirspace& t) const { return RoughAltitude(t.GetTop().altitude) >= h_min && RoughAltitude(t.GetBase().altitude) <= h_max; }
void FlarmComputer::Process(FlarmData &flarm, const FlarmData &last_flarm, const NMEAInfo &basic) { // Cleanup old calculation instances if (basic.time_available) flarm_calculations.CleanUp(basic.time); // if (FLARM data is available) if (!flarm.IsDetected()) return; double north_to_latitude(0); double east_to_longitude(0); if (basic.location_available) { // Precalculate relative east and north projection to lat/lon // for Location calculations of each target constexpr Angle delta_lat = Angle::Degrees(0.01); constexpr Angle delta_lon = Angle::Degrees(0.01); GeoPoint plat = basic.location; plat.latitude += delta_lat; GeoPoint plon = basic.location; plon.longitude += delta_lon; double dlat = basic.location.DistanceS(plat); double dlon = basic.location.DistanceS(plon); if (fabs(dlat) > 0 && fabs(dlon) > 0) { north_to_latitude = delta_lat.Degrees() / dlat; east_to_longitude = delta_lon.Degrees() / dlon; } } // for each item in traffic for (auto &traffic : flarm.traffic.list) { // if we don't know the target's name yet if (!traffic.HasName()) { // lookup the name of this target's id const TCHAR *fname = FlarmDetails::LookupCallsign(traffic.id); if (fname != NULL) traffic.name = fname; } // Calculate distance traffic.distance = hypot(traffic.relative_north, traffic.relative_east); // Calculate Location traffic.location_available = basic.location_available; if (traffic.location_available) { traffic.location.latitude = Angle::Degrees(traffic.relative_north * north_to_latitude) + basic.location.latitude; traffic.location.longitude = Angle::Degrees(traffic.relative_east * east_to_longitude) + basic.location.longitude; } // Calculate absolute altitude traffic.altitude_available = basic.gps_altitude_available; if (traffic.altitude_available) traffic.altitude = traffic.relative_altitude + RoughAltitude(basic.gps_altitude); // Calculate average climb rate traffic.climb_rate_avg30s_available = traffic.altitude_available; if (traffic.climb_rate_avg30s_available) traffic.climb_rate_avg30s = flarm_calculations.Average30s(traffic.id, basic.time, traffic.altitude); // The following calculations are only relevant for targets // where information is missing if (traffic.track_received && traffic.turn_rate_received && traffic.speed_received && traffic.climb_rate_received) continue; // Check if the target has been seen before in the last seconds const FlarmTraffic *last_traffic = last_flarm.traffic.FindTraffic(traffic.id); if (last_traffic == NULL || !last_traffic->valid) continue; // Calculate the time difference between now and the last contact double dt = traffic.valid.GetTimeDifference(last_traffic->valid); if (dt > 0) { // Calculate the immediate climb rate if (!traffic.climb_rate_received) traffic.climb_rate = (traffic.relative_altitude - last_traffic->relative_altitude) / dt; } else { // Since the time difference is zero (or negative) // we can just copy the old values if (!traffic.climb_rate_received) traffic.climb_rate = last_traffic->climb_rate; } if (dt > 0 && traffic.location_available && last_traffic->location_available) { // Calculate the GeoVector between now and the last contact GeoVector vec = last_traffic->location.DistanceBearing(traffic.location); if (!traffic.track_received) traffic.track = vec.bearing; // Calculate the turn rate if (!traffic.turn_rate_received) { Angle turn_rate = traffic.track - last_traffic->track; traffic.turn_rate = turn_rate.AsDelta().Degrees() / dt; } // Calculate the speed [m/s] if (!traffic.speed_received) traffic.speed = vec.distance / dt; } else { // Since the time difference is zero (or negative) // we can just copy the old values if (!traffic.track_received) traffic.track = last_traffic->track; if (!traffic.turn_rate_received) traffic.turn_rate = last_traffic->turn_rate; if (!traffic.speed_received) traffic.speed = last_traffic->speed; } } }
RoughAltitude RoutePolars::CalcVHeight(const RouteLink &link) const { return RoughAltitude(polar_glide.GetPoint(link.polar_index).gradient * link.d); }
TerrainRoute::TerrainRoute(): m_inx_terrain(0, 0, RoughAltitude(0)) { }