From cc0470302f68fd1282655478bbe1692a76e5add7 Mon Sep 17 00:00:00 2001 From: AlexBeattie42 <30098201+alexbeattie42@users.noreply.github.com> Date: Fri, 18 Oct 2024 20:23:48 +0300 Subject: [PATCH] Minimum stod change --- OpenSim/Common/APDMDataReader.cpp | 23 ++++++++-------- OpenSim/Common/CMakeLists.txt | 6 ++-- OpenSim/Common/DelimFileAdapter.h | 32 +++++++++++----------- OpenSim/Common/IO.cpp | 27 ++++++++++++++++-- OpenSim/Common/IO.h | 5 ++++ OpenSim/Common/TRCFileAdapter.cpp | 8 +++--- OpenSim/Common/Test/testSTOFileAdapter.cpp | 5 ++-- OpenSim/Common/Test/testTRCFileAdapter.cpp | 4 +-- OpenSim/Common/XsensDataReader.cpp | 16 +++++------ OpenSim/Sandbox/ImuStreaming.cpp | 14 +++++----- OpenSim/Simulation/MarkersReference.cpp | 2 +- dependencies/CMakeLists.txt | 9 ++++++ 12 files changed, 96 insertions(+), 55 deletions(-) diff --git a/OpenSim/Common/APDMDataReader.cpp b/OpenSim/Common/APDMDataReader.cpp index d10eef0e03..73e60cbf4b 100644 --- a/OpenSim/Common/APDMDataReader.cpp +++ b/OpenSim/Common/APDMDataReader.cpp @@ -4,6 +4,7 @@ #include "FileAdapter.h" #include "TimeSeriesTable.h" #include "APDMDataReader.h" +#include "IO.h" namespace OpenSim { @@ -101,7 +102,7 @@ APDMDataReader::extendRead(const std::string& fileName) const { // Line 2 std::getline(in_stream, line); tokens = FileAdapter::tokenize(line, ","); - dataRate = std::stod(tokens[1]); + dataRate = OpenSim::IO::stod(tokens[1]); // Line 3, find columns for IMUs std::getline(in_stream, line); tokens = FileAdapter::tokenize(line, ","); @@ -155,20 +156,20 @@ APDMDataReader::extendRead(const std::string& fileName) const { for (int imu_index = 0; imu_index < n_imus; ++imu_index) { // parse gyro info from in_stream if (foundLinearAccelerationData) - accel_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[accIndex[imu_index]]), - std::stod(nextRow[accIndex[imu_index] + 1]), std::stod(nextRow[accIndex[imu_index] + 2])); + accel_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[accIndex[imu_index]]), + OpenSim::IO::stod(nextRow[accIndex[imu_index] + 1]), OpenSim::IO::stod(nextRow[accIndex[imu_index] + 2])); if (foundMagneticHeadingData) - magneto_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[magIndex[imu_index]]), - std::stod(nextRow[magIndex[imu_index] + 1]), std::stod(nextRow[magIndex[imu_index] + 2])); + magneto_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[magIndex[imu_index]]), + OpenSim::IO::stod(nextRow[magIndex[imu_index] + 1]), OpenSim::IO::stod(nextRow[magIndex[imu_index] + 2])); if (foundAngularVelocityData) - gyro_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[gyroIndex[imu_index]]), - std::stod(nextRow[gyroIndex[imu_index] + 1]), std::stod(nextRow[gyroIndex[imu_index] + 2])); + gyro_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[gyroIndex[imu_index]]), + OpenSim::IO::stod(nextRow[gyroIndex[imu_index] + 1]), OpenSim::IO::stod(nextRow[gyroIndex[imu_index] + 2])); // Create Quaternion from values in file, assume order in file W, X, Y, Z orientation_row_vector[imu_index] = - SimTK::Quaternion(std::stod(nextRow[orientationsIndex[imu_index]]), - std::stod(nextRow[orientationsIndex[imu_index] + 1]), - std::stod(nextRow[orientationsIndex[imu_index] + 2]), - std::stod(nextRow[orientationsIndex[imu_index] + 3])); + SimTK::Quaternion(OpenSim::IO::stod(nextRow[orientationsIndex[imu_index]]), + OpenSim::IO::stod(nextRow[orientationsIndex[imu_index] + 1]), + OpenSim::IO::stod(nextRow[orientationsIndex[imu_index] + 2]), + OpenSim::IO::stod(nextRow[orientationsIndex[imu_index] + 3])); } // append to the tables times[rowNumber] = time; diff --git a/OpenSim/Common/CMakeLists.txt b/OpenSim/Common/CMakeLists.txt index afabfd39de..3f46cb70b9 100644 --- a/OpenSim/Common/CMakeLists.txt +++ b/OpenSim/Common/CMakeLists.txt @@ -12,12 +12,14 @@ if (NOT WITH_EZC3D) unset(ezc3d_LIBRARY) endif() +find_package(FastFloat REQUIRED + HINTS "${OPENSIM_DEPENDENCIES_DIR}/fast_float") OpenSimAddLibrary( KIT Common AUTHORS "Clay_Anderson-Ayman_Habib-Peter_Loan" # Clients of osimCommon need not link to ezc3d. - LINKLIBS PUBLIC ${Simbody_LIBRARIES} spdlog::spdlog osimLepton - PRIVATE ${ezc3d_LIBRARY} + LINKLIBS PUBLIC ${Simbody_LIBRARIES} spdlog::spdlog osimLepton + PRIVATE ${ezc3d_LIBRARY} FastFloat::fast_float INCLUDES ${INCLUDES} SOURCES ${SOURCES} TESTDIRS "Test" diff --git a/OpenSim/Common/DelimFileAdapter.h b/OpenSim/Common/DelimFileAdapter.h index 1224ef2034..9414060f9d 100644 --- a/OpenSim/Common/DelimFileAdapter.h +++ b/OpenSim/Common/DelimFileAdapter.h @@ -437,7 +437,7 @@ DelimFileAdapter::extendRead(const std::string& fileName) const { } // Time is column 0. - timeVec.push_back(std::stod(row.front())); + timeVec.push_back(OpenSim::IO::stod(row.front())); row.erase(row.begin()); auto row_vector = readElems(row); @@ -482,7 +482,7 @@ DelimFileAdapter::readElems_impl(const std::vector& tokens, double) const { SimTK::RowVector_ elems{static_cast(tokens.size())}; for(auto i = 0u; i < tokens.size(); ++i) - elems[static_cast(i)] = std::stod(tokens[i]); + elems[static_cast(i)] = OpenSim::IO::stod(tokens[i]); return elems; } @@ -497,9 +497,9 @@ DelimFileAdapter::readElems_impl(const std::vector& tokens, OPENSIM_THROW_IF(comps.size() != 3, IncorrectNumTokens, "Expected 3x (multiple of 3) number of tokens."); - elems[i] = SimTK::UnitVec3{std::stod(comps[0]), - std::stod(comps[1]), - std::stod(comps[2])}; + elems[i] = SimTK::UnitVec3{OpenSim::IO::stod(comps[0]), + OpenSim::IO::stod(comps[1]), + OpenSim::IO::stod(comps[2])}; } return elems; @@ -515,10 +515,10 @@ DelimFileAdapter::readElems_impl(const std::vector& tokens, OPENSIM_THROW_IF(comps.size() != 4, IncorrectNumTokens, "Expected 4x (multiple of 4) number of tokens."); - elems[i] = SimTK::Quaternion{std::stod(comps[0]), - std::stod(comps[1]), - std::stod(comps[2]), - std::stod(comps[3])}; + elems[i] = SimTK::Quaternion{OpenSim::IO::stod(comps[0]), + OpenSim::IO::stod(comps[1]), + OpenSim::IO::stod(comps[2]), + OpenSim::IO::stod(comps[3])}; } return elems; @@ -534,12 +534,12 @@ DelimFileAdapter::readElems_impl(const std::vector& tokens, OPENSIM_THROW_IF(comps.size() != 6, IncorrectNumTokens, "Expected 6x (multiple of 6) number of tokens."); - elems[i] = SimTK::SpatialVec{{std::stod(comps[0]), - std::stod(comps[1]), - std::stod(comps[2])}, - {std::stod(comps[3]), - std::stod(comps[4]), - std::stod(comps[5])}}; + elems[i] = SimTK::SpatialVec{{OpenSim::IO::stod(comps[0]), + OpenSim::IO::stod(comps[1]), + OpenSim::IO::stod(comps[2])}, + {OpenSim::IO::stod(comps[3]), + OpenSim::IO::stod(comps[4]), + OpenSim::IO::stod(comps[5])}}; } return elems; @@ -559,7 +559,7 @@ DelimFileAdapter::readElems_impl(const std::vector& tokens, "x (multiple of " + std::to_string(M) + ") number of tokens."); for(int j = 0; j < M; ++j) { - elems[i][j] = std::stod(comps[j]); + elems[i][j] = OpenSim::IO::stod(comps[j]); } } diff --git a/OpenSim/Common/IO.cpp b/OpenSim/Common/IO.cpp index 66828b49b1..56dd6f7815 100644 --- a/OpenSim/Common/IO.cpp +++ b/OpenSim/Common/IO.cpp @@ -51,6 +51,9 @@ #include #endif +#include "fast_float/fast_float.h" +// #include + // CONSTANTS @@ -64,6 +67,8 @@ int IO::_Pad = 8; int IO::_Precision = 8; char IO::_DoubleFormat[] = "%16.8lf"; bool IO::_PrintOfflineDocuments = true; +std::string IO::_locale = std::locale::classic().name(); + //============================================================================= @@ -450,10 +455,14 @@ OpenFile(const string &aFileName,const string &aMode) /** * Open a file. */ -ifstream *IO:: +std::ifstream *IO:: OpenInputFile(const string &aFileName,ios_base::openmode mode) { ifstream *fs = new ifstream(aFileName.c_str(), ios_base::in | mode); + // std::ifstream *fs = new std::ifstream(); + log_info(_locale); + // fs->imbue(std::locale(_locale)); + // fs->open(aFileName.c_str(), ios_base::in | mode); if(!fs || !(*fs)) { log_error("IO.OpenInputFile(const string&,openmode mode): " "failed to open {}.", aFileName); @@ -462,10 +471,13 @@ OpenInputFile(const string &aFileName,ios_base::openmode mode) return(fs); } -ofstream *IO:: +std::ofstream *IO:: OpenOutputFile(const string &aFileName,ios_base::openmode mode) { ofstream *fs = new ofstream(aFileName.c_str(), ios_base::out | mode); + // std::ofstream *fs = new std::ofstream(); + // fs->imbue(std::locale(_locale)); + // fs->open(aFileName.c_str(), ios_base::out | mode); if(!fs || !(*fs)) { log_error("IO.OpenOutputFile(const string&,openmode mode): failed to " "open {}.", aFileName); @@ -474,6 +486,17 @@ OpenOutputFile(const string &aFileName,ios_base::openmode mode) return(fs); } + +double IO:: +stod(const std::string& __str, std::size_t* __idx) +{ + double result; + // std::istringstream iss(__str); + // iss.imbue(std::locale(_locale)); + // iss >> result; + fast_float::from_chars(__str.data(), __str.data()+__str.size(), result); + return result; +} //_____________________________________________________________________________ /** * Create a directory. Potentially platform dependent. diff --git a/OpenSim/Common/IO.h b/OpenSim/Common/IO.h index 2b403b959d..2109159bfd 100644 --- a/OpenSim/Common/IO.h +++ b/OpenSim/Common/IO.h @@ -66,6 +66,8 @@ class OSIMCOMMON_API IO { static char _DoubleFormat[IO_DBLFMTLEN]; /** Whether offline documents should also be printed when Object::print is called. */ static bool _PrintOfflineDocuments; + /** Locale specifier for reading and writing files */ + static std::string _locale; //============================================================================= @@ -103,7 +105,10 @@ class OSIMCOMMON_API IO { static FILE* OpenFile(const std::string &aFileName,const std::string &aMode); static std::ifstream* OpenInputFile(const std::string &aFileName,std::ios_base::openmode mode=std::ios_base::in); static std::ofstream* OpenOutputFile(const std::string &aFileName,std::ios_base::openmode mode=std::ios_base::out); + static double stod(const std::string& __str, std::size_t* __idx = 0); #endif + + // // Directory management static int makeDir(const std::string &aDirName); static int chDir(const std::string &aDirName); diff --git a/OpenSim/Common/TRCFileAdapter.cpp b/OpenSim/Common/TRCFileAdapter.cpp index 39747fc43b..54ba7a8bd6 100644 --- a/OpenSim/Common/TRCFileAdapter.cpp +++ b/OpenSim/Common/TRCFileAdapter.cpp @@ -193,15 +193,15 @@ TRCFileAdapter::extendRead(const std::string& fileName) const { //only if each component is specified read process as a Vec3 if ( !(row.at(c).empty() || row.at(c + 1).empty() || row.at(c + 2).empty()) ) { - row_vector[ind] = SimTK::Vec3{ std::stod(row.at(c)), - std::stod(row.at(c + 1)), - std::stod(row.at(c + 2)) }; + row_vector[ind] = SimTK::Vec3{ OpenSim::IO::stod(row.at(c)), + OpenSim::IO::stod(row.at(c + 1)), + OpenSim::IO::stod(row.at(c + 2)) }; } // otherwise the value will remain NaN (default) ++ind; } markerData[rowNumber] = row_vector; // Column 1 is time. - times[rowNumber] = std::stod(row.at(1)); + times[rowNumber] = OpenSim::IO::stod(row.at(1)); rowNumber++; if (rowNumber== last_size) { // resize all Data/Matrices, double the size while keeping data diff --git a/OpenSim/Common/Test/testSTOFileAdapter.cpp b/OpenSim/Common/Test/testSTOFileAdapter.cpp index 600058d1cc..6f3cd746d2 100644 --- a/OpenSim/Common/Test/testSTOFileAdapter.cpp +++ b/OpenSim/Common/Test/testSTOFileAdapter.cpp @@ -22,6 +22,7 @@ #include "OpenSim/Common/Adapters.h" #include "OpenSim/Common/CommonUtilities.h" +#include "OpenSim/Common/IO.h" #include #include #include @@ -140,8 +141,8 @@ void compareFiles(const std::string& filenameA, double d_tokenA{}; double d_tokenB{}; try { - d_tokenA = std::stod(tokenA); - d_tokenB = std::stod(tokenB); + d_tokenA = OpenSim::IO::stod(tokenA); + d_tokenB = OpenSim::IO::stod(tokenB); } catch(std::invalid_argument&) { testFailed(filenameA, tokenA, tokenB); } diff --git a/OpenSim/Common/Test/testTRCFileAdapter.cpp b/OpenSim/Common/Test/testTRCFileAdapter.cpp index 555d4c7acd..5512f8d917 100644 --- a/OpenSim/Common/Test/testTRCFileAdapter.cpp +++ b/OpenSim/Common/Test/testTRCFileAdapter.cpp @@ -107,8 +107,8 @@ void compareFiles(const std::string& filenameA, double d_tokenA{}; double d_tokenB{}; try { - d_tokenA = std::stod(tokenA); - d_tokenB = std::stod(tokenB); + d_tokenA = OpenSim::IO::stod(tokenA); + d_tokenB = OpenSim::IO::stod(tokenB); } catch (std::invalid_argument&) { testFailed(filenameA, tokenA, tokenB); diff --git a/OpenSim/Common/XsensDataReader.cpp b/OpenSim/Common/XsensDataReader.cpp index 5d5e1c9896..3287430eba 100644 --- a/OpenSim/Common/XsensDataReader.cpp +++ b/OpenSim/Common/XsensDataReader.cpp @@ -82,7 +82,7 @@ XsensDataReader::extendRead(const std::string& folderName) const { std::map::iterator it = headersKeyValuePairs.find("Update Rate"); if (it != headersKeyValuePairs.end()) - dataRate = std::stod(it->second); + dataRate = OpenSim::IO::stod(it->second); else dataRate = 40.0; // Need confirmation from XSens as later files don't specify rate // internally keep track of what data was found in input files @@ -122,20 +122,20 @@ XsensDataReader::extendRead(const std::string& folderName) const { break; } if (foundLinearAccelerationData) - accel_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[accIndex]), - std::stod(nextRow[accIndex + 1]), std::stod(nextRow[accIndex + 2])); + accel_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[accIndex]), + OpenSim::IO::stod(nextRow[accIndex + 1]), OpenSim::IO::stod(nextRow[accIndex + 2])); if (foundMagneticHeadingData) - magneto_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[magIndex]), - std::stod(nextRow[magIndex + 1]), std::stod(nextRow[magIndex + 2])); + magneto_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[magIndex]), + OpenSim::IO::stod(nextRow[magIndex + 1]), OpenSim::IO::stod(nextRow[magIndex + 2])); if (foundAngularVelocityData) - gyro_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[gyroIndex]), - std::stod(nextRow[gyroIndex + 1]), std::stod(nextRow[gyroIndex + 2])); + gyro_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[gyroIndex]), + OpenSim::IO::stod(nextRow[gyroIndex + 1]), OpenSim::IO::stod(nextRow[gyroIndex + 2])); // Create Mat33 then convert into Quaternion SimTK::Mat33 imu_matrix{ SimTK::NaN }; int matrix_entry_index = 0; for (int mcol = 0; mcol < 3; mcol++) { for (int mrow = 0; mrow < 3; mrow++) { - imu_matrix[mrow][mcol] = std::stod(nextRow[rotationsIndex + matrix_entry_index]); + imu_matrix[mrow][mcol] = OpenSim::IO::stod(nextRow[rotationsIndex + matrix_entry_index]); matrix_entry_index++; } } diff --git a/OpenSim/Sandbox/ImuStreaming.cpp b/OpenSim/Sandbox/ImuStreaming.cpp index 210ccc9cb2..21e8ad19f4 100644 --- a/OpenSim/Sandbox/ImuStreaming.cpp +++ b/OpenSim/Sandbox/ImuStreaming.cpp @@ -142,22 +142,22 @@ parseImuData(const char* str, const size_t len) { std::smatch result_ts{}; if(!std::regex_search(data, result_ts, regex_ts)) throw std::runtime_error{"No timestamp in data stream."}; - double timestamp{std::stod(result_ts[0])}; + double timestamp{OpenSim::IO::stod(result_ts[0])}; // Acceleration. std::array gravity{}; std::smatch result_gravity{}; if(std::regex_search(data, result_gravity, regex_gravity)) { - gravity[0] = std::stod(result_gravity[1]); - gravity[1] = std::stod(result_gravity[2]); - gravity[2] = std::stod(result_gravity[3]); + gravity[0] = OpenSim::IO::stod(result_gravity[1]); + gravity[1] = OpenSim::IO::stod(result_gravity[2]); + gravity[2] = OpenSim::IO::stod(result_gravity[3]); } // Angular veclocity. std::array omega{}; std::smatch result_omega{}; if(std::regex_search(data, result_omega, regex_omega)) { - omega[0] = std::stod(result_omega[1]); - omega[1] = std::stod(result_omega[2]); - omega[2] = std::stod(result_omega[3]); + omega[0] = OpenSim::IO::stod(result_omega[1]); + omega[1] = OpenSim::IO::stod(result_omega[2]); + omega[2] = OpenSim::IO::stod(result_omega[3]); } return std::make_tuple(timestamp, gravity, omega); diff --git a/OpenSim/Simulation/MarkersReference.cpp b/OpenSim/Simulation/MarkersReference.cpp index c36326cb52..53df0784a4 100644 --- a/OpenSim/Simulation/MarkersReference.cpp +++ b/OpenSim/Simulation/MarkersReference.cpp @@ -228,7 +228,7 @@ double MarkersReference::getSamplingFrequency() const { if(_markerTable.hasTableMetaDataKey("DataRate")) { auto datarate = _markerTable.getTableMetaData("DataRate"); - return std::stod(datarate); + return IO::stod(datarate); } else return SimTK::NaN; } diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index c89eec0314..404768132b 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -216,6 +216,15 @@ AddDependency(NAME catch2 GIT_URL https://github.com/catchorg/Catch2.git GIT_TAG v3.5.0) + +AddDependency(NAME fast_float + DEFAULT ON + GIT_URL https://github.com/fastfloat/fast_float.git + GIT_TAG tags/v6.1.6 + GIT_SHALLOW TRUE) + +mark_as_advanced(SUPERBUILD_fast_float) + # Moco settings. # -------------- option(OPENSIM_WITH_CASADI