diff --git a/README.md b/README.md index 73b8ac5..4f55f79 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # llh_converter -(Updated 2022/03/24) +(Updated 2025/03/17) This repository has two class implementation. @@ -17,6 +17,7 @@ Convert height between ellipsoid and orthometric library * EGM2008-1 * GSIGEO2011 Ver2.1 +* GSIGEO2024beta (Data file will be updated in 2025/04) ### Usage @@ -27,7 +28,7 @@ hc.setGeoidType(height_converter::GeoidType::GSIGEO2011); // Select Geoid Model // hc.setGeoidType(height_converter::GeoidType::EGM2008); hc.setGSIGEOGeoidFile(path_to_gsigeo_asc_file); // Load geoid data file when you select GSIGEO -// hc.setGSIGEOGeoidFile(); // If called with void, it try to read /usr/share/GSIGEO/gsigeo2011_ver2_1.asc +// hc.setGSIGEOGeoidFile(); // If called with void, it try to read geoid data files under /usr/share/GSIGEO/ double geoid_heith = hc.getGeoid(lat, lon); // Get geoid heigth with latitude/longitude in decimal degree @@ -46,19 +47,29 @@ Convert latitude/longitude/altitude into XYZ coordinate system. * Millitary Grid Reference System (MGRS) * Japan Plane Rectangular Coordinate System (JPRCS) +* Transverse Mercator with an arbitrary origin (TM) ### Usage ``` llh_converter::LLHConverter lc; -llh_converter::LLHParam param; // parameter for conversion -param.use_mgrs = true; // set true if you want to use MGRS -param.plane_num = 9; // set the plane number when you use JPRCS -param.mgrs_code = "53SPU"; // MGRS grid code is required when you revert MGRS x/y into lat/lon +llh_converter::LLHParam param; // parameter for conversion +param.projection_method = llh_converter::ProjectionMethod::TM; + // set the projection method TM/JPRCS/MGRS +param.plane_num = "9"; // set the grid code for JPRCS/MGRS + // for MGRS, it's required only when reverting to lat/lon param.height_convert_type = llh_converter::ConvertType::ELLIPS2ORTHO; - // You can also convert height + // You can also convert height param.geoid_type = llh_converter::GeoidType::EGM2008; - // Set geoid model + // Set geoid model +// The following tm_param is required only when the projection method is TM +param.tm_param.inv_flatten_ratio = 298.257222101; + // Set the inverse flattening ratio +param.tm_param.semi_mejor_axis = 6378137.0; // Set the semi-major axis +param.tm_param.scale_factor = 0.9996; // Set the scale factor +param.tm_param.origin_lat_rad = 35.0 * M_PI / 180.; +param.tm_param.origin_lon_rad = 139.0 * M_PI / 180.; + // Set the origin double lat_deg, lon_deg, alt; double lat_rad = lat_deg * M_PI / 180.; @@ -87,8 +98,8 @@ The meridian convergence angle is calculated by the `getMeridianConvergence()` f ``` llh_converter::LLHConverter lc; llh_converter::LLHParam param; - param.use_mgrs = false; - param.plane_num = 7; + param.projection_method = llh_converter::ProjectionMethod::JPRCS; + param.grid_code = "7"; param.height_convert_type = llh_converter::ConvertType::NONE; param.geoid_type = llh_converter::GeoidType::EGM2008; @@ -130,4 +141,4 @@ This package contains GSIGEO2011/GSIGEO2024 geoid data file which is provided by ## LICENSE -This package is provided under the [BSD 3-Clauses](LICENSE) License. \ No newline at end of file +This package is provided under the [BSD 3-Clauses](LICENSE) License. diff --git a/include/llh_converter/llh_converter.hpp b/include/llh_converter/llh_converter.hpp index a9c9735..5755838 100644 --- a/include/llh_converter/llh_converter.hpp +++ b/include/llh_converter/llh_converter.hpp @@ -53,14 +53,30 @@ enum class MGRSPrecision MICRO_METER_100 = 9, }; +enum class ProjectionMethod +{ + TM = 0, + JPRCS = 1, + MGRS = 2, +}; + +struct TMParam +{ + double inv_flatten_ratio; + double semi_major_axis; + double scale_factor; + double origin_lat_rad; + double origin_lon_rad; +}; + struct LLHParam { - bool use_mgrs; - int plane_num; - std::string mgrs_code; + ProjectionMethod projection_method; + std::string grid_code; // MGRSPrecision precision; ConvertType height_convert_type; GeoidType geoid_type; + TMParam tm_param; }; class LLHConverter @@ -75,8 +91,11 @@ class LLHConverter void revertXYZ2Deg(const double& x, const double& y, double& lat_deg, double& lon_deg, const LLHParam& param); void revertXYZ2Rad(const double& x, const double& y, double& lat_rad, double& lon_rad, const LLHParam& param); - void convertMGRS2JPRCS(const double& m_x, const double& m_y, double& j_x, double& j_y, const LLHParam& param); - void convertJPRCS2MGRS(const double& j_x, const double& j_y, double& m_x, double& m_y, const LLHParam& param); + void convertMGRS2JPRCS(const double& m_x, const double& m_y, double& j_x, double& j_y, const std::string& mgrs_code, + const int jprcs_code); + void convertJPRCS2MGRS(const double& j_x, const double& j_y, double& m_x, double& m_y, const int jprcs_code); + void convertProj2Proj(const double& before_x, const double& before_y, const LLHParam& before_param, double& after_x, + double& after_y, const LLHParam& after_param); void getMapOriginDeg(double& lat_rad, double& lon_rad, const LLHParam& param); void getMapOriginRad(double& lat_rad, double& lon_rad, const LLHParam& param); @@ -100,23 +119,29 @@ class LLHConverter bool use_origin_zone_ = true; bool is_origin_set_ = false; - // Constant param - const double F_ = 298.257222101; - const double a_ = 6378137; - const double m0_ = 0.9999; + // Constant param for MGRS const int grid_code_size_ = 5; MGRSPrecision precision_ = MGRSPrecision::MICRO_METER_100; + // origins for JPRCS + std::vector jprcs_origin_lat_rads_; + std::vector jprcs_origin_lon_rads_; + TMParam jprcs_tm_param_; + // Object HeightConverter height_converter_; // Functions + // TransverseMercator + void convRad2TM(const double& lat_rad, const double& lon_rad, const TMParam& param, double& x, double& y); + void revTM2Rad(const double& x, const double& y, const TMParam& param, double& lat_rad, double& lon_rad); // Japan Plane Rectangular Coordinate System - void convRad2JPRCS(const double& lat_rad, const double& lon_rad, double& x, double& y); - void revJPRCS2Rad(const double& x, const double& y, double& lat_rad, double& lon_rad); + void initializeJPRCSOrigins(); + void convRad2JPRCS(const double& lat_rad, const double& lon_rad, const int grid_code, double& x, double& y); + void revJPRCS2Rad(const double& x, const double& y, const int grid_code, double& lat_rad, double& lon_rad); // MGRS void convRad2MGRS(const double& lat_rad, const double& lon_rad, double& x, double& y); - void revMGRS2Rad(const double& x, const double& y, double& lat_rad, double& lon_rad); + void revMGRS2Rad(const double& x, const double& y, std::string mgrs_code, double& lat_rad, double& lon_rad); std::pair getCrossNum(const double& x); std::string getOffsetZone(const std::string& zone, const int& offset); diff --git a/include/llh_converter/meridian_convergence_angle_correction.hpp b/include/llh_converter/meridian_convergence_angle_correction.hpp index 117d426..7c1a7cc 100644 --- a/include/llh_converter/meridian_convergence_angle_correction.hpp +++ b/include/llh_converter/meridian_convergence_angle_correction.hpp @@ -49,7 +49,7 @@ struct XYZ double z; }; -double getMeridianConvergence(const LLA &lla, const XYZ &xyz, LLHConverter &llhc, const LLHParam &llhc_param); +double getMeridianConvergence(const LLA& lla, const XYZ& xyz, LLHConverter& llhc, const LLHParam& llhc_param); double getMeridianConvergence(const LLA& lla, LLHConverter& llhc, const LLHParam& llhc_param); double getMeridianConvergence(const XYZ& xyz, LLHConverter& llhc, const LLHParam& llhc_param); } // namespace llh_converter diff --git a/src/gsigeo2024.cpp b/src/gsigeo2024.cpp index 1ee5ec4..733788e 100644 --- a/src/gsigeo2024.cpp +++ b/src/gsigeo2024.cpp @@ -39,113 +39,112 @@ #include namespace llh_converter { - -GSIGEO2024::GSIGEO2024() {} - -GSIGEO2024::~GSIGEO2024() {} - -void GSIGEO2024::loadGeoidMap(const std::string &geoid_file) +GSIGEO2024::GSIGEO2024() { - std::ifstream infile(geoid_file); - if (!infile) - { - std::cerr << "Error opening file: " << geoid_file << std::endl; - return; - } - - std::string line; - bool header_end = false; - while (std::getline(infile, line)) - { - if (line.find("end_of_head") != std::string::npos) - { - header_end = true; - break; - } - } - - if (!header_end) - { - std::cerr << "Header not properly formatted in file." << std::endl; - return; - } - - geoid_map_.resize(row_size_, std::vector(column_size_, -9999.0)); - - for (int i = 0; i < row_size_; ++i) - { - for (int j = 0; j < column_size_; ++j) - { - if (!(infile >> geoid_map_[i][j])) - { - std::cerr << "Error reading geoid data at row " << i << ", col " << j << std::endl; - return; - } - } - } - - is_geoid_loaded_ = true; } -double GSIGEO2024::getGeoid(const double &lat, const double &lon) +GSIGEO2024::~GSIGEO2024() { - if (!is_geoid_loaded_) - { - std::cerr << "Geoid map not loaded." << std::endl; - return std::numeric_limits::quiet_NaN(); - } +} - if (lat < 15.0 || lat > 50.0 || lon < 120.0 || lon > 160.0) +void GSIGEO2024::loadGeoidMap(const std::string& geoid_file) +{ + std::ifstream infile(geoid_file); + if (!infile) + { + std::cerr << "Error opening file: " << geoid_file << std::endl; + return; + } + + std::string line; + bool header_end = false; + while (std::getline(infile, line)) + { + if (line.find("end_of_head") != std::string::npos) { - std::cerr << "Input coordinates out of range." << std::endl; - return std::numeric_limits::quiet_NaN(); + header_end = true; + break; } + } - // Grid step (latitude 1 minute = 0.0166667 degrees, longitude 1.5 minutes = 0.025 degrees) - const double lat_step = 1.0 / 60.0; - const double lon_step = 1.5 / 60.0; - - // The index calculation from the reference point (latitude 50 degrees, longitude 120 degrees) - int i1 = static_cast((50.0 - lat) / lat_step); - int j1 = static_cast((lon - 120.0) / lon_step); - - int i2 = i1 + 1; - int j2 = j1 + 1; + if (!header_end) + { + std::cerr << "Header not properly formatted in file." << std::endl; + return; + } - if (i1 < 0 || i2 >= row_size_ || j1 < 0 || j2 >= column_size_) - { - return std::numeric_limits::quiet_NaN(); - } + geoid_map_.resize(row_size_, std::vector(column_size_, -9999.0)); - // Geoid height at the four corners - double f00 = geoid_map_[i1][j1]; - double f10 = geoid_map_[i1][j2]; - double f01 = geoid_map_[i2][j1]; - double f11 = geoid_map_[i2][j2]; - - if (f00 == -9999.0 || f10 == -9999.0 || f01 == -9999.0 || f11 == -9999.0) + for (int i = 0; i < row_size_; ++i) + { + for (int j = 0; j < column_size_; ++j) { - return std::numeric_limits::quiet_NaN(); + if (!(infile >> geoid_map_[i][j])) + { + std::cerr << "Error reading geoid data at row " << i << ", col " << j << std::endl; + return; + } } + } - // Coordinates of the four corners - double lat1 = 50.0 - i1 * lat_step; - double lat2 = lat1 - lat_step; - double lon1 = 120.0 + j1 * lon_step; - double lon2 = lon1 + lon_step; - - // Calculate interpolation coefficients correctly - double t = (lat1 - lat) / (lat1 - lat2); - double u = (lon - lon1) / (lon2 - lon1); - - // Bilinear interpolation - double interpolated_value = - (1 - t) * (1 - u) * f00 + - (1 - t) * u * f10 + - t * (1 - u) * f01 + - t * u * f11; + is_geoid_loaded_ = true; +} - return interpolated_value; +double GSIGEO2024::getGeoid(const double& lat, const double& lon) +{ + if (!is_geoid_loaded_) + { + std::cerr << "Geoid map not loaded." << std::endl; + return std::numeric_limits::quiet_NaN(); + } + + if (lat < 15.0 || lat > 50.0 || lon < 120.0 || lon > 160.0) + { + std::cerr << "Input coordinates out of range." << std::endl; + return std::numeric_limits::quiet_NaN(); + } + + // Grid step (latitude 1 minute = 0.0166667 degrees, longitude 1.5 minutes = 0.025 degrees) + const double lat_step = 1.0 / 60.0; + const double lon_step = 1.5 / 60.0; + + // The index calculation from the reference point (latitude 50 degrees, longitude 120 degrees) + int i1 = static_cast((50.0 - lat) / lat_step); + int j1 = static_cast((lon - 120.0) / lon_step); + + int i2 = i1 + 1; + int j2 = j1 + 1; + + if (i1 < 0 || i2 >= row_size_ || j1 < 0 || j2 >= column_size_) + { + return std::numeric_limits::quiet_NaN(); + } + + // Geoid height at the four corners + double f00 = geoid_map_[i1][j1]; + double f10 = geoid_map_[i1][j2]; + double f01 = geoid_map_[i2][j1]; + double f11 = geoid_map_[i2][j2]; + + if (f00 == -9999.0 || f10 == -9999.0 || f01 == -9999.0 || f11 == -9999.0) + { + return std::numeric_limits::quiet_NaN(); + } + + // Coordinates of the four corners + double lat1 = 50.0 - i1 * lat_step; + double lat2 = lat1 - lat_step; + double lon1 = 120.0 + j1 * lon_step; + double lon2 = lon1 + lon_step; + + // Calculate interpolation coefficients correctly + double t = (lat1 - lat) / (lat1 - lat2); + double u = (lon - lon1) / (lon2 - lon1); + + // Bilinear interpolation + double interpolated_value = (1 - t) * (1 - u) * f00 + (1 - t) * u * f10 + t * (1 - u) * f01 + t * u * f11; + + return interpolated_value; } -} // namespace llh_converter +} // namespace llh_converter diff --git a/src/llh_converter.cpp b/src/llh_converter.cpp index a489929..c60752b 100644 --- a/src/llh_converter.cpp +++ b/src/llh_converter.cpp @@ -40,10 +40,9 @@ namespace llh_converter { template -boost::bimaps::bimap -makeBimap(std::initializer_list::value_type> list) +boost::bimaps::bimap makeBimap(std::initializer_list::value_type> list) { - return boost::bimaps::bimap(list.begin(), list.end()); + return boost::bimaps::bimap(list.begin(), list.end()); } // Constructor @@ -51,6 +50,7 @@ LLHConverter::LLHConverter() { height_converter_.loadGSIGEOGeoidFile(); height_converter_.loadGSIGEO2024GeoidFile(); + initializeJPRCSOrigins(); initializeMGRSAlphabet(); } @@ -68,14 +68,30 @@ void LLHConverter::convertRad2XYZ(const double& lat_rad, const double& lon_rad, double& z, const LLHParam& param) { // Convert lat/lon to x/y - if (param.use_mgrs) + switch (param.projection_method) { - convRad2MGRS(lat_rad, lon_rad, x, y); - } - else - { - setPlaneNum(param.plane_num); - convRad2JPRCS(lat_rad, lon_rad, x, y); + case ProjectionMethod::TM: + { + convRad2TM(lat_rad, lon_rad, param.tm_param, x, y); + break; + } + case ProjectionMethod::JPRCS: + { + convRad2JPRCS(lat_rad, lon_rad, std::stoi(param.grid_code), x, y); + break; + } + case ProjectionMethod::MGRS: + { + convRad2MGRS(lat_rad, lon_rad, x, y); + break; + } + default: + { + std::cerr << "\033[31;1mError: Invalid projection method, " << static_cast(param.projection_method) + << "\033[0m" << std::endl; + std::cerr << "\033[31;1mError: LLHConverter::convertRad2XYZ()\033[0m" << std::endl; + exit(EXIT_FAILURE); + } } // Convert h to z @@ -95,40 +111,113 @@ void LLHConverter::revertXYZ2Deg(const double& x, const double& y, double& lat_d void LLHConverter::revertXYZ2Rad(const double& x, const double& y, double& lat_rad, double& lon_rad, const LLHParam& param) { - // Revert lat/lon to x/y - if (param.use_mgrs) - { - mgrs_code_ = param.mgrs_code; - revMGRS2Rad(x, y, lat_rad, lon_rad); - } - else + switch (param.projection_method) { - setPlaneNum(param.plane_num); - revJPRCS2Rad(x, y, lat_rad, lon_rad); + case ProjectionMethod::TM: + { + revTM2Rad(x, y, param.tm_param, lat_rad, lon_rad); + break; + } + case ProjectionMethod::JPRCS: + { + revJPRCS2Rad(x, y, std::stoi(param.grid_code), lat_rad, lon_rad); + break; + } + case ProjectionMethod::MGRS: + { + revMGRS2Rad(x, y, param.grid_code, lat_rad, lon_rad); + break; + } + default: + { + std::cerr << "\033[31;1mError: Invalid projection method, " << static_cast(param.projection_method) + << "\033[0m" << std::endl; + exit(EXIT_FAILURE); + } } } void LLHConverter::convertMGRS2JPRCS(const double& m_x, const double& m_y, double& j_x, double& j_y, - const LLHParam& param) + const std::string& mgrs_code, const int jprcs_code) { double lat_rad, lon_rad; - mgrs_code_ = param.mgrs_code; - revMGRS2Rad(m_x, m_y, lat_rad, lon_rad); + revMGRS2Rad(m_x, m_y, mgrs_code, lat_rad, lon_rad); - setPlaneNum(param.plane_num); - convRad2JPRCS(lat_rad, lon_rad, j_x, j_y); + convRad2JPRCS(lat_rad, lon_rad, jprcs_code, j_x, j_y); } void LLHConverter::convertJPRCS2MGRS(const double& j_x, const double& j_y, double& m_x, double& m_y, - const LLHParam& param) + const int jprcs_code) { double lat_rad, lon_rad; - setPlaneNum(param.plane_num); - revJPRCS2Rad(j_x, j_y, lat_rad, lon_rad); + revJPRCS2Rad(j_x, j_y, jprcs_code, lat_rad, lon_rad); convRad2MGRS(lat_rad, lon_rad, m_x, m_y); } +void LLHConverter::convertProj2Proj(const double& before_x, const double& before_y, const LLHParam& before_param, + double& after_x, double& after_y, const LLHParam& after_param) +{ + double lat_rad, lon_rad; + + if (before_param.projection_method == after_param.projection_method) + { + after_x = before_x; + after_y = before_y; + return; + } + + switch (before_param.projection_method) + { + case ProjectionMethod::TM: + { + revTM2Rad(before_x, before_y, before_param.tm_param, lat_rad, lon_rad); + break; + } + case ProjectionMethod::JPRCS: + { + revJPRCS2Rad(before_x, before_y, std::stoi(before_param.grid_code), lat_rad, lon_rad); + break; + } + case ProjectionMethod::MGRS: + { + revMGRS2Rad(before_x, before_y, before_param.grid_code, lat_rad, lon_rad); + break; + } + default: + { + std::cerr << "\033[31;1mError: Invalid projection method, " << static_cast(before_param.projection_method) + << "\033[0m" << std::endl; + exit(EXIT_FAILURE); + } + } + + switch (after_param.projection_method) + { + case ProjectionMethod::TM: + { + convRad2TM(lat_rad, lon_rad, after_param.tm_param, after_x, after_y); + break; + } + case ProjectionMethod::JPRCS: + { + convRad2JPRCS(lat_rad, lon_rad, std::stoi(after_param.grid_code), after_x, after_y); + break; + } + case ProjectionMethod::MGRS: + { + convRad2MGRS(lat_rad, lon_rad, after_x, after_y); + break; + } + default: + { + std::cerr << "\033[31;1mError: Invalid projection method, " << static_cast(after_param.projection_method) + << "\033[0m" << std::endl; + exit(EXIT_FAILURE); + } + } +} + void LLHConverter::getMapOriginDeg(double& lat_deg, double& lon_deg, const LLHParam& param) { double lat_rad = 0, lon_rad = 0; @@ -139,21 +228,36 @@ void LLHConverter::getMapOriginDeg(double& lat_deg, double& lon_deg, const LLHPa void LLHConverter::getMapOriginRad(double& lat_rad, double& lon_rad, const LLHParam& param) { - if (param.use_mgrs) - { - mgrs_code_ = param.mgrs_code; - revMGRS2Rad(0, 0, lat_rad, lon_rad); - } - else + switch (param.projection_method) { - setPlaneNum(param.plane_num); - lat_rad = plane_lat_rad_; - lon_rad = plane_lon_rad_; + case ProjectionMethod::TM: + { + lat_rad = param.tm_param.origin_lat_rad; + lon_rad = param.tm_param.origin_lon_rad; + break; + } + case ProjectionMethod::JPRCS: + { + lat_rad = jprcs_origin_lat_rads_[std::stoi(param.grid_code)]; + lon_rad = jprcs_origin_lon_rads_[std::stoi(param.grid_code)]; + break; + } + case ProjectionMethod::MGRS: + { + revMGRS2Rad(0, 0, param.grid_code, lat_rad, lon_rad); + break; + } + default: + { + std::cerr << "\033[31;1mError: Invalid projection method, " << static_cast(param.projection_method) + << "\033[0m" << std::endl; + exit(EXIT_FAILURE); + } } } // Private Member functions -void LLHConverter::convRad2JPRCS(const double& lat_rad, const double& lon_rad, double& x, double& y) +void LLHConverter::convRad2TM(const double& lat_rad, const double& lon_rad, const TMParam& param, double& x, double& y) { double PS, PSo, PDL, Pt, PN, PW; @@ -162,7 +266,7 @@ void LLHConverter::convRad2JPRCS(const double& lat_rad, const double& lon_rad, d double Pe, Pet, Pnn; double F_W; - F_W = 1.0 / F_; // Geometrical flattening + F_W = 1.0 / param.inv_flatten_ratio; // Geometrical flattening Pe = (double)sqrt(2.0 * F_W - pow(F_W, 2)); Pet = (double)sqrt(pow(Pe, 2) / (1.0 - pow(Pe, 2))); @@ -196,28 +300,30 @@ void LLHConverter::convRad2JPRCS(const double& lat_rad, const double& lon_rad, d PI = (double)765765.0 / 7516192768.0 * pow(Pe, 16); - PB1 = (double)a_ * (1.0 - pow(Pe, 2)) * PA; - PB2 = (double)a_ * (1.0 - pow(Pe, 2)) * PB / -2.0; - PB3 = (double)a_ * (1.0 - pow(Pe, 2)) * PC / 4.0; - PB4 = (double)a_ * (1.0 - pow(Pe, 2)) * PD / -6.0; - PB5 = (double)a_ * (1.0 - pow(Pe, 2)) * PE / 8.0; - PB6 = (double)a_ * (1.0 - pow(Pe, 2)) * PF_ / -10.0; - PB7 = (double)a_ * (1.0 - pow(Pe, 2)) * PG / 12.0; - PB8 = (double)a_ * (1.0 - pow(Pe, 2)) * PH / -14.0; - PB9 = (double)a_ * (1.0 - pow(Pe, 2)) * PI / 16.0; + PB1 = param.semi_major_axis * (1.0 - pow(Pe, 2)) * PA; + PB2 = param.semi_major_axis * (1.0 - pow(Pe, 2)) * PB / -2.0; + PB3 = param.semi_major_axis * (1.0 - pow(Pe, 2)) * PC / 4.0; + PB4 = param.semi_major_axis * (1.0 - pow(Pe, 2)) * PD / -6.0; + PB5 = param.semi_major_axis * (1.0 - pow(Pe, 2)) * PE / 8.0; + PB6 = param.semi_major_axis * (1.0 - pow(Pe, 2)) * PF_ / -10.0; + PB7 = param.semi_major_axis * (1.0 - pow(Pe, 2)) * PG / 12.0; + PB8 = param.semi_major_axis * (1.0 - pow(Pe, 2)) * PH / -14.0; + PB9 = param.semi_major_axis * (1.0 - pow(Pe, 2)) * PI / 16.0; PS = (double)PB1 * lat_rad + PB2 * sin(2.0 * lat_rad) + PB3 * sin(4.0 * lat_rad) + PB4 * sin(6.0 * lat_rad) + PB5 * sin(8.0 * lat_rad) + PB6 * sin(10.0 * lat_rad) + PB7 * sin(12.0 * lat_rad) + PB8 * sin(14.0 * lat_rad) + PB9 * sin(16.0 * lat_rad); - PSo = (double)PB1 * plane_lat_rad_ + PB2 * sin(2.0 * plane_lat_rad_) + PB3 * sin(4.0 * plane_lat_rad_) + - PB4 * sin(6.0 * plane_lat_rad_) + PB5 * sin(8.0 * plane_lat_rad_) + PB6 * sin(10.0 * plane_lat_rad_) + - PB7 * sin(12.0 * plane_lat_rad_) + PB8 * sin(14.0 * plane_lat_rad_) + PB9 * sin(16.0 * plane_lat_rad_); + PSo = (double)PB1 * param.origin_lat_rad + PB2 * sin(2.0 * param.origin_lat_rad) + + PB3 * sin(4.0 * param.origin_lat_rad) + PB4 * sin(6.0 * param.origin_lat_rad) + + PB5 * sin(8.0 * param.origin_lat_rad) + PB6 * sin(10.0 * param.origin_lat_rad) + + PB7 * sin(12.0 * param.origin_lat_rad) + PB8 * sin(14.0 * param.origin_lat_rad) + + PB9 * sin(16.0 * param.origin_lat_rad); - PDL = (double)lon_rad - plane_lon_rad_; + PDL = (double)lon_rad - param.origin_lon_rad; Pt = (double)tan(lat_rad); PW = (double)sqrt(1.0 - pow(Pe, 2) * pow(sin(lat_rad), 2)); - PN = (double)a_ / PW; + PN = param.semi_major_axis / PW; Pnn = (double)sqrt(pow(Pet, 2) * pow(cos(lat_rad), 2)); y = (double)((PS - PSo) + (1.0 / 2.0) * PN * pow(cos(lat_rad), 2.0) * Pt * pow(PDL, 2.0) + @@ -228,7 +334,7 @@ void LLHConverter::convRad2JPRCS(const double& lat_rad, const double& lon_rad, d pow(PDL, 6) - (1.0 / 40320.0) * PN * pow(cos(lat_rad), 8) * Pt * (-1385.0 + 3111 * pow(Pt, 2) - 543 * pow(Pt, 4) + pow(Pt, 6)) * pow(PDL, 8)) * - m0_; + param.scale_factor; x = (double)(PN * cos(lat_rad) * PDL - 1.0 / 6.0 * PN * pow(cos(lat_rad), 3) * (-1 + pow(Pt, 2) - pow(Pnn, 2)) * pow(PDL, 3) - @@ -237,12 +343,12 @@ void LLHConverter::convRad2JPRCS(const double& lat_rad, const double& lon_rad, d pow(PDL, 5) - 1.0 / 5040.0 * PN * pow(cos(lat_rad), 7) * (-61.0 + 479.0 * pow(Pt, 2) - 179.0 * pow(Pt, 4) + pow(Pt, 6)) * pow(PDL, 7)) * - m0_; + param.scale_factor; } -void LLHConverter::revJPRCS2Rad(const double& x, const double& y, double& lat_rad, double& lon_rad) +void LLHConverter::revTM2Rad(const double& x, const double& y, const TMParam& param, double& lat_rad, double& lon_rad) { - double n = 1. / (2 * F_ - 1); + double n = 1. / (2 * param.inv_flatten_ratio - 1); double n2 = n * n; double n3 = n2 * n; double n4 = n3 * n; @@ -269,12 +375,12 @@ void LLHConverter::revJPRCS2Rad(const double& x, const double& y, double& lat_ra double d5 = 4174 / 315. * n5 - 144838 / 6237. * n6; double d6 = 601676 / 22275. * n6; - double Sb = A0 * plane_lat_rad_ + A1 * std::sin(2 * plane_lat_rad_) + A2 * std::sin(4 * plane_lat_rad_) + - A3 * std::sin(6 * plane_lat_rad_) + A4 * std::sin(8 * plane_lat_rad_) + - A5 * std::sin(10 * plane_lat_rad_); - Sb = m0_ * a_ / (1 + n) * Sb; + double Sb = A0 * param.origin_lat_rad + A1 * std::sin(2 * param.origin_lat_rad) + + A2 * std::sin(4 * param.origin_lat_rad) + A3 * std::sin(6 * param.origin_lat_rad) + + A4 * std::sin(8 * param.origin_lat_rad) + A5 * std::sin(10 * param.origin_lat_rad); + Sb = param.scale_factor * param.semi_major_axis / (1 + n) * Sb; - double Ab = m0_ * a_ / (1 + n) * A0; + double Ab = param.scale_factor * param.semi_major_axis / (1 + n) * A0; double eps = (y + Sb) / Ab; double eta = x / Ab; @@ -290,7 +396,26 @@ void LLHConverter::revJPRCS2Rad(const double& x, const double& y, double& lat_ra lat_rad = X + d1 * std::sin(2 * X) + d2 * std::sin(4 * X) + d3 * std::sin(6 * X) + d4 * std::sin(8 * X) + d5 * std::sin(10 * X) + d6 * std::sin(12 * X); - lon_rad = plane_lon_rad_ + std::atan(std::sinh(eta1) / std::cos(eps1)); + lon_rad = param.origin_lon_rad + std::atan(std::sinh(eta1) / std::cos(eps1)); +} + +void LLHConverter::convRad2JPRCS(const double& lat_rad, const double& lon_rad, const int grid_code, double& x, + double& y) +{ + TMParam tm_param = jprcs_tm_param_; + tm_param.origin_lat_rad = jprcs_origin_lat_rads_[grid_code]; + tm_param.origin_lon_rad = jprcs_origin_lon_rads_[grid_code]; + + convRad2TM(lat_rad, lon_rad, tm_param, x, y); +} + +void LLHConverter::revJPRCS2Rad(const double& x, const double& y, const int grid_code, double& lat_rad, double& lon_rad) +{ + TMParam tm_param = jprcs_tm_param_; + tm_param.origin_lat_rad = jprcs_origin_lat_rads_[grid_code]; + tm_param.origin_lon_rad = jprcs_origin_lon_rads_[grid_code]; + + revTM2Rad(x, y, tm_param, lat_rad, lon_rad); } void LLHConverter::convRad2MGRS(const double& lat_rad, const double& lon_rad, double& x, double& y) @@ -351,22 +476,23 @@ void LLHConverter::convRad2MGRS(const double& lat_rad, const double& lon_rad, do } } -void LLHConverter::revMGRS2Rad(const double& x, const double& y, double& lat_rad, double& lon_rad) +void LLHConverter::revMGRS2Rad(const double& x, const double& y, std::string mgrs_code, double& lat_rad, + double& lon_rad) { auto [x_cross_num, x_in_grid] = getCrossNum(x); auto [y_cross_num, y_in_grid] = getCrossNum(y); if (x_cross_num != 0) { - std::string x_zone = mgrs_code_.substr(3, 1); + std::string x_zone = mgrs_code.substr(3, 1); x_zone = getOffsetZone(x_zone, x_cross_num); - mgrs_code_.replace(3, 1, x_zone); + mgrs_code.replace(3, 1, x_zone); } if (y_cross_num != 0) { - std::string y_zone = mgrs_code_.substr(4, 1); + std::string y_zone = mgrs_code.substr(4, 1); y_zone = getOffsetZone(y_zone, y_cross_num); - mgrs_code_.replace(4, 1, y_zone); + mgrs_code.replace(4, 1, y_zone); } std::ostringstream mgrs_x_code, mgrs_y_code; @@ -374,7 +500,7 @@ void LLHConverter::revMGRS2Rad(const double& x, const double& y, double& lat_rad mgrs_x_code << std::setw(digit_number) << std::setfill('0') << std::to_string(static_cast(x_in_grid * 100)); mgrs_y_code << std::setw(digit_number) << std::setfill('0') << std::to_string(static_cast(y_in_grid * 100)); - std::string mgrs_code = mgrs_code_ + mgrs_x_code.str() + mgrs_y_code.str(); + std::string whole_mgrs_code = mgrs_code + mgrs_x_code.str() + mgrs_y_code.str(); try { @@ -382,7 +508,7 @@ void LLHConverter::revMGRS2Rad(const double& x, const double& y, double& lat_rad bool northup; double utm_x, utm_y; double lat_deg, lon_deg; - GeographicLib::MGRS::Reverse(mgrs_code, zone, northup, utm_x, utm_y, prec); + GeographicLib::MGRS::Reverse(whole_mgrs_code, zone, northup, utm_x, utm_y, prec); GeographicLib::UTMUPS::Reverse(zone, northup, utm_x, utm_y, lat_deg, lon_deg); lat_rad = lat_deg * M_PI / 180.; lon_rad = lon_deg * M_PI / 180.; @@ -398,7 +524,7 @@ std::pair LLHConverter::getCrossNum(const double& x) { int cross_num = 0; double ret_x = x; - double grid_size = 100000; // 100km + double grid_size = 100000; // 100km while (ret_x >= grid_size) { ret_x -= grid_size; @@ -415,12 +541,15 @@ std::pair LLHConverter::getCrossNum(const double& x) std::string LLHConverter::getOffsetZone(const std::string& zone, const int& offset) { - if (offset == 0) return zone; + if (offset == 0) + return zone; int zone_num = mgrs_alphabet_.left.at(zone); zone_num += offset; - if (zone_num > 23) zone_num -= 24; - else if (zone_num < 0) zone_num += 24; + if (zone_num > 23) + zone_num -= 24; + else if (zone_num < 0) + zone_num += 24; return mgrs_alphabet_.right.at(zone_num); } @@ -471,157 +600,81 @@ int LLHConverter::checkCrossBoader(const std::string& code_origin, const std::st } } -void LLHConverter::setPlaneNum(int plane_num) +void LLHConverter::initializeJPRCSOrigins() { - // longitude and latitude of origin of each plane in Japan - int lon_deg, lon_min, lat_deg, lat_min; + // Initialize JPRCS origins + // Empty element at index 0 to align with JPRCS grid code which starts from 1 + jprcs_origin_lat_rads_.resize(20); + jprcs_origin_lon_rads_.resize(20); - if (plane_num == 1) - { - lat_deg = 33; - lat_min = 0; - lon_deg = 129; - lon_min = 30; - } - else if (plane_num == 2) - { - lat_deg = 33; - lat_min = 0; - lon_deg = 131; - lon_min = 0; - } - else if (plane_num == 3) - { - lat_deg = 36; - lat_min = 0; - lon_deg = 132; - lon_min = 10; - } - else if (plane_num == 4) - { - lat_deg = 33; - lat_min = 0; - lon_deg = 133; - lon_min = 30; - } - else if (plane_num == 5) - { - lat_deg = 36; - lat_min = 0; - lon_deg = 134; - lon_min = 20; - } - else if (plane_num == 6) - { - lat_deg = 36; - lat_min = 0; - lon_deg = 136; - lon_min = 0; - } - else if (plane_num == 7) - { - lat_deg = 36; - lat_min = 0; - lon_deg = 137; - lon_min = 10; - } - else if (plane_num == 8) - { - lat_deg = 36; - lat_min = 0; - lon_deg = 138; - lon_min = 30; - } - else if (plane_num == 9) - { - lat_deg = 36; - lat_min = 0; - lon_deg = 139; - lon_min = 50; - } - else if (plane_num == 10) - { - lat_deg = 40; - lat_min = 0; - lon_deg = 140; - lon_min = 50; - } - else if (plane_num == 11) - { - lat_deg = 44; - lat_min = 0; - lon_deg = 140; - lon_min = 15; - } - else if (plane_num == 12) - { - lat_deg = 44; - lat_min = 0; - lon_deg = 142; - lon_min = 15; - } - else if (plane_num == 13) - { - lat_deg = 44; - lat_min = 0; - lon_deg = 144; - lon_min = 15; - } - else if (plane_num == 14) - { - lat_deg = 26; - lat_min = 0; - lon_deg = 142; - lon_min = 0; - } - else if (plane_num == 15) - { - lat_deg = 26; - lat_min = 0; - lon_deg = 127; - lon_min = 30; - } - else if (plane_num == 16) - { - lat_deg = 26; - lat_min = 0; - lon_deg = 124; - lon_min = 0; - } - else if (plane_num == 17) - { - lat_deg = 26; - lat_min = 0; - lon_deg = 131; - lon_min = 0; - } - else if (plane_num == 18) - { - lat_deg = 20; - lat_min = 0; - lon_deg = 136; - lon_min = 0; - } - else if (plane_num == 19) - { - lat_deg = 26; - lat_min = 0; - lon_deg = 154; - lon_min = 0; - } + jprcs_origin_lat_rads_[1] = (33.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[1] = (129.0 + 30.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[2] = (33.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[2] = (131.0 + 0.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[3] = (36.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[3] = (132.0 + 10.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[4] = (33.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[4] = (133.0 + 30.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[5] = (36.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[5] = (134.0 + 20.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[6] = (36.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[6] = (136.0 + 0.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[7] = (36.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[7] = (137.0 + 10.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[8] = (36.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[8] = (138.0 + 30.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[9] = (36.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[9] = (139.0 + 50.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[10] = (40.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[10] = (140.0 + 50.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[11] = (44.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[11] = (140.0 + 15.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[12] = (44.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[12] = (142.0 + 15.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[13] = (44.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[13] = (140.0 + 15.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[14] = (26.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[14] = (142.0 + 0.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[15] = (26.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[15] = (127.0 + 30.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[16] = (26.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[16] = (124.0 + 0.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[17] = (26.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[17] = (131.0 + 0.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[18] = (20.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[18] = (136.0 + 0.0 / 60.0) / 180.0 * M_PI; + + jprcs_origin_lat_rads_[19] = (26.0 + 0.0 / 60.0) / 180.0 * M_PI; + jprcs_origin_lon_rads_[19] = (154.0 + 0.0 / 60.0) / 180.0 * M_PI; - // swap longitude and latitude - plane_lat_rad_ = M_PI * ((double)lat_deg + (double)lat_min / 60.0) / 180.0; - plane_lon_rad_ = M_PI * ((double)lon_deg + (double)lon_min / 60.0) / 180.0; + jprcs_tm_param_.inv_flatten_ratio = 298.257222101; + jprcs_tm_param_.semi_major_axis = 6378137; + jprcs_tm_param_.scale_factor = 0.9999; } void LLHConverter::initializeMGRSAlphabet() { - mgrs_alphabet_ = makeBimap({ { "A", 0 }, { "B", 1 }, { "C", 2 }, { "D", 3 }, { "E", 4 }, - { "F", 5 }, { "G", 6 }, { "H", 7 }, { "J", 8 }, { "K", 9 }, - { "L", 10 }, { "M", 11 }, { "N", 12 }, { "P", 13 }, { "Q", 14 }, - { "R", 15 }, { "S", 16 }, { "T", 17 }, { "U", 18 }, { "V", 19 }, - { "W", 20 }, { "X", 21 }, { "Y", 22 }, { "Z", 23 } }); + mgrs_alphabet_ = makeBimap( + { { "A", 0 }, { "B", 1 }, { "C", 2 }, { "D", 3 }, { "E", 4 }, { "F", 5 }, { "G", 6 }, { "H", 7 }, + { "J", 8 }, { "K", 9 }, { "L", 10 }, { "M", 11 }, { "N", 12 }, { "P", 13 }, { "Q", 14 }, { "R", 15 }, + { "S", 16 }, { "T", 17 }, { "U", 18 }, { "V", 19 }, { "W", 20 }, { "X", 21 }, { "Y", 22 }, { "Z", 23 } }); } double LLHConverter::getMeridianConvergenceRad(const double x, const double y, const LLHParam& param) diff --git a/src/meridian_convergence_angle_correction.cpp b/src/meridian_convergence_angle_correction.cpp index 9744ccd..deb992b 100644 --- a/src/meridian_convergence_angle_correction.cpp +++ b/src/meridian_convergence_angle_correction.cpp @@ -32,18 +32,18 @@ namespace llh_converter { -double getMeridianConvergence(const LLA &lla, const XYZ &xyz, LLHConverter &llhc, const LLHParam &llhc_param) +double getMeridianConvergence(const LLA& lla, const XYZ& xyz, LLHConverter& llhc, const LLHParam& llhc_param) { LLA offset_lla = lla; XYZ offset_by_cartesian = xyz; XYZ offset_by_geodetic; - offset_lla.latitude += 0.01; // neary 1.11km. This value has no special meaning. - offset_by_cartesian.y += 1000.0; // 1km. This value has no special meaning. + offset_lla.latitude += 0.01; // neary 1.11km. This value has no special meaning. + offset_by_cartesian.y += 1000.0; // 1km. This value has no special meaning. llhc.convertDeg2XYZ(offset_lla.latitude, offset_lla.longitude, offset_lla.altitude, offset_by_geodetic.x, - offset_by_geodetic.y, offset_by_geodetic.z, llhc_param); + offset_by_geodetic.y, offset_by_geodetic.z, llhc_param); double cartesian_diff_x = offset_by_cartesian.x - xyz.x; double cartesian_diff_y = offset_by_cartesian.y - xyz.y; diff --git a/test/height_converter_test.cpp b/test/height_converter_test.cpp index a99db34..c8639c5 100644 --- a/test/height_converter_test.cpp +++ b/test/height_converter_test.cpp @@ -48,6 +48,24 @@ void test(const double& result, const double& answer) } } +void test2(const double result0, const double result1, const double answer0, const double answer1) +{ + int i_result0 = std::round(result0 * 10000); + int i_result1 = std::round(result1 * 10000); + int i_answer0 = std::round(answer0 * 10000); + int i_answer1 = std::round(answer1 * 10000); + if (i_result0 == i_answer0 && i_result1 == i_answer1) + { + std::cout << "\033[32;1mTEST SUCCESS: " << result0 << ", " << result1 << " == " << answer0 << ", " << answer1 + << "\033[m" << std::endl; + } + else + { + std::cout << "\033[31;1mTEST FAILED : " << result0 << ", " << result1 << " != " << answer0 << ", " << answer1 + << "\033[m" << std::endl; + } +} + int main(int argc, char** argv) { llh_converter::HeightConverter hc; @@ -101,24 +119,36 @@ int main(int argc, char** argv) std::cout << "LLHConverter Test" << std::endl; llh_converter::LLHConverter llh_converter; llh_converter::LLHParam param; - param.use_mgrs = true; + param.projection_method = llh_converter::ProjectionMethod::MGRS; param.height_convert_type = llh_converter::ConvertType::NONE; param.geoid_type = llh_converter::GeoidType::EGM2008; double test_lat = 35.5, test_lon = 135.5; double x, y, z; - std::cout << "TEST MGRS" << std::endl; + std::cout << "Testing MGRS ... "; llh_converter.convertDeg2XYZ(test_lat, test_lon, 50, x, y, z, param); - std::cout << std::setprecision(15) << x << ", " << y << ", " << z << std::endl; + test2(x, y, 45346.7389, 28608.3575); // Test JPRCS - param.use_mgrs = false; - param.plane_num = 5; + param.projection_method = llh_converter::ProjectionMethod::JPRCS; + param.grid_code = std::to_string(5); + + std::cout << "Testing JPRCS ... "; + llh_converter.convertDeg2XYZ(test_lat, test_lon, 50, x, y, z, param); + test2(x, y, 105842.7741, -54845.8269); + + // Test TM + param.projection_method = llh_converter::ProjectionMethod::TM; + param.tm_param.inv_flatten_ratio = 298.257222101; + param.tm_param.semi_major_axis = 6378137.0; + param.tm_param.scale_factor = 0.9996; + param.tm_param.origin_lat_rad = 35.5 * M_PI / 180.; + param.tm_param.origin_lon_rad = 135.5 * M_PI / 180.; - std::cout << "TEST JPRCS" << std::endl; + std::cout << "Testing TM ... "; llh_converter.convertDeg2XYZ(test_lat, test_lon, 50, x, y, z, param); - std::cout << std::setprecision(15) << x << ", " << y << ", " << z << std::endl; + test2(x, y, 0.0, 0.0); return 0; } diff --git a/test/meridian_convergence_angle_correction_test.cpp b/test/meridian_convergence_angle_correction_test.cpp index e1138ca..f09d630 100644 --- a/test/meridian_convergence_angle_correction_test.cpp +++ b/test/meridian_convergence_angle_correction_test.cpp @@ -40,17 +40,20 @@ void test(const double& result, const double& answer) double diff_theres = 0.0001; if (diff < diff_theres) { - std::cout << "\033[32;1mTEST SUCCESS: |(" << result << ") - (" << answer << ")| = " << diff << "< " << diff_theres << "\033[m" << std::endl; + std::cout << "\033[32;1mTEST SUCCESS: |(" << result << ") - (" << answer << ")| = " << diff << "< " << diff_theres + << "\033[m" << std::endl; } else { - std::cout << "\033[31;1mTEST FAILED: |(" << result << ") - (" << answer << ")| = " << diff << ">= " << diff_theres << "\033[m" << std::endl; + std::cout << "\033[31;1mTEST FAILED: |(" << result << ") - (" << answer << ")| = " << diff << ">= " << diff_theres + << "\033[m" << std::endl; } } -void meridian_convergence_angle_correction_test(const double& test_lat, const double& test_lon, const double& answered_angle, - llh_converter::LLHConverter &llh_converter, const llh_converter::LLHParam ¶m -) +void meridian_convergence_angle_correction_test(const double& test_lat, const double& test_lon, + const double& answered_angle, + llh_converter::LLHConverter& llh_converter, + const llh_converter::LLHParam& param) { llh_converter::LLA lla; llh_converter::XYZ xyz; @@ -58,12 +61,12 @@ void meridian_convergence_angle_correction_test(const double& test_lat, const do lla.longitude = test_lon; lla.altitude = 30.0; llh_converter.convertDeg2XYZ(lla.latitude, lla.longitude, lla.altitude, xyz.x, xyz.y, xyz.z, param); - double mca = llh_converter::getMeridianConvergence(lla, xyz, llh_converter, param); // meridian convergence angle + double mca = llh_converter::getMeridianConvergence(lla, xyz, llh_converter, param); // meridian convergence angle std::cout << "-------------------------------------------------------------------------------------" << std::endl; - std::cout << "Testing LatLon (" << std::setw(6) << test_lat << ", " << std::setw(6) << test_lat << ") ... " << std::endl; + std::cout << "Testing LatLon (" << std::setw(6) << test_lat << ", " << std::setw(6) << test_lat << ") ... " + << std::endl; std::cout << "Calcalated Meridian Convergence Angle (" << mca << ")" << std::endl; test(mca * 180.0 / M_PI, answered_angle); - } int main(int argc, char** argv) @@ -71,8 +74,8 @@ int main(int argc, char** argv) // Meridian Convergence Angle Correction Test llh_converter::LLHConverter llh_converter; llh_converter::LLHParam param; - param.use_mgrs = false; - param.plane_num = 7; + param.projection_method = llh_converter::ProjectionMethod::JPRCS; + param.grid_code = std::to_string(7); param.height_convert_type = llh_converter::ConvertType::NONE; param.geoid_type = llh_converter::GeoidType::EGM2008; @@ -80,11 +83,11 @@ int main(int argc, char** argv) // https://vldb.gsi.go.jp/sokuchi/surveycalc/surveycalc/bl2xyf.html // nagoya city ueda double test_lat = 35.141168610, test_lon = 136.989591759; - double answered_angle = -0.101925000; // [deg] + double answered_angle = -0.101925000; // [deg] meridian_convergence_angle_correction_test(test_lat, test_lon, answered_angle, llh_converter, param); // nagoya city ozone double test_lat2 = 35.188843433, test_lon2 = 136.943096063; - double answered_angle2 = -0.128838889; // [deg] + double answered_angle2 = -0.128838889; // [deg] meridian_convergence_angle_correction_test(test_lat2, test_lon2, answered_angle2, llh_converter, param); return 0;