TimeSeriesTableVec3 convertRotationsToEulerAngles( const TimeSeriesTable_<SimTK::Rotation>& rotTable) { auto labels = rotTable.getColumnLabels(); auto& times = rotTable.getIndependentColumn(); const auto& rotations = rotTable.getMatrix(); int nc = int(labels.size()); int nt = int(times.size()); SimTK::Matrix_<SimTK::Vec3> eulerMatrix(nt, nc, SimTK::Vec3(SimTK::NaN)); for (int i = 0; i < nt; ++i) { for (int j = 0; j < nc; ++j) { eulerMatrix.updElt(i, j) = rotations(i, j).convertRotationToBodyFixedXYZ(); } } TimeSeriesTableVec3 eulerData{ times, eulerMatrix, labels }; eulerData.updTableMetaData() .setValueForKey("Units", std::string("Radians")); return eulerData; }
void testOrientationsReference() { // column labels for orientation sensor data vector<std::string> labels{ "A", "B", "C", "D", "E", "F" }; // for testing construct a set of marker weights in a different order vector<int> order = { 3, 5, 1, 4, 0, 2 }; size_t nc = labels.size(); // number of columns of orientation data size_t nr = 5; // number of rows of orientation data TimeSeriesTable_<SimTK::Rotation> orientationData; orientationData.setColumnLabels(labels); for (size_t r{ 0 }; r < nr; ++r) { SimTK::RowVector_<SimTK::Rotation> row{ int(nc), SimTK::Rotation() }; orientationData.appendRow(0.1*r, row); } Set<OrientationWeight> orientationWeights; for (size_t m{ 0 }; m < nc; ++m) orientationWeights.adoptAndAppend( new OrientationWeight(labels[order[m]], double(order[m]))); std::cout << orientationWeights.dump() << std::endl; OrientationsReference orientationsRef(orientationData, &orientationWeights); Model model; SimTK::State& s = model.initSystem(); s.updTime() = 0.0; SimTK::Array_<string> names = orientationsRef.getNames(); SimTK::Array_<double> weights; orientationsRef.getWeights(s, weights); SimTK_ASSERT_ALWAYS(names.size() == weights.size(), "Number of markers does not match number of weights."); for (unsigned int i{ 0 }; i < names.size(); ++i) { std::cout << names[i] << ": " << weights[i] << std::endl; SimTK_ASSERT_ALWAYS(weights[i] == double(i), "Mismatched weight to marker."); } // Add marker weights for markers not present in the data orientationWeights.adoptAndAppend(new OrientationWeight("X", 0.1)); orientationWeights.insert(0, new OrientationWeight("Y", 0.01)); OrientationsReference orientationsRef2(orientationData, &orientationWeights); auto& oWeightSet = orientationsRef2.get_orientation_weights(); // verify that internal weight set was updated std::cout << oWeightSet.dump() << std::endl; names = orientationsRef2.getNames(); orientationsRef2.getWeights(s, weights); SimTK_ASSERT_ALWAYS(names.size() == weights.size(), "Number of orientation sensors does not match number of weights."); for (unsigned int i = 0; i < names.size(); ++i) { std::cout << names[i] << ": " << weights[i] << std::endl; SimTK_ASSERT_ALWAYS(weights[i] == double(i), "Mismatched weight to orientation sensor."); } }
int main() { using namespace SimTK; using namespace OpenSim; using OpenSim::Exception; // Default construct, add metadata to columns, append rows one at a time. ValueArray<std::string> labels{}; for(unsigned i = 1; i <= 5; ++i) labels.upd().push_back(SimTK::Value<std::string>{std::to_string(i)}); ValueArray<unsigned> col_index{}; for(unsigned i = 1; i <= 5; ++i) col_index.upd().push_back(SimTK::Value<unsigned>{i}); DataTable::DependentsMetaData dep_metadata{}; dep_metadata.setValueArrayForKey("labels", labels); dep_metadata.setValueArrayForKey("column-index", col_index); DataTable::IndependentMetaData ind_metadata{}; ind_metadata.setValueForKey("labels", std::string{"0"}); ind_metadata.setValueForKey("column-index", unsigned{0}); TimeSeriesTable table{}; { ASSERT(!table.hasColumnLabels()); table.setColumnLabels({"0", "1", "2", "3"}); ASSERT(table.hasColumnLabels()); ASSERT(table.hasColumn("1")); ASSERT(table.hasColumn("2")); ASSERT(!table.hasColumn("column-does-not-exist")); table.setColumnLabel(0, "zero"); table.setColumnLabel(2, "two"); ASSERT(table.getColumnLabel(0) == "zero"); ASSERT(table.getColumnLabel(2) == "two"); table.setColumnLabel(0, "0"); table.setColumnLabel(2, "2"); const auto& labels = table.getColumnLabels(); for(size_t i = 0; i < labels.size(); ++i) if(labels.at(i) != std::to_string(i)) throw Exception{"Test failed: labels.at(i) != " "std::to_string(i)"}; for(size_t i = 0; i < labels.size(); ++i) if(table.getColumnIndex(labels.at(i)) != i) throw Exception{"Test failed: " "table.getColumnIndex(labels.at(i)) != i"}; } // Print out the DataTable to console. try { std::cout << table << std::endl; throw Exception{"Test failed: Exception expected."}; } catch(const OpenSim::EmptyTable&) {} table.setDependentsMetaData(dep_metadata); table.setIndependentMetaData(ind_metadata); SimTK::RowVector_<double> row{5, double{0}}; for(unsigned i = 0; i < 5; ++i) table.appendRow(0.00 + 0.25 * i, row + i); for(unsigned i = 0; i < 5; ++i) table.updRowAtIndex(i) += 1; for(unsigned i = 0; i < 5; ++i) table.updRow(0 + 0.25 * i) -= 1; try { table.appendRow(0.5, row); } catch (OpenSim::Exception&) {} const auto& avgRow = table.averageRow(0.2, 0.8); for(int i = 0; i < avgRow.ncol(); ++i) OPENSIM_THROW_IF(std::abs(avgRow[i] - 2) > 1e-8/*epsilon*/, Exception, "Test failed: averageRow() failed."); const auto& nearRow = table.getNearestRow(0.55); for(int i = 0; i < nearRow.ncol(); ++i) ASSERT(nearRow[i] == 2); table.updNearestRow(0.55) += 2; table.updNearestRow(0.55) -= 2; for(int i = 0; i < nearRow.ncol(); ++i) ASSERT(nearRow[i] == 2); table.updMatrix() += 2; table.updMatrixBlock(0, 0, table.getNumRows(), table.getNumColumns()) -= 2; table.updTableMetaData().setValueForKey("DataRate", 600); table.updTableMetaData().setValueForKey("Filename", std::string{"/path/to/file"}); ASSERT(table.hasColumn(0)); ASSERT(table.hasColumn(2)); ASSERT(!table.hasColumn(100)); // Print out the DataTable to console. std::cout << table << std::endl; std::cout << table.toString({}, {"1", "4"}); std::cout << table.toString({-1, -2}, {"1", "4"}); // Retrieve added metadata and rows to check. if(table.getNumRows() != unsigned{5}) throw Exception{"Test Failed: table.getNumRows() != unsigned{5}"}; if(table.getNumColumns() != unsigned{5}) throw Exception{"Test Failed: table.getNumColumns() != unsigned{5}"}; const auto& dep_metadata_ref = table.getDependentsMetaData(); const auto& labels_ref = dep_metadata_ref.getValueArrayForKey("labels"); for(unsigned i = 0; i < 5; ++i) if(labels_ref[i].getValue<std::string>() != std::to_string(i + 1)) throw Exception{"Test failed: labels_ref[i].getValue<std::string>()" " != std::to_string(i + 1)"}; { const auto& labels = table.getColumnLabels(); for(unsigned i = 0; i < 5; ++i) if(labels.at(i) != std::to_string(i + 1)) throw Exception{"Test failed: labels[i].getValue<std::string>()" " != std::to_string(i + 1)"}; } const auto& col_index_ref = dep_metadata_ref.getValueArrayForKey("column-index"); for(unsigned i = 0; i < 5; ++i) if(col_index_ref[i].getValue<unsigned>() != i + 1) throw Exception{"Test failed: col_index_ref[i].getValue<unsigned>()" " != i + 1"}; const auto& ind_metadata_ref = table.getIndependentMetaData(); if(ind_metadata_ref.getValueForKey("labels").getValue<std::string>() != std::string{"0"}) throw Exception{"Test failed: ind_metadata_ref.getValueForKey" "(\"labels\").getValue<std::string>() != std::string{\"0\"}"}; if(ind_metadata_ref.getValueForKey("column-index").getValue<unsigned>() != unsigned{0}) throw Exception{"Test failed: ind_metadata_ref.getValueForKey" "(\"column-index\").getValue<unsigned>() != unsigned{0}"}; table.updDependentColumnAtIndex(0) += 2; table.updDependentColumnAtIndex(2) += 2; table.updDependentColumn("1") -= 2; table.updDependentColumn("3") -= 2; for(unsigned i = 0; i < 5; ++i) { for(unsigned j = 0; j < 5; ++j) { const auto row_i_1 = table.getRowAtIndex(i); if(row_i_1[j] != (row + i)[j]) throw Exception{"Test failed: row_i_1[j] != (row + i)[j]"}; const auto row_i_2 = table.getRow(0 + 0.25 * i); if(row_i_2[j] != (row + i)[j]) throw Exception{"Test failed: row_i_2[j] != (row + i)[j]"}; const auto col_i = table.getDependentColumnAtIndex(i); if(col_i[j] != j) throw Exception{"Test failed: table.getDependentColumnAtIndex" "(i)[j] != j"}; } } const auto& tab_metadata_ref = table.getTableMetaData(); if(tab_metadata_ref.getValueForKey("DataRate").getValue<int>() != 600) throw Exception{"Test failed: tab_metadata_ref.getValueForKey" "(\"DataRate\").getValue<int>() != 600"}; if(tab_metadata_ref.getValueForKey("Filename").getValue<std::string>() != std::string{"/path/to/file"}) throw Exception{"Test failed: tab_metadata_ref.getValueForKey" "(\"Filename\").getValue<std::string>() != std::string" "{\"/path/to/file\"}"}; { std::cout << "Test feeding rows of one table to another [double]." << std::endl; TimeSeriesTable tableCopy{}; tableCopy.setColumnLabels(table.getColumnLabels()); for(unsigned row = 0; row < table.getNumRows(); ++row) tableCopy.appendRow(table.getIndependentColumn()[row], table.getRowAtIndex(row)); ASSERT(tableCopy.getNumColumns() == table.getNumColumns()); ASSERT(tableCopy.getNumRows() == table.getNumRows()); for(unsigned r = 0; r < table.getNumRows(); ++r) for(unsigned c = 0; c < table.getNumColumns(); ++c) ASSERT(tableCopy.getRowAtIndex(r)[c] == table.getRowAtIndex(r)[c]); } std::cout << "Test numComponentsPerElement()." << std::endl; ASSERT((static_cast<AbstractDataTable&&> (DataTable_<double, double >{})). numComponentsPerElement() == 1); ASSERT((static_cast<AbstractDataTable&&> (DataTable_<double, Vec3 >{})). numComponentsPerElement() == 3); ASSERT((static_cast<AbstractDataTable&&> (DataTable_<double, UnitVec3 >{})). numComponentsPerElement() == 3); ASSERT((static_cast<AbstractDataTable&&> (DataTable_<double, Quaternion>{})). numComponentsPerElement() == 4); ASSERT((static_cast<AbstractDataTable&&> (DataTable_<double, SpatialVec>{})). numComponentsPerElement() == 6); { std::cout << "Test DataTable flattening constructor for Vec3." << std::endl; DataTable_<double, Vec3> tableVec3{}; tableVec3.setColumnLabels({"col0", "col1", "col2"}); tableVec3.appendRow(0.1, {{1, 1, 1}, {2, 2, 2}, {3, 3, 3}}); tableVec3.appendRow(0.2, {{3, 3, 3}, {1, 1, 1}, {2, 2, 2}}); tableVec3.appendRow(0.3, {{2, 2, 2}, {3, 3, 3}, {1, 1, 1}}); std::cout << tableVec3 << std::endl; DataTable_<double, double> tableDouble{tableVec3}; std::vector<std::string> expLabels{"col0_1", "col0_2", "col0_3", "col1_1", "col1_2", "col1_3", "col2_1", "col2_2", "col2_3"}; ASSERT(tableDouble.getColumnLabels() == expLabels); ASSERT(tableDouble.getNumRows() == 3); ASSERT(tableDouble.getNumColumns() == 9); { const auto& row0 = tableDouble.getRowAtIndex(0); const auto& row1 = tableDouble.getRowAtIndex(1); const auto& row2 = tableDouble.getRowAtIndex(2); ASSERT(row0[0] == 1); ASSERT(row1[0] == 3); ASSERT(row2[0] == 2); ASSERT(row0[8] == 3); ASSERT(row1[8] == 2); ASSERT(row2[8] == 1); } { std::cout << "Test feeding rows of one table to another [Vec3]." << std::endl; DataTableVec3 tableVec3Copy{}; tableVec3Copy.setColumnLabels(tableVec3.getColumnLabels()); for(unsigned row = 0; row < tableVec3.getNumRows(); ++row) tableVec3Copy.appendRow(tableVec3.getIndependentColumn()[row], tableVec3.getRowAtIndex(row)); ASSERT(tableVec3Copy.getNumColumns() == tableVec3.getNumColumns()); ASSERT(tableVec3Copy.getNumRows() == tableVec3.getNumRows()); for(unsigned r = 0; r < tableVec3.getNumRows(); ++r) { for(unsigned c = 0; c < tableVec3.getNumColumns(); ++c) { ASSERT(tableVec3Copy.getRowAtIndex(r)[c][0] == tableVec3.getRowAtIndex(r)[c][0]); ASSERT(tableVec3Copy.getRowAtIndex(r)[c][1] == tableVec3.getRowAtIndex(r)[c][1]); ASSERT(tableVec3Copy.getRowAtIndex(r)[c][2] == tableVec3.getRowAtIndex(r)[c][2]); } } } std::cout << "Test DataTable flatten() for Vec3." << std::endl; auto tableFlat = tableVec3.flatten({"_x", "_y", "_z"}); expLabels = {"col0_x", "col0_y", "col0_z", "col1_x", "col1_y", "col1_z", "col2_x", "col2_y", "col2_z"}; ASSERT(tableFlat.getColumnLabels() == expLabels); ASSERT(tableFlat.getNumRows() == 3); ASSERT(tableFlat.getNumColumns() == 9); { const auto& row0 = tableFlat.getRowAtIndex(0); const auto& row1 = tableFlat.getRowAtIndex(1); const auto& row2 = tableFlat.getRowAtIndex(2); ASSERT(row0[0] == 1); ASSERT(row1[0] == 3); ASSERT(row2[0] == 2); ASSERT(row0[8] == 3); ASSERT(row1[8] == 2); ASSERT(row2[8] == 1); } std::cout << tableFlat << std::endl; std::cout << "Test DataTable flattening constructor for Quaternion." << std::endl; DataTable_<double, Quaternion> tableQuat{}; tableQuat.setColumnLabels({"col0", "col1", "col2"}); tableQuat.appendRow(0.1, {{1, 1, 1, 1}, {2, 2, 2, 2}, {3, 3, 3, 3}}); tableQuat.appendRow(0.2, {{3, 3, 3, 3}, {1, 1, 1, 1}, {2, 2, 2, 2}}); tableQuat.appendRow(0.3, {{2, 2, 2, 2}, {3, 3, 3, 3}, {1, 1, 1, 1}}); std::cout << tableQuat << std::endl; tableDouble = tableQuat; ASSERT(tableDouble.getColumnLabels().size() == 12); ASSERT(tableDouble.getNumRows() == 3); ASSERT(tableDouble.getNumColumns() == 12); std::cout << "Test DataTable flattening constructor for UnitVec3." << std::endl; DataTable_<double, Vec3> tableUnitVec3{}; tableUnitVec3.setColumnLabels({"col0", "col1", "col2"}); tableUnitVec3.appendRow(0.1, {{1, 1, 1}, {2, 2, 2}, {3, 3, 3}}); tableUnitVec3.appendRow(0.2, {{3, 3, 3}, {1, 1, 1}, {2, 2, 2}}); tableUnitVec3.appendRow(0.3, {{2, 2, 2}, {3, 3, 3}, {1, 1, 1}}); std::cout << tableUnitVec3 << std::endl; tableDouble = tableUnitVec3; ASSERT(tableDouble.getColumnLabels().size() == 9); ASSERT(tableDouble.getNumRows() == 3); ASSERT(tableDouble.getNumColumns() == 9); std::cout << "Test DataTable flattening constructor for SpatialVec." << std::endl; DataTable_<double, SpatialVec> tableSpatialVec{}; tableSpatialVec.setColumnLabels({"col0", "col1", "col2"}); tableSpatialVec.appendRow(0.1, {{{1, 1, 1}, {1, 1, 1}}, {{2, 2, 2}, {2, 2, 2}}, {{3, 3, 3}, {3, 3, 3}}}); tableSpatialVec.appendRow(0.2, {{{3, 3, 3}, {3, 3, 3}}, {{1, 1, 1}, {1, 1, 1}}, {{2, 2, 2}, {2, 2, 2}}}); tableSpatialVec.appendRow(0.3, {{{2, 2, 2}, {2, 2, 2}}, {{3, 3, 3}, {3, 3, 3}}, {{1, 1, 1}, {1, 1, 1}}}); std::cout << tableSpatialVec << std::endl; tableDouble = tableSpatialVec; ASSERT(tableDouble.getColumnLabels().size() == 18); ASSERT(tableDouble.getNumRows() == 3); ASSERT(tableDouble.getNumColumns() == 18); std::cout << tableDouble << std::endl; } { std::cout << "Test TimeSeriesTable flattening constructor for Vec3" << std::endl; TimeSeriesTable_<Vec3> tableVec3{}; tableVec3.setColumnLabels({"col0", "col1", "col2"}); tableVec3.appendRow(0.1, {{1, 1, 1}, {2, 2, 2}, {3, 3, 3}}); tableVec3.appendRow(0.2, {{3, 3, 3}, {1, 1, 1}, {2, 2, 2}}); tableVec3.appendRow(0.3, {{2, 2, 2}, {3, 3, 3}, {1, 1, 1}}); const auto& avgRowVec3 = tableVec3.averageRow(0.1, 0.2); for(size_t i = 0; i < 3; ++i) OPENSIM_THROW_IF(std::abs(avgRowVec3[0][i] - 2) > 1e-8/*epsilon*/, Exception, "Test failed: averageRow() failed."); const auto& nearRowVec3 = tableVec3.getNearestRow(0.29); for(size_t i = 0; i < 3; ++i) ASSERT(nearRowVec3[0][i] == 2); tableVec3.updNearestRow(0.29) += SimTK::Vec3{2}; tableVec3.updNearestRow(0.29) -= SimTK::Vec3{2}; for(size_t i = 0; i < 3; ++i) ASSERT(nearRowVec3[0][i] == 2); std::cout << tableVec3 << std::endl; TimeSeriesTable_<double> tableDouble{tableVec3}; std::vector<std::string> expLabels{"col0_1", "col0_2", "col0_3", "col1_1", "col1_2", "col1_3", "col2_1", "col2_2", "col2_3"}; ASSERT(tableDouble.getColumnLabels() == expLabels); ASSERT(tableDouble.getNumRows() == 3); ASSERT(tableDouble.getNumColumns() == 9); { const auto& row0 = tableDouble.getRowAtIndex(0); const auto& row1 = tableDouble.getRowAtIndex(1); const auto& row2 = tableDouble.getRowAtIndex(2); ASSERT(row0[0] == 1); ASSERT(row1[0] == 3); ASSERT(row2[0] == 2); ASSERT(row0[8] == 3); ASSERT(row1[8] == 2); ASSERT(row2[8] == 1); } std::cout << tableDouble << std::endl; std::cout << "Test TimeSeriesTable flatten() for Vec3." << std::endl; auto tableFlat = tableVec3.flatten({"_x", "_y", "_z"}); expLabels = {"col0_x", "col0_y", "col0_z", "col1_x", "col1_y", "col1_z", "col2_x", "col2_y", "col2_z"}; ASSERT(tableFlat.getColumnLabels() == expLabels); ASSERT(tableFlat.getNumRows() == 3); ASSERT(tableFlat.getNumColumns() == 9); { const auto& row0 = tableFlat.getRowAtIndex(0); const auto& row1 = tableFlat.getRowAtIndex(1); const auto& row2 = tableFlat.getRowAtIndex(2); ASSERT(row0[0] == 1); ASSERT(row1[0] == 3); ASSERT(row2[0] == 2); ASSERT(row0[8] == 3); ASSERT(row1[8] == 2); ASSERT(row2[8] == 1); } std::cout << tableFlat << std::endl; std::cout << "Test TimeSeriesTable flattening constructor for " "Quaternion" << std::endl; TimeSeriesTable_<Quaternion> tableQuat{}; tableQuat.setColumnLabels({"col0", "col1", "col2"}); tableQuat.appendRow(0.1, {{1, 1, 1, 1}, {2, 2, 2, 2}, {3, 3, 3, 3}}); tableQuat.appendRow(0.2, {{3, 3, 3, 3}, {1, 1, 1, 1}, {2, 2, 2, 2}}); tableQuat.appendRow(0.3, {{2, 2, 2, 2}, {3, 3, 3, 3}, {1, 1, 1, 1}}); const auto& avgRowQuat = tableQuat.averageRow(0.1, 0.2); for(size_t i = 0; i < 4; ++i) { OPENSIM_THROW_IF(std::abs(avgRowQuat[0][i] - 0.5) > 1e-8/*epsilon*/, Exception, "Test failed: averageRow() failed."); } const auto& nearRowQuat = tableQuat.getNearestRow(0.29); for(size_t i = 0; i < 4; ++i) ASSERT(std::abs(nearRowQuat[0][i] - 0.5) < 1e-8/*eps*/); std::cout << tableQuat << std::endl; tableDouble = tableQuat; ASSERT(tableDouble.getColumnLabels().size() == 12); ASSERT(tableDouble.getNumRows() == 3); ASSERT(tableDouble.getNumColumns() == 12); std::cout << tableDouble << std::endl; std::cout << "Test TimeSeriesTable flattening constructor for UnitVec3" << std::endl; TimeSeriesTable_<Vec3> tableUnitVec3{}; tableUnitVec3.setColumnLabels({"col0", "col1", "col2"}); tableUnitVec3.appendRow(0.1, {{1, 1, 1}, {2, 2, 2}, {3, 3, 3}}); tableUnitVec3.appendRow(0.2, {{3, 3, 3}, {1, 1, 1}, {2, 2, 2}}); tableUnitVec3.appendRow(0.3, {{2, 2, 2}, {3, 3, 3}, {1, 1, 1}}); std::cout << tableUnitVec3 << std::endl; tableDouble = tableUnitVec3; ASSERT(tableDouble.getColumnLabels().size() == 9); ASSERT(tableDouble.getNumRows() == 3); ASSERT(tableDouble.getNumColumns() == 9); std::cout << tableDouble << std::endl; std::cout << "Test TimeSeriesTable flattening constructor for " "SpatialVec" << std::endl; TimeSeriesTable_<SpatialVec> tableSpatialVec{}; tableSpatialVec.setColumnLabels({"col0", "col1", "col2"}); tableSpatialVec.appendRow(0.1, {{{1, 1, 1}, {1, 1, 1}}, {{2, 2, 2}, {2, 2, 2}}, {{3, 3, 3}, {3, 3, 3}}}); tableSpatialVec.appendRow(0.2, {{{3, 3, 3}, {3, 3, 3}}, {{1, 1, 1}, {1, 1, 1}}, {{2, 2, 2}, {2, 2, 2}}}); tableSpatialVec.appendRow(0.3, {{{2, 2, 2}, {2, 2, 2}}, {{3, 3, 3}, {3, 3, 3}}, {{1, 1, 1}, {1, 1, 1}}}); const auto& avgRowSVec = tableSpatialVec.averageRow(0.1, 0.2); for(size_t i = 0; i < 3; ++i) { OPENSIM_THROW_IF(std::abs(avgRowSVec[0][0][i] - 2) > 1e-8/*eps*/, Exception, "Test failed: averageRow() failed."); } const auto& nearRowSVec = tableSpatialVec.getNearestRow(0.29); for(size_t i = 0; i < 3; ++i) ASSERT(nearRowSVec[0][0][i] == 2); tableSpatialVec.updNearestRow(0.29) += SimTK::SpatialVec{{2, 2, 2}, {2, 2, 2}}; tableSpatialVec.updNearestRow(0.29) -= SimTK::SpatialVec{{2, 2, 2}, {2, 2, 2}}; for(size_t i = 0; i < 3; ++i) ASSERT(nearRowSVec[0][0][i] == 2); std::cout << tableSpatialVec << std::endl; tableDouble = tableSpatialVec; ASSERT(tableDouble.getColumnLabels().size() == 18); ASSERT(tableDouble.getNumRows() == 3); ASSERT(tableDouble.getNumColumns() == 18); std::cout << tableDouble << std::endl; } { std::cout << "Test DataTable packing." << std::endl; DataTable_<double, double> tableDouble{}; tableDouble.setColumnLabels({"col0_x", "col0_y", "col0_z", "col1_x", "col1_y", "col1_z", "col2_x", "col2_y", "col2_z", "col3_x", "col3_y", "col3_z"}); tableDouble.appendRow(1, {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); tableDouble.appendRow(2, {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}); tableDouble.appendRow(3, {3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3}); tableDouble.addTableMetaData("string", std::string{"string"}); tableDouble.addTableMetaData("int", 10); ASSERT(tableDouble.getColumnLabels().size() == 12); ASSERT(tableDouble.getNumRows() == 3); ASSERT(tableDouble.getNumColumns() == 12); std::cout << tableDouble << std::endl; std::cout << "Test DataTable packing for Vec3 with suffix specified." << std::endl; auto tableVec3_1 = tableDouble.pack<SimTK::Vec3>({"_x", "_y", "_z"}); std::vector<std::string> expLabels{"col0", "col1", "col2", "col3"}; ASSERT(tableVec3_1.getColumnLabels() == expLabels); ASSERT(tableVec3_1.getNumRows() == 3); ASSERT(tableVec3_1.getNumColumns() == 4); ASSERT(tableVec3_1.getTableMetaData<std::string>("string") == "string"); ASSERT(tableVec3_1.getTableMetaData<int>("int") == 10); std::cout << tableVec3_1 << std::endl; std::cout << "Test DataTable packing for Vec3 with suffix unspecified." << std::endl; auto tableVec3_2 = tableDouble.pack<SimTK::Vec3>(); ASSERT(tableVec3_2.getColumnLabels() == expLabels); ASSERT(tableVec3_2.getNumRows() == 3); ASSERT(tableVec3_2.getNumColumns() == 4); ASSERT(tableVec3_2.getTableMetaData<std::string>("string") == "string"); ASSERT(tableVec3_2.getTableMetaData<int>("int") == 10); std::cout << tableVec3_2 << std::endl; std::cout << "Test DataTable packing for UnitVec3." << std::endl; auto tableUVec3 = tableDouble.pack<SimTK::UnitVec3>(); ASSERT(tableUVec3.getColumnLabels() == expLabels); ASSERT(tableUVec3.getNumRows() == 3); ASSERT(tableUVec3.getNumColumns() == 4); ASSERT(tableUVec3.getTableMetaData<std::string>("string") == "string"); ASSERT(tableUVec3.getTableMetaData<int>("int") == 10); std::cout << tableUVec3 << std::endl; std::cout << "Test DataTable packing for Quaternion." << std::endl; tableDouble.setColumnLabels({"col0.0", "col0.1", "col0.2", "col0.3", "col1.0", "col1.1", "col1.2", "col1.3", "col2.0", "col2.1", "col2.2", "col2.3"}); auto tableQuat = tableDouble.pack<SimTK::Quaternion>(); expLabels = {"col0", "col1", "col2"}; ASSERT(tableQuat.getColumnLabels() == expLabels); ASSERT(tableQuat.getNumRows() == 3); ASSERT(tableQuat.getNumColumns() == 3); ASSERT(tableQuat.getTableMetaData<std::string>("string") == "string"); ASSERT(tableQuat.getTableMetaData<int>("int") == 10); std::cout << tableQuat << std::endl; std::cout << "Test DataTable packing for SpatialVec" << std::endl; tableDouble.setColumnLabels({"col0.0", "col0.1", "col0.2", "col0.3", "col0.4", "col0.5", "col1.0", "col1.1", "col1.2", "col1.3", "col1.4", "col1.5"}); auto tableSVec = tableDouble.pack<SimTK::SpatialVec>(); expLabels = {"col0", "col1"}; ASSERT(tableSVec.getColumnLabels() == expLabels); ASSERT(tableSVec.getNumRows() == 3); ASSERT(tableSVec.getNumColumns() == 2); ASSERT(tableSVec.getTableMetaData<std::string>("string") == "string"); ASSERT(tableSVec.getTableMetaData<int>("int") == 10); std::cout << tableSVec << std::endl; } { std::cout << "Test TimeSeriesTable packing." << std::endl; TimeSeriesTable_<double> tableDouble{}; tableDouble.setColumnLabels({"col0_x", "col0_y", "col0_z", "col1_x", "col1_y", "col1_z", "col2_x", "col2_y", "col2_z", "col3_x", "col3_y", "col3_z"}); tableDouble.appendRow(1, {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); tableDouble.appendRow(2, {2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2}); tableDouble.appendRow(3, {3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3}); tableDouble.addTableMetaData("string", std::string{"string"}); tableDouble.addTableMetaData("int", 10); ASSERT(tableDouble.getColumnLabels().size() == 12); ASSERT(tableDouble.getNumRows() == 3); ASSERT(tableDouble.getNumColumns() == 12); std::cout << tableDouble << std::endl; std::cout << "Test TimeSeriesTable packing for Vec3 with suffix" " specified." << std::endl; TimeSeriesTable_<SimTK::Vec3> tableVec3_1 = tableDouble.pack<SimTK::Vec3>({"_x", "_y", "_z"}); std::vector<std::string> expLabels{"col0", "col1", "col2", "col3"}; ASSERT(tableVec3_1.getColumnLabels() == expLabels); ASSERT(tableVec3_1.getNumRows() == 3); ASSERT(tableVec3_1.getNumColumns() == 4); ASSERT(tableVec3_1.getTableMetaData<std::string>("string") == "string"); ASSERT(tableVec3_1.getTableMetaData<int>("int") == 10); std::cout << tableVec3_1 << std::endl; std::cout << "Test TimeSeriesTable packing for Vec3 with suffix" " unspecified." << std::endl; TimeSeriesTable_<SimTK::Vec3> tableVec3_2 = tableDouble.pack<SimTK::Vec3>(); ASSERT(tableVec3_2.getColumnLabels() == expLabels); ASSERT(tableVec3_2.getNumRows() == 3); ASSERT(tableVec3_2.getNumColumns() == 4); ASSERT(tableVec3_2.getTableMetaData<std::string>("string") == "string"); ASSERT(tableVec3_2.getTableMetaData<int>("int") == 10); std::cout << tableVec3_2 << std::endl; std::cout << "Test TimeSeriesTable packing for UnitVec3." << std::endl; auto tableUVec3 = tableDouble.pack<SimTK::UnitVec3>(); ASSERT(tableUVec3.getColumnLabels() == expLabels); ASSERT(tableUVec3.getNumRows() == 3); ASSERT(tableUVec3.getNumColumns() == 4); ASSERT(tableUVec3.getTableMetaData<std::string>("string") == "string"); ASSERT(tableUVec3.getTableMetaData<int>("int") == 10); std::cout << tableUVec3 << std::endl; std::cout << "Test TimeSeriesTable packing for Quaternion." << std::endl; tableDouble.setColumnLabels({"col0.0", "col0.1", "col0.2", "col0.3", "col1.0", "col1.1", "col1.2", "col1.3", "col2.0", "col2.1", "col2.2", "col2.3"}); TimeSeriesTable_<SimTK::Quaternion> tableQuat = tableDouble.pack<SimTK::Quaternion>(); expLabels = {"col0", "col1", "col2"}; ASSERT(tableQuat.getColumnLabels() == expLabels); ASSERT(tableQuat.getNumRows() == 3); ASSERT(tableQuat.getNumColumns() == 3); ASSERT(tableQuat.getTableMetaData<std::string>("string") == "string"); ASSERT(tableQuat.getTableMetaData<int>("int") == 10); std::cout << tableQuat << std::endl; std::cout << "Test TimeSeriesTable packing for SpatialVec" << std::endl; tableDouble.setColumnLabels({"col0.0", "col0.1", "col0.2", "col0.3", "col0.4", "col0.5", "col1.0", "col1.1", "col1.2", "col1.3", "col1.4", "col1.5"}); TimeSeriesTable_<SimTK::SpatialVec> tableSVec = tableDouble.pack<SimTK::SpatialVec>(); expLabels = {"col0", "col1"}; ASSERT(tableSVec.getColumnLabels() == expLabels); ASSERT(tableSVec.getNumRows() == 3); ASSERT(tableSVec.getNumColumns() == 2); ASSERT(tableSVec.getTableMetaData<std::string>("string") == "string"); ASSERT(tableSVec.getTableMetaData<int>("int") == 10); std::cout << tableSVec << std::endl; } return 0; }