From 789551e23de5ed065498ad11823ed2c2bb8f0ba2 Mon Sep 17 00:00:00 2001 From: Howard Soh Date: Wed, 11 Dec 2024 16:28:16 +0000 Subject: [PATCH] #3012 Added check_llh_to_ecef and test_llh_to_ecef --- .../libcode/vx_grid/search_3d_kdtree.cc | 340 ++++++++++-------- 1 file changed, 185 insertions(+), 155 deletions(-) diff --git a/internal/test_util/libcode/vx_grid/search_3d_kdtree.cc b/internal/test_util/libcode/vx_grid/search_3d_kdtree.cc index bf2097cf1f..f047c58637 100644 --- a/internal/test_util/libcode/vx_grid/search_3d_kdtree.cc +++ b/internal/test_util/libcode/vx_grid/search_3d_kdtree.cc @@ -11,6 +11,8 @@ // // lat=-23.114, lon=334.77, alt=100 (meters) // ==> X: 5309.352 km, Y: -2501.788 km, Z: -2488.375 km +// lat=34.0522, lon=-118.40806, alt=0. (meters) +// ==> X: -2516.715 km, Y: -4653.00 km, Z: 3551.245 km // index=0 -9.59874, 287.326, 100] to [812813, 6.22979e+06, 1.09673e+06]) // ==> X: 1873.072 km, Y: -6004.143 km, Z: -1056.531 km // index=1152 -27.9711, 107.326, 100] to [-5.30871e+06, -2.98241e+06, -1.89224e+06]) @@ -49,13 +51,13 @@ using IndexKDTree = atlas::util::IndexKDTree; static Geometry atlas_geometry; -IndexKDTree *build_tree(vector lat, vector lon, vector alt); -void llh_to_ecef(double lat, double lon, double alt, double *x_km, double *y_km, double *z_km); -void llh_to_ecef_radian(double lat_r, double lon_r, double alt, double *x_km, double *y_km, double *z_km); +IndexKDTree *build_tree(vector &lat, vector &lon, vector &alt_m); +void check_llh_to_ecef(); +void llh_to_ecef(double lat, double lon, double alt_m, double *x_km, double *y_km, double *z_km); +void test_llh_to_ecef(double lat, double lon, double alt_m, double true_x_km, double true_y_km, double true_z_km, string location); //////////////////////////////////////////////////////////////////////// - int main(int argc, char *argv[]) { @@ -63,164 +65,160 @@ int main(int argc, char *argv[]) string lon_name = "glon"; string alt_name = "zalt"; - if(argc < 2) cerr << "\n" << argv[0] - << ": Must specify input NetCDF name to process!\n\n"; - if(argc < 3) cerr << "\n" << argv[0] - << ": Must specify latitude to search\n\n"; - if(argc < 4) cerr << "\n" << argv[0] - << ": Must specify longitude to search\n\n"; - if(argc < 5) cerr << "\n" << argv[0] - << ": Must specify altitude to search\n\n"; - + if(argc < 2) cerr << argv[0] << ": Must specify input NetCDF name to process!\n"; + if(argc < 3) cerr << argv[0] << ": Must specify latitude to search\n"; + if(argc < 4) cerr << argv[0] << ": Must specify longitude to search\n"; if(argc < 5) { + cerr << argv[0] << ": Must specify altitude to search\n"; cerr << "\n Usage: " << argv[0] << " netcdf_name lat_deg lon_deg alt_meters \n"; cerr << " def_lat_var_name=" << lat_name << ", def_lon_var_name=" << lon_name - << ", def_alt_var_name=" << alt_name << "\n\n"; + << ", def_alt_var_name=" << alt_name << "\n"; + + if(argc < 2) check_llh_to_ecef(); + exit(1); } -NcFile * _ncFile = open_ncfile(argv[1]); -if (IS_INVALID_NC_P(_ncFile)) { - if (_ncFile) { - delete _ncFile; - _ncFile = (NcFile *)nullptr; - } - exit(1); -} - -double lat = std::stod(argv[2]); -double lon = std::stod(argv[3]); -double alt = std::stod(argv[4]); + NcFile * _ncFile = open_ncfile(argv[1]); + if (IS_INVALID_NC_P(_ncFile)) { + if (_ncFile) { + delete _ncFile; + _ncFile = (NcFile *)nullptr; + } + exit(1); + } -if(argc > 5) lat_name = argv[5]; -if(argc > 6) lon_name = argv[6]; -if(argc > 7) alt_name = argv[7]; + double lat = std::stod(argv[2]); + double lon = std::stod(argv[3]); + double alt = std::stod(argv[4]); -cout << "Requested lat=" << lat << ", lon=" << lon << ", alt=" << alt - << ", lat_name=" << lat_name << ", lon_name=" << lon_name - << ", alt_name=" << alt_name << "\n\n"; + if(argc > 5) lat_name = argv[5]; + if(argc > 6) lon_name = argv[6]; + if(argc > 7) alt_name = argv[7]; -NcVar lat_var = get_nc_var(_ncFile, lat_name); -NcVar lon_var = get_nc_var(_ncFile, lon_name); -NcVar alt_var = get_nc_var(_ncFile, alt_name); + cout << "Requested lat=" << lat << ", lon=" << lon << ", alt=" << alt + << ", lat_name=" << lat_name << ", lon_name=" << lon_name + << ", alt_name=" << alt_name << "\n\n"; + NcVar lat_var = get_nc_var(_ncFile, lat_name); + NcVar lon_var = get_nc_var(_ncFile, lon_name); + NcVar alt_var = get_nc_var(_ncFile, alt_name); -const int nlat = get_data_size(&lat_var); -const int nlon = get_data_size(&lon_var); -const int nalt = get_data_size(&alt_var); -vector lat_values(nlat); -vector lon_values(nlon); -vector alt_values(nalt); + const int nlat = get_data_size(&lat_var); + const int nlon = get_data_size(&lon_var); + const int nalt = get_data_size(&alt_var); + vector lat_values(nlat); + vector lon_values(nlon); + vector alt_values(nalt); -if (!get_nc_data(&lat_var, lat_values.data())) { - cerr << "\n" - << " Fail to read " << lat_name << "\n\n"; -} -if (!get_nc_data(&lon_var, lon_values.data())) { - cerr << "\n" - << " Fail to read " << lon_name << "\n\n"; -} -if (!get_nc_data(&alt_var, alt_values.data())) { - cerr << "\n" - << " Fail to read " << alt_name << "\n\n"; -} - -double x,y,z; -double x_f,y_f,z_f; -double x_m,y_m,z_m; -double x_l,y_l,z_l; - -llh_to_ecef(lat, lon, alt, &x, &y, &z); -PointXYZ target_point(x, y, z); - -int idx = 0; -llh_to_ecef(lat_values[idx], lon_values[idx], alt_values[idx], &x_f, &y_f, &z_f); -PointXYZ target_point_first(x_f, y_f, z_f); - -idx = lat_values.size()/2; -llh_to_ecef(lat_values[idx], lon_values[idx], alt_values[idx], &x_m, &y_m, &z_m); -PointXYZ target_point_mid(x_m, y_m, z_m); - -idx = lat_values.size() - 1; -llh_to_ecef(lat_values[idx], lon_values[idx], alt_values[idx], &x_l, &y_l, &z_l); -PointXYZ target_point_last(x_l, y_l, z_l); - -//bool in_distance; -int closest_n = 5; -double distance_km; -IndexKDTree *kdtree = build_tree(lat_values, lon_values, alt_values); - -idx = 0; -cout << "The first point from the input file: " - << " index=" << idx << " " << lat_values[idx] << ", " << lon_values[idx] << ", " << alt_values[idx] << "] to [" << x_f << ", " << y_f << ", " << z_f << "])\n"; -IndexKDTree::ValueList neighbor = kdtree->closestPoints(target_point_first, closest_n); -for (size_t i=0; i < neighbor.size(); i++) { - size_t index(neighbor[i].payload()); - int index2 = (int)neighbor[i].payload(); - distance_km = neighbor[i].distance() / 1000.; - //in_distance = is_in_distance(distance_km); - llh_to_ecef(lat_values[index], lon_values[index], alt_values[index], &x, &y, &z); - cout << " - " << i << ": index=" << index << " distance=" << distance_km << " from [" - << lat_values[index] << ", " << lon_values[index] << ", " << alt_values[index] << "] (" - << x << ", " << y << ", " << z << ")\n"; -} -cout << "\n"; - -idx = lat_values.size()/2; -cout << "The mid point from the input file: " - << " index=" << idx << " " << lat_values[idx] << ", " << lon_values[idx] << ", " << alt_values[idx] << "] to [" << x_m << ", " << y_m << ", " << z_m << "])\n"; -neighbor = kdtree->closestPoints(target_point_mid, closest_n); -for (size_t i=0; i < neighbor.size(); i++) { - size_t index(neighbor[i].payload()); - int index2 = (int)neighbor[i].payload(); - distance_km = neighbor[i].distance() / 1000.; - //in_distance = is_in_distance(distance_km); - llh_to_ecef(lat_values[index], lon_values[index], alt_values[index], &x, &y, &z); - cout << " - " << i << ": index=" << index << " distance=" << distance_km << " from [" - << lat_values[index] << ", " << lon_values[index] << ", " << alt_values[index] << "] (" - << x << ", " << y << ", " << z << ")\n"; -} -cout << "\n"; - -idx = lat_values.size() - 1; -cout << "The last point from the input file: " - << " index=" << idx << " " << lat_values[idx] << ", " << lon_values[idx] << ", " << alt_values[idx] << "] to [" << x_m << ", " << y_m << ", " << z_m << "])\n"; -neighbor = kdtree->closestPoints(target_point_last, closest_n); -for (size_t i=0; i < neighbor.size(); i++) { - size_t index(neighbor[i].payload()); - int index2 = (int)neighbor[i].payload(); - distance_km = neighbor[i].distance() / 1000.; - //in_distance = is_in_distance(distance_km); - llh_to_ecef(lat_values[index], lon_values[index], alt_values[index], &x, &y, &z); - cout << " - " << i << ": index=" << index << " distance=" << distance_km << " from [" - << lat_values[index] << ", " << lon_values[index] << ", " << alt_values[index] << "] (" - << x << ", " << y << ", " << z << ")\n"; -} -cout << "\n"; - -cout << "The requested point: " - << lat << ", " << lon << ", " << alt - << "] to [" << x << ", " << y << ", " << z << "])\n"; -neighbor = kdtree->closestPoints(target_point, closest_n); -for (size_t i=0; i < neighbor.size(); i++) { - size_t index(neighbor[i].payload()); - int index2 = (int)neighbor[i].payload(); - distance_km = neighbor[i].distance() / 1000.; - //in_distance = is_in_distance(distance_km); - llh_to_ecef(lat_values[index], lon_values[index], alt_values[index], &x, &y, &z); - cout << " - " << i << ": index=" << index << " distance=" << distance_km << " from [" - << lat_values[index] << ", " << lon_values[index] << ", " << alt_values[index] << "] (" - << x << ", " << y << ", " << z << ")\n"; -} -cout << "\n"; + if (!get_nc_data(&lat_var, lat_values.data())) { + cerr << "\n" + << " Fail to read " << lat_name << "\n\n"; + } + if (!get_nc_data(&lon_var, lon_values.data())) { + cerr << "\n" + << " Fail to read " << lon_name << "\n\n"; + } + if (!get_nc_data(&alt_var, alt_values.data())) { + cerr << "\n" + << " Fail to read " << alt_name << "\n\n"; + } + double x,y,z; + double x_f,y_f,z_f; + double x_m,y_m,z_m; + double x_l,y_l,z_l; + + llh_to_ecef(lat, lon, alt, &x, &y, &z); + PointXYZ target_point(x, y, z); + + int idx = 0; + llh_to_ecef(lat_values[idx], lon_values[idx], alt_values[idx], &x_f, &y_f, &z_f); + PointXYZ target_point_first(x_f, y_f, z_f); + + idx = lat_values.size()/2; + llh_to_ecef(lat_values[idx], lon_values[idx], alt_values[idx], &x_m, &y_m, &z_m); + PointXYZ target_point_mid(x_m, y_m, z_m); + + idx = lat_values.size() - 1; + llh_to_ecef(lat_values[idx], lon_values[idx], alt_values[idx], &x_l, &y_l, &z_l); + PointXYZ target_point_last(x_l, y_l, z_l); + + //bool in_distance; + int closest_n = 5; + double distance_km; + IndexKDTree *kdtree = build_tree(lat_values, lon_values, alt_values); + + idx = 0; + cout << "The first point from the input file: " + << " index=" << idx << " " << lat_values[idx] << ", " << lon_values[idx] << ", " << alt_values[idx] << "] to [" << x_f << ", " << y_f << ", " << z_f << "])\n"; + IndexKDTree::ValueList neighbor = kdtree->closestPoints(target_point_first, closest_n); + for (size_t i=0; i < neighbor.size(); i++) { + size_t index(neighbor[i].payload()); + int index2 = (int)neighbor[i].payload(); + distance_km = neighbor[i].distance() / 1000.; + //in_distance = is_in_distance(distance_km); + llh_to_ecef(lat_values[index], lon_values[index], alt_values[index], &x, &y, &z); + cout << " - " << i << ": index=" << index << " distance=" << distance_km << " from [" + << lat_values[index] << ", " << lon_values[index] << ", " << alt_values[index] << "] (" + << x << ", " << y << ", " << z << ")\n"; + } + cout << "\n"; + + idx = lat_values.size()/2; + cout << "The mid point from the input file: " + << " index=" << idx << " " << lat_values[idx] << ", " << lon_values[idx] << ", " << alt_values[idx] << "] to [" << x_m << ", " << y_m << ", " << z_m << "])\n"; + neighbor = kdtree->closestPoints(target_point_mid, closest_n); + for (size_t i=0; i < neighbor.size(); i++) { + size_t index(neighbor[i].payload()); + int index2 = (int)neighbor[i].payload(); + distance_km = neighbor[i].distance() / 1000.; + //in_distance = is_in_distance(distance_km); + llh_to_ecef(lat_values[index], lon_values[index], alt_values[index], &x, &y, &z); + cout << " - " << i << ": index=" << index << " distance=" << distance_km << " from [" + << lat_values[index] << ", " << lon_values[index] << ", " << alt_values[index] << "] (" + << x << ", " << y << ", " << z << ")\n"; + } + cout << "\n"; + + idx = lat_values.size() - 1; + cout << "The last point from the input file: " + << " index=" << idx << " " << lat_values[idx] << ", " << lon_values[idx] << ", " << alt_values[idx] << "] to [" << x_m << ", " << y_m << ", " << z_m << "])\n"; + neighbor = kdtree->closestPoints(target_point_last, closest_n); + for (size_t i=0; i < neighbor.size(); i++) { + size_t index(neighbor[i].payload()); + int index2 = (int)neighbor[i].payload(); + distance_km = neighbor[i].distance() / 1000.; + //in_distance = is_in_distance(distance_km); + llh_to_ecef(lat_values[index], lon_values[index], alt_values[index], &x, &y, &z); + cout << " - " << i << ": index=" << index << " distance=" << distance_km << " from [" + << lat_values[index] << ", " << lon_values[index] << ", " << alt_values[index] << "] (" + << x << ", " << y << ", " << z << ")\n"; + } + cout << "\n"; + + cout << "The requested point: " + << lat << ", " << lon << ", " << alt + << "] to [" << x << ", " << y << ", " << z << "])\n"; + neighbor = kdtree->closestPoints(target_point, closest_n); + for (size_t i=0; i < neighbor.size(); i++) { + size_t index(neighbor[i].payload()); + int index2 = (int)neighbor[i].payload(); + distance_km = neighbor[i].distance() / 1000.; + //in_distance = is_in_distance(distance_km); + llh_to_ecef(lat_values[index], lon_values[index], alt_values[index], &x, &y, &z); + cout << " - " << i << ": index=" << index << " distance=" << distance_km << " from [" + << lat_values[index] << ", " << lon_values[index] << ", " << alt_values[index] << "] (" + << x << ", " << y << ", " << z << ")\n"; + } + cout << "\n"; -if (_ncFile) { - delete _ncFile; - _ncFile = (NcFile *)nullptr; -} + if (_ncFile) { + delete _ncFile; + _ncFile = (NcFile *)nullptr; + } // // done @@ -230,7 +228,9 @@ if (_ncFile) { } -IndexKDTree *build_tree(vector lat, vector lon, vector alt) { +//////////////////////////////////////////////////////////////////////// + +IndexKDTree *build_tree(vector &lat, vector &lon, vector &alt_m) { double x; double y; @@ -240,8 +240,7 @@ IndexKDTree *build_tree(vector lat, vector lon, vector a IndexKDTree *kdtree = new IndexKDTree(atlas_geometry); kdtree->reserve(count); for (int i=0; iinsert(point, n++); } @@ -251,11 +250,27 @@ IndexKDTree *build_tree(vector lat, vector lon, vector a return kdtree; } -void llh_to_ecef(double lat, double lon, double alt, double *x_km, double *y_km, double *z_km) { - llh_to_ecef_radian(lat*rad_per_deg, lon*rad_per_deg, alt, x_km, y_km, z_km); +//////////////////////////////////////////////////////////////////////// + +void check_llh_to_ecef() { + double lat; + double lon; + double alt_m; + double true_x_km; + double true_y_km; + double true_z_km; + + test_llh_to_ecef(34.0522, -118.40806, 0.0, -2516.715, -4653.003, 3551.245, "LA"); + test_llh_to_ecef(-9.59874, 287.326, 100, 1873.072, -6004.143, -1056.531, "First Point"); + test_llh_to_ecef(-27.9711, 107.326, 100, -1678.837, 5381.522, -2973.724, "Middle Point"); + test_llh_to_ecef(75.6264, 287.326, 100, 473.024, -1516.283, 6156.59, "Last Point"); } -void llh_to_ecef_radian(double lat_r, double lon_r, double alt, double *x_km, double *y_km, double *z_km) { +//////////////////////////////////////////////////////////////////////// + +void llh_to_ecef(double lat, double lon, double alt_m, double *x_km, double *y_km, double *z_km) { + const double lat_r = lat*rad_per_deg; + const double lon_r = lon*rad_per_deg; const double radius = 6378137.0; // Radius of the Earth (in meters) const double flat_factor = 1.0/298.257223563; // Flattening factor WGS84 Model double cosLat = cos(lat_r); @@ -264,14 +279,28 @@ void llh_to_ecef_radian(double lat_r, double lon_r, double alt, double *x_km, do double C = 1./sqrt(cosLat*cosLat + FF * sinLat*sinLat); double S = C * FF; - *x_km = (radius * C + alt) * cosLat * cos(lon_r) / 1000.; - *y_km = (radius * C + alt) * cosLat * sin(lon_r) / 1000.; - *z_km = (radius * S + alt) * sinLat / 1000.; + *x_km = (radius * C + alt_m) * cosLat * cos(lon_r) / 1000.; + *y_km = (radius * C + alt_m) * cosLat * sin(lon_r) / 1000.; + *z_km = (radius * S + alt_m) * sinLat / 1000.; } +//////////////////////////////////////////////////////////////////////// + +void test_llh_to_ecef(double lat, double lon, double alt_m, double true_x_km, double true_y_km, double true_z_km, string location) { + double x_km; + double y_km; + double z_km; + + llh_to_ecef(lat, lon, alt_m, &x_km, &y_km, &z_km); + cout << location << ": (" << lat << ", " << lon << ", " << alt_m + << ") => (" << x_km << ", " << y_km << ", " << z_km + << ") Diff: (" << (true_x_km - x_km) << ", " << (true_y_km - y_km) << ", " << (true_z_km - z_km) << ")\n"; + +} //////////////////////////////////////////////////////////////////////// + /* https://stackoverflow.com/questions/10473852/convert-latitude-and-longitude-to-point-in-3d-space @@ -322,6 +351,7 @@ Flattening factor, f depends on the model you assume for your conversion. Typica Personally, I like to use this link to Naval Postgraduate School for sanity checks on my conversions. https://www.oc.nps.edu/oc2902w/coord/llhxyz.html +https://www.oc.nps.edu/oc2902w/coord/llhxyz.htm https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates