Skip to content

Commit

Permalink
Merge pull request #3943 from gateway240/main
Browse files Browse the repository at this point in the history 
Improved String to Decimal parsing
  • Loading branch information
@adamkewley
adamkewley authored Oct 23, 2024
2 parents edbc2e4 + 4ee0e57 commit 33f3f59
Show file tree
Hide file tree
Showing 10 changed files with 72 additions and 51 deletions.
23 changes: 12 additions & 11 deletions OpenSim/Common/APDMDataReader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include "FileAdapter.h"
#include "TimeSeriesTable.h"
#include "APDMDataReader.h"
#include "IO.h"

namespace OpenSim {

Expand Down Expand Up @@ -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, ",");
Expand Down Expand Up @@ -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;
Expand Down
32 changes: 16 additions & 16 deletions OpenSim/Common/DelimFileAdapter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -437,7 +437,7 @@ DelimFileAdapter<T>::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);
Expand Down Expand Up @@ -482,7 +482,7 @@ DelimFileAdapter<T>::readElems_impl(const std::vector<std::string>& tokens,
double) const {
SimTK::RowVector_<double> elems{static_cast<int>(tokens.size())};
for(auto i = 0u; i < tokens.size(); ++i)
elems[static_cast<int>(i)] = std::stod(tokens[i]);
elems[static_cast<int>(i)] = OpenSim::IO::stod(tokens[i]);

return elems;
}
Expand All @@ -497,9 +497,9 @@ DelimFileAdapter<T>::readElems_impl(const std::vector<std::string>& 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;
Expand All @@ -515,10 +515,10 @@ DelimFileAdapter<T>::readElems_impl(const std::vector<std::string>& 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;
Expand All @@ -534,12 +534,12 @@ DelimFileAdapter<T>::readElems_impl(const std::vector<std::string>& 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;
Expand All @@ -559,7 +559,7 @@ DelimFileAdapter<T>::readElems_impl(const std::vector<std::string>& 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]);
}
}

Expand Down
17 changes: 17 additions & 0 deletions OpenSim/Common/IO.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,10 @@

#include "Logger.h"
#include <climits>
#include <limits>
#include <math.h>
#include <string>
#include <sstream>
#include <time.h>
#if defined(__linux__) || defined(__APPLE__)
#include <sys/stat.h>
Expand Down Expand Up @@ -64,6 +66,7 @@ int IO::_Pad = 8;
int IO::_Precision = 8;
char IO::_DoubleFormat[] = "%16.8lf";
bool IO::_PrintOfflineDocuments = true;
const std::locale _locale = std::locale::classic();


//=============================================================================
Expand Down Expand Up @@ -474,6 +477,20 @@ OpenOutputFile(const string &aFileName,ios_base::openmode mode)

return(fs);
}

double IO::
stod(const std::string& __str, std::size_t* __idx)
{
std::istringstream iss(__str);
iss.imbue(_locale);
double result;
iss >> result;
if(iss.fail()){
result = std::numeric_limits<double>::quiet_NaN();
log_warn("Encountered non-numeric string value: {} ; parsed value:{}",__str, result);
}
return result;
}
//_____________________________________________________________________________
/**
* Create a directory. Potentially platform dependent.
Expand Down
1 change: 1 addition & 0 deletions OpenSim/Common/IO.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,7 @@ 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);
Expand Down
8 changes: 4 additions & 4 deletions OpenSim/Common/TRCFileAdapter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
5 changes: 3 additions & 2 deletions OpenSim/Common/Test/testSTOFileAdapter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@

#include "OpenSim/Common/Adapters.h"
#include "OpenSim/Common/CommonUtilities.h"
#include "OpenSim/Common/IO.h"
#include <cstdio>
#include <fstream>
#include <unordered_set>
Expand Down Expand Up @@ -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);
}
Expand Down
4 changes: 2 additions & 2 deletions OpenSim/Common/Test/testTRCFileAdapter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
16 changes: 8 additions & 8 deletions OpenSim/Common/XsensDataReader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ XsensDataReader::extendRead(const std::string& folderName) const {
std::map<std::string, std::string>::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
Expand Down Expand Up @@ -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++;
}
}
Expand Down
14 changes: 7 additions & 7 deletions OpenSim/Sandbox/ImuStreaming.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double, 3> 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<double, 3> 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);
Expand Down
3 changes: 2 additions & 1 deletion OpenSim/Simulation/MarkersReference.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
* -------------------------------------------------------------------------- */

#include "MarkersReference.h"
#include "OpenSim/Common/IO.h"
#include <SimTKcommon/internal/State.h>
#include <cmath>

Expand Down Expand Up @@ -228,7 +229,7 @@ double
MarkersReference::getSamplingFrequency() const {
if(_markerTable.hasTableMetaDataKey("DataRate")) {
auto datarate = _markerTable.getTableMetaData<std::string>("DataRate");
return std::stod(datarate);
return OpenSim::IO::stod(datarate);
} else
return SimTK::NaN;
}
Expand Down

0 comments on commit 33f3f59

Please sign in to comment.