From f70e4ccfaae7e8a7f3d072f74d5ca0f9cdab03a6 Mon Sep 17 00:00:00 2001 From: Howard Soh Date: Wed, 11 Dec 2024 16:50:51 +0000 Subject: [PATCH] #3012 Reanmed point_lonlat to points_lonlat. Added points_XYZ & points_XYZ_km. Added llh_to_ecef APIs --- src/libcode/vx_grid/unstructured_grid.cc | 345 +++++++++++++++++++++-- 1 file changed, 318 insertions(+), 27 deletions(-) diff --git a/src/libcode/vx_grid/unstructured_grid.cc b/src/libcode/vx_grid/unstructured_grid.cc index 98cf8bccb9..9958a803e4 100644 --- a/src/libcode/vx_grid/unstructured_grid.cc +++ b/src/libcode/vx_grid/unstructured_grid.cc @@ -28,16 +28,29 @@ using namespace std; +using PointXYZ = atlas::PointXYZ; using PointLonLat = atlas::PointLonLat; using Geometry = atlas::Geometry; using IndexKDTree = atlas::util::IndexKDTree; //////////////////////////////////////////////////////////////////////// +constexpr double EARTH_RADIUS = 6378137.0; // Radius of the Earth (in meters) +constexpr double FLAT_FACTOR = 1.0/298.257223563; // Flattening factor WGS84 Model + + static const int UGRID_DEBUG_LEVEL = 9; static atlas::Geometry atlas_geometry; +//////////////////////////////////////////////////////////////////////// + +void llh_to_ecef(double lat, double lon, double alt_m, + double *x_km, double *y_km, double *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, + string location); + //////////////////////////////////////////////////////////////////////// @@ -103,7 +116,12 @@ void UnstructuredGrid::set_from_data(const UnstructuredData &data) { Data.n_node = data.n_node; Data.max_distance_km = data.max_distance_km; - Data.set_points(Nx, data.point_lonlat); + if (data.has_PointLatLon()) { + Data.set_points(Nx, data.points_lonlat); + } + else { + Data.set_points(Nx, data.points_XYZ); + } } @@ -120,9 +138,9 @@ void UnstructuredGrid::set_max_distance_km(double max_distance) { void UnstructuredGrid::latlon_to_xy(double lat, double lon, double &x, double &y) const { - PointLonLat _point_lonlat(lon, lat); + PointLonLat point_lonlat(lon, lat); - IndexKDTree::ValueList neighbor = Data.kdtree->closestPoints(_point_lonlat, 1); + IndexKDTree::ValueList neighbor = Data.closest_points(lat, lon, 1); size_t index(neighbor[0].payload()); double distance_km(neighbor[0].distance()/1000.); bool in_distance = Data.is_in_distance(distance_km); @@ -133,9 +151,9 @@ void UnstructuredGrid::latlon_to_xy(double lat, double lon, double &x, double &y if(mlog.verbosity_level() >= UGRID_DEBUG_LEVEL) mlog << Debug(UGRID_DEBUG_LEVEL) << "UnstructuredGrid::latlon_to_xy() " << "input=(" << lon << ", " << lat << ") ==> (" << x << ", " << y << ") == (" - << Data.point_lonlat[index].x() << ", " << Data.point_lonlat[index].y() + << Data.points_lonlat[index].x() << ", " << Data.points_lonlat[index].y() << ") distance= " << distance_km << "km, " - << _point_lonlat.distance(Data.point_lonlat[index]) + << point_lonlat.distance(Data.points_lonlat[index]) << " degree" << (in_distance ? " " : ", rejected") << "\n"; } @@ -143,10 +161,42 @@ void UnstructuredGrid::latlon_to_xy(double lat, double lon, double &x, double &y //////////////////////////////////////////////////////////////////////// +//void UnstructuredGrid::latlonalt_to_xy(double lat, double lon, double alt_m, double &x, double &y) const { +// +// double x_km; +// double y_km; +// double z_km; +// llh_to_ecef(lat, lon, alt_m, &x_km, &y_km, &z_km); +// points_XYZ_km[i] = {x_km, y_km, z_km}; +// PointXYZ _pointXYZ(x_km, y_km, z_km); +// +// IndexKDTree::ValueList neighbor = Data.closest_points(lat, lon, 1, alt_m); +// size_t index(neighbor[0].payload()); +// double distance_km(neighbor[0].distance()/1000.); +// bool in_distance = Data.is_in_distance(distance_km); +// +// x = in_distance ? index : -1.0; +// y = 0; +// +// if(mlog.verbosity_level() >= UGRID_DEBUG_LEVEL) mlog +// << Debug(UGRID_DEBUG_LEVEL) << "UnstructuredGrid::latlonalt_to_xy() " +// << "input(lat,lon,alt)=(" << lat << ", " << lon <<", " << alt +// << ") ==> km(" << x_km << ", " << y_km << ", " << z_km +// << ") ==> (" << x << ", " << y << ") == (" +// << Data.points_XYZ[index].x() << ", " << Data.points_XYZ[index].y() +// << ", " << Data.points_XYZ[index].z() << ") distance= " << distance_km << "km, " +// << _pointXYZ.distance(Data.points_XYZ[index]) +// << " km" << (in_distance ? " " : ", rejected") << "\n"; +//} + + +//////////////////////////////////////////////////////////////////////// + + void UnstructuredGrid::xy_to_latlon(double x, double y, double &lat, double &lon) const { - lat = Data.point_lonlat[x].y(); - lon = Data.point_lonlat[x].x(); + lat = Data.points_lonlat[x].y(); + lon = Data.points_lonlat[x].x(); if(mlog.verbosity_level() >= UGRID_DEBUG_LEVEL) mlog << Debug(UGRID_DEBUG_LEVEL) << "UnstructuredGrid::xy_to_latlon() " @@ -349,22 +399,73 @@ void UnstructuredData::build_tree() { atlas::idx_t n = 0; kdtree = new IndexKDTree(atlas_geometry); kdtree->reserve(n_face); - for (int i=0; iinsert(pointLL, n++); - lat_checksum += (i+1) * point_lonlat[i].y(); - lon_checksum += (i+1) * point_lonlat[i].x(); + if (has_PointLatLon()) { + for (int i=0; iinsert(pointLL, n++); + lat_checksum += (i+1) * points_lonlat[i].y(); + lon_checksum += (i+1) * points_lonlat[i].x(); + } + } + else { + double x_km; + double y_km; + double z_km; + points_XYZ_km.reserve(n_face); + for (int i=0; iinsert(points_XYZ_km[i], n++); + lat_checksum += (i+1) * y_km; + lon_checksum += (i+1) * x_km; + alt_checksum += (i+1) * z_km; + } } kdtree->build(); + ConcatString cs; + if (get_env("MET_TEST_UGRID_KDTREE", cs)) test_kdtree(); } //////////////////////////////////////////////////////////////////////// +IndexKDTree::ValueList UnstructuredData::closest_points(const double &lat, const double &lon, + const size_t &k, const double &alt_m) const { + const string method_name = "UnstructuredData::closest_points() --> "; + if (has_PointLatLon()) { + PointLonLat point_lonlat(lon, lat); + if (!is_eq(alt_m, bad_data_double)) { + mlog << Debug(4) << method_name << "ignored the altitude (" << alt_m << ")\n"; + } + return kdtree->closestPoints(point_lonlat, k); + } + else { + double x_km; + double y_km; + double z_km; + double _alt_m = alt_m; + if (is_eq(_alt_m, bad_data_double)) { + _alt_m = EARTH_RADIUS; + mlog << Debug(4) << method_name << "Set the altitude to earth radius (" << EARTH_RADIUS << ")\n"; + } + llh_to_ecef(lat, lon, _alt_m, &x_km, &y_km, &z_km); + PointXYZ point_XYZ(x_km, y_km, z_km); + return kdtree->closestPoints(point_XYZ, k); + } +}; + +//////////////////////////////////////////////////////////////////////// + void UnstructuredData::copy_from(const UnstructuredData &us_data) { - set_points(us_data.n_face, us_data.point_lonlat); + if (us_data.has_PointLatLon()) { + set_points(us_data.n_face, us_data.points_lonlat); + } + else { + set_points(us_data.n_face, us_data.points_XYZ); + } n_edge = us_data.n_edge; n_node = us_data.n_node; max_distance_km = us_data.max_distance_km; @@ -373,7 +474,12 @@ void UnstructuredData::copy_from(const UnstructuredData &us_data) { //////////////////////////////////////////////////////////////////////// void UnstructuredData::copy_from(const UnstructuredData *us_data) { - set_points(us_data->n_face, us_data->point_lonlat); + if (us_data->has_PointLatLon()) { + set_points(us_data->n_face, us_data->points_lonlat); + } + else { + set_points(us_data->n_face, us_data->points_XYZ); + } n_edge = us_data->n_edge; n_node = us_data->n_node; max_distance_km = us_data->max_distance_km; @@ -381,6 +487,12 @@ void UnstructuredData::copy_from(const UnstructuredData *us_data) { //////////////////////////////////////////////////////////////////////// +bool UnstructuredData::has_PointLatLon() const { + return (points_lonlat.size() > 0); +} + +//////////////////////////////////////////////////////////////////////// + bool UnstructuredData::is_in_distance(double distance_km) const { bool in_distance = is_eq(max_distance_km, bad_data_double) || (max_distance_km <= 0) @@ -397,14 +509,14 @@ void UnstructuredData::set_points(int count, double *_lon, double *_lat) { clear_data(); n_face = count; - point_lonlat.reserve(count); + points_lonlat.reserve(count); for (int i=0; i= UGRID_DEBUG_LEVEL) mlog << Debug(UGRID_DEBUG_LEVEL) << "UnstructuredData::set_points(int, double *, double *) first (" - << point_lonlat[0].x() << ", " << point_lonlat[0].y() << ") and last (" - << point_lonlat[count-1].x() << ", " << point_lonlat[count-1].y() << ") from (" + << points_lonlat[0].x() << ", " << points_lonlat[0].y() << ") and last (" + << points_lonlat[count-1].x() << ", " << points_lonlat[count-1].y() << ") from (" << _lon[0] << ", " << _lat[0] << ") and (" << _lon[count-1] << ", " << _lat[count-1] << ")\n"; @@ -414,24 +526,203 @@ void UnstructuredData::set_points(int count, double *_lon, double *_lat) { //////////////////////////////////////////////////////////////////////// -void UnstructuredData::set_points(int count, const std::vector &ptLonLat) { +void UnstructuredData::set_points(int count, double *_lon, double *_lat, double *_alt) { clear_data(); + double x_km; + double y_km; + double z_km; n_face = count; - point_lonlat.reserve(count); + points_XYZ.reserve(count); for (int i=0; i= UGRID_DEBUG_LEVEL) mlog - << Debug(UGRID_DEBUG_LEVEL) << "UnstructuredData::set_points(int, std::vector &) first: (" - << point_lonlat[0].x() << ", " << point_lonlat[0].y() << ") and last (" - << point_lonlat[count-1].x() << ", " << point_lonlat[count-1].y() << ") from (" - << ptLonLat[0].x() << ", " << ptLonLat[0].y() << ") and (" - << ptLonLat[count-1].x() << ", " << ptLonLat[count-1].y() << ")\n"; + << Debug(UGRID_DEBUG_LEVEL) << "UnstructuredData::set_points(int, double *lon, double *lat, double *alt) first (" + << points_XYZ[0].x() << ", " << points_XYZ[0].y() << ", " << points_XYZ[0].z() << ") and last (" + << points_XYZ[count-1].x() << ", " << points_XYZ[count-1].y() << ") from (" + << _lon[0] << ", " << _lat[0] << ", " << _alt[0] << ") and (" + << _lon[count-1] << ", " << _lat[count-1] << ")\n"; build_tree(); } //////////////////////////////////////////////////////////////////////// + +void UnstructuredData::set_points(int count, const std::vector &pointsLL) { + static const string method_name + = "UnstructuredData::set_points(int, std::vector &) -> "; + + clear_data(); + + if (count != pointsLL.size()) { + mlog << Warning << "\n" << method_name + << " The count argument (" << count << ") does not match with vector (" + << pointsLL.size() << ")\n\n"; + } + + n_face = count; + points_lonlat.reserve(count); + for (int i=0; i= UGRID_DEBUG_LEVEL) mlog + << Debug(UGRID_DEBUG_LEVEL) << method_name << " first: (" + << points_lonlat[0].x() << ", " << points_lonlat[0].y() << ") and last (" + << points_lonlat[count-1].x() << ", " << points_lonlat[count-1].y() << ") from (" + << pointsLL[0].x() << ", " << pointsLL[0].y() << ") and (" + << pointsLL[count-1].x() << ", " << pointsLL[count-1].y() << ")\n"; + + build_tree(); + +} + +//////////////////////////////////////////////////////////////////////// + +void UnstructuredData::set_points(int count, const std::vector &pointsXYZ) { + static const string method_name + = "UnstructuredData::set_points(int, std::vector &) -> "; + + clear_data(); + + if (count != pointsXYZ.size()) { + mlog << Warning << "\n" << method_name + << " The count argument (" << count << ") does not match with vector (" + << pointsXYZ.size() << ")\n\n"; + } + + n_face = count; + points_XYZ.reserve(count); + for (int i=0; i= UGRID_DEBUG_LEVEL) { + int last_i = count - 1; + mlog << Debug(UGRID_DEBUG_LEVEL) << method_name + << "first: (" << points_XYZ[0].x() << ", " << points_XYZ[0].y() << ", " + << points_XYZ[0].z() << ") and last (" << points_XYZ[last_i].x() << ", " + << points_XYZ[last_i].y() << ", " << points_XYZ[last_i].z() << ") from (" + << pointsXYZ[0].x() << ", " << pointsXYZ[0].y() << ", " << pointsXYZ[0].z() + << ") and (" << pointsXYZ[last_i].x() << ", " << pointsXYZ[last_i].y() + << ", " << pointsXYZ[last_i].z() << ")\n"; + } + + build_tree(); + +} + +//////////////////////////////////////////////////////////////////////// + +void UnstructuredData::test_kdtree() { + static const string method_name + = "UnstructuredData::test_kdtree() -> "; + + int closest_n = 5; + double lat; + double lon; + double distance_km; + vector indices = {0, n_face/2, (n_face - 1)}; + if (has_PointLatLon()) { + if (n_face != points_lonlat.size()) { + mlog << Warning << "\n" << method_name + << " The count argument (" << n_face << ") does not match with the vector (latlon=" + << points_lonlat.size() << ", XYZ=" << points_XYZ.size() << ")\n\n"; + } + for (int idx : indices) { + lat = points_lonlat[idx].y(); + lon = points_lonlat[idx].x(); + cout << " - search index=" << idx << " (" << lat << ", " << lon << ")\n"; + IndexKDTree::ValueList neighbor = closest_points(lon, lat, closest_n); + for (size_t i=0; i < neighbor.size(); i++) { + int index = (int)neighbor[i].payload(); + distance_km = neighbor[i].distance() / 1000.; + cout << " + closest index=" << index << " distance=" << distance_km << " from (" + << points_lonlat[index].y() << ", " << points_lonlat[index].x() << ")\n"; + } + cout << "\n"; + } + } + else { + double x_km; + double y_km; + double z_km; + double alt_m; + if (n_face != points_XYZ.size()) { + mlog << Warning << "\n" << method_name + << " The count argument (" << n_face << ") does not match with the vector (XYZ=" + << points_XYZ.size() << ", latlon=" << points_lonlat.size() << ")\n\n"; + } + for (int idx : indices) { + lat = points_XYZ[idx].y(); + lon = points_XYZ[idx].x(); + alt_m = points_XYZ[idx].z(); + cout << " - search index=" << idx << " (" << lat << ", " << lon << ", " << alt_m << ")\n"; + IndexKDTree::ValueList neighbor = closest_points(lat, lon, closest_n, alt_m); + for (size_t i=0; i < neighbor.size(); i++) { + int index = (int)neighbor[i].payload(); + distance_km = neighbor[i].distance() / 1000.; + llh_to_ecef(lat, lon, alt_m, &x_km, &y_km, &z_km); + cout << " + closest index=" << index << " distance=" << distance_km << " from (" + << points_XYZ[index].y() << ", " << points_XYZ[index].x() << ", " << points_XYZ[index].z() + << ") km: [" << x_km << ", " << y_km << ", " << z_km << "]\n"; + } + cout << "\n"; + } + //if(mlog.verbosity_level() >= UGRID_DEBUG_LEVEL) { + // mlog << Debug(UGRID_DEBUG_LEVEL) << method_name + // << "first: (" << points_XYZ[0].x() << ", " << points_XYZ[0].y() << ", " + // << points_XYZ[0].z() << ") and last (" << points_XYZ[last_i].x() << ", " + // << points_XYZ[last_i].y() << ", " << points_XYZ[last_i].z() << ") from (" + // << points_XYZ[0].x() << ", " << points_XYZ[0].y() << ", " << points_XYZ[0].z() + // << ") and (" << points_XYZ[last_i].x() << ", " << points_XYZ[last_i].y() + // << ", " << points_XYZ[last_i].z() << ")\n"; + //} + } + +} + +//////////////////////////////////////////////////////////////////////// +// Called by internal/test_util/libcode/vx_grid/search_3d_kdtree_api.cc +// Input: /d1/personal/dadriaan/projects/NRL/PyIRI/pyiri_f4_2020.nc +// - search index= 0 (-9.59874, 287.326, 100) ==> ( 1873.072, -6004.143, -1056.531) km +// - search index=1152 (-27.9711, 107.326, 100) ==> (-1678.837, 5381.522, -2973.724) km +// - search index=2303 ( 75.6264, 287.326, 100) ==> ( 473.024, -1516.283, 6156.59 ) km + +void UnstructuredData::test_llh_to_ecef() { + check_llh_to_ecef( 34.0522, -118.40806, 0., -2516.715, -4653.003, 3551.245, " LA"); + check_llh_to_ecef(-9.59874, 287.326, 100., 1873.072, -6004.143, -1056.531, " First Point"); + check_llh_to_ecef(-27.9711, 107.326, 100., -1678.837, 5381.522, -2973.724, " Middle Point"); + check_llh_to_ecef( 75.6264, 287.326, 100., 473.024, -1516.283, 6156.59, " Last Point"); +} + +//////////////////////////////////////////////////////////////////////// + +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; + double cosLat = cos(lat_r); + double sinLat = sin(lat_r); + double FF = (1.0-FLAT_FACTOR)*(1.0-FLAT_FACTOR); + double C = 1./sqrt(cosLat*cosLat + FF * sinLat*sinLat); + double S = C * FF; + + *x_km = (EARTH_RADIUS * C + alt_m) * cosLat * cos(lon_r) / 1000.; + *y_km = (EARTH_RADIUS * C + alt_m) * cosLat * sin(lon_r) / 1000.; + *z_km = (EARTH_RADIUS * S + alt_m) * sinLat / 1000.; +} + +//////////////////////////////////////////////////////////////////////// + +void check_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"; + +}