Skip to content

Commit

Permalink
Update for minimal solution with stod
Browse files Browse the repository at this point in the history
  • Loading branch information
@alexbeattie42
alexbeattie42 committed Oct 18, 2024
2 parents 41d8dd2 + cc04703 commit b40c9f9
Show file tree
Hide file tree
Showing 5 changed files with 66 additions and 45 deletions.
22 changes: 11 additions & 11 deletions OpenSim/Common/APDMDataReader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ APDMDataReader::extendRead(const std::string& fileName) const {
// Line 2
std::getline(*in_stream, line);
tokens = FileAdapter::tokenize(line, ",");
dataRate = IO::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 @@ -156,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(IO::stod(nextRow[accIndex[imu_index]]),
IO::stod(nextRow[accIndex[imu_index] + 1]), IO::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(IO::stod(nextRow[magIndex[imu_index]]),
IO::stod(nextRow[magIndex[imu_index] + 1]), IO::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(IO::stod(nextRow[gyroIndex[imu_index]]),
IO::stod(nextRow[gyroIndex[imu_index] + 1]), IO::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(IO::stod(nextRow[orientationsIndex[imu_index]]),
IO::stod(nextRow[orientationsIndex[imu_index] + 1]),
IO::stod(nextRow[orientationsIndex[imu_index] + 2]),
IO::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 @@ -438,7 +438,7 @@ DelimFileAdapter<T>::extendRead(const std::string& fileName) const {

double result = -1;
// Time is column 0.
timeVec.push_back(IO::stod(row.front()));
timeVec.push_back(OpenSim::IO::stod(row.front()));
row.erase(row.begin());

auto row_vector = readElems(row);
Expand Down Expand Up @@ -483,7 +483,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)] = IO::stod(tokens[i]);
elems[static_cast<int>(i)] = OpenSim::IO::stod(tokens[i]);

return elems;
}
Expand All @@ -498,9 +498,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{IO::stod(comps[0]),
IO::stod(comps[1]),
IO::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 @@ -516,10 +516,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{IO::stod(comps[0]),
IO::stod(comps[1]),
IO::stod(comps[2]),
IO::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 @@ -535,12 +535,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{{IO::stod(comps[0]),
IO::stod(comps[1]),
IO::stod(comps[2])},
{IO::stod(comps[3]),
IO::stod(comps[4]),
IO::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 @@ -560,7 +560,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] = IO::stod(comps[j]);
elems[i][j] = OpenSim::IO::stod(comps[j]);
}
}

Expand Down
33 changes: 27 additions & 6 deletions OpenSim/Common/IO.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -432,17 +432,37 @@ bool IO::FileExists(const std::string& filePath) {
return std::ifstream(filePath).good();
}

//_____________________________________________________________________________
/**
* Open a file.
*/
FILE* IO::
OpenFile(const string &aFileName,const string &aMode)
{
FILE *fp = NULL;

// OPEN THE FILE
fp = fopen(aFileName.c_str(),aMode.c_str());
if(fp==NULL) {
log_error("IO.OpenFile(const string&,const string&): "
"failed to open {}.", aFileName);
return(NULL);
}

return(fp);
}
//_____________________________________________________________________________
/**
* Open a file.
*/
std::ifstream *IO::
OpenInputFile(const string &aFileName,ios_base::openmode mode)
{
std::ifstream *fs = new std::ifstream();
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);
// 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);
Expand All @@ -454,9 +474,10 @@ OpenInputFile(const string &aFileName,ios_base::openmode mode)
std::ofstream *IO::
OpenOutputFile(const string &aFileName,ios_base::openmode mode)
{
std::ofstream *fs = new std::ofstream();
fs->imbue(std::locale(_locale));
fs->open(aFileName.c_str(), ios_base::out | 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);
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 @@ -195,15 +195,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{ IO::stod(row.at(c)),
IO::stod(row.at(c + 1)),
IO::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] = IO::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
16 changes: 8 additions & 8 deletions OpenSim/Common/XsensDataReader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,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 = IO::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(IO::stod(nextRow[accIndex]),
IO::stod(nextRow[accIndex + 1]), IO::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(IO::stod(nextRow[magIndex]),
IO::stod(nextRow[magIndex + 1]), IO::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(IO::stod(nextRow[gyroIndex]),
IO::stod(nextRow[gyroIndex + 1]), IO::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] = IO::stod(nextRow[rotationsIndex + matrix_entry_index]);
imu_matrix[mrow][mcol] = OpenSim::IO::stod(nextRow[rotationsIndex + matrix_entry_index]);
matrix_entry_index++;
}
}
Expand Down

0 comments on commit b40c9f9

Please sign in to comment.