diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b1f98ed..007fc5fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,10 +23,13 @@ ENDIF() set(SRC_LIST src/math/Matrix.cpp src/math/Matrix33.cpp + src/math/Matrix44.cpp src/math/Matrix66.cpp src/math/Vector3d.cpp src/math/Vector6d.cpp src/Analogs.cpp + src/AnalogsInfo.cpp + src/AnalogsSubframe.cpp src/Channel.cpp src/Data.cpp src/ezc3d.cpp @@ -37,7 +40,11 @@ set(SRC_LIST src/Parameters.cpp src/Point.cpp src/Points.cpp - src/Subframe.cpp + src/PointsInfo.cpp + src/Rotation.cpp + src/Rotations.cpp + src/RotationsInfo.cpp + src/RotationsSubframe.cpp src/modules/ForcePlatforms.cpp ) diff --git a/README.md b/README.md index 0eb9a27b..e6d18adb 100644 --- a/README.md +++ b/README.md @@ -216,7 +216,7 @@ c3d.parameter("GroupName", param); // Add the parameter to the c3d structure Please note that if this parameter already exist in the group named "GroupName", then this parameter is replaced by the new one. Otherwise, if it doesn't exist or the group doesn't exist, then it is added to the group or the group is created then the parameter is added. For more information on how to set a new parameter from `c3d` accessors methods, please refer to the documentation on [c3d](https://pyomeca.github.io/Documentation/ezc3d/classezc3d_1_1c3d.html). #### Get data -Point and analogous data are the core of the C3D file. To understand the structure though it is essential to understand that everything is based on points. For example, the base frame rate the point frame rate, while the analogous data is based on the number of data per point frame. Therefore to get a particular point in time, you must get the data at a certain frame and specify which point you are interested in, while to get a particular analogous data you must also specify the subframe. +Point and analogous data are the core of the C3D file (please note that rotation data are also available, but are non-standard). To understand the structure though it is essential to understand that everything is based on points. For example, the base frame rate the point frame rate, while the analogous data is based on the number of data per point frame. Therefore to get a particular point in time, you must get the data at a certain frame and specify which point you are interested in, while to get a particular analogous data you must also specify the subframe. ```C++ ezc3d::c3d c3d("path_to_c3d.c3d"); ezc3d::DataNS::Points3dNS::Point pt(new_c3d.c3d.data().frame(f).points().point(0)); diff --git a/binding/ezc3d.i b/binding/ezc3d.i index a0701899..ec290c5e 100644 --- a/binding/ezc3d.i +++ b/binding/ezc3d.i @@ -27,7 +27,8 @@ namespace std { %template(VecPoints) vector; %template(VecAnalogSubFrames) vector; %template(VecAnalogChannels) vector; - + %template(VecRotationSubFrames) vector; + %template(VecRotations) vector; } // Manage exceptions raised @@ -51,11 +52,16 @@ namespace std { } // Includes all necessary files from the API +%rename(AnalogsSubframe) ezc3d::DataNS::AnalogsNS::SubFrame; +%rename(RotationsSubframe) ezc3d::DataNS::RotationNS::SubFrame; +%rename(RotationsInfo) ezc3d::DataNS::RotationNS::Info; + #define __attribute__(x) %include "ezc3dConfig.h" %include "ezc3d.h" %include "math/Matrix.h" %include "math/Matrix33.h" +%include "math/Matrix44.h" %include "math/Matrix66.h" %include "math/Vector3d.h" %include "math/Vector6d.h" @@ -68,8 +74,13 @@ namespace std { %include "Points.h" %include "Point.h" %include "Analogs.h" -%include "Subframe.h" +%include "AnalogsSubframe.h" %include "Channel.h" +%include "Rotations.h" +%include "RotationsSubframe.h" +%include "Rotation.h" +%include "RotationsInfo.h" + // Add the modules namespace std { diff --git a/binding/python3/__init__.py b/binding/python3/__init__.py index 75a5bd3f..83b8aa17 100644 --- a/binding/python3/__init__.py +++ b/binding/python3/__init__.py @@ -127,19 +127,25 @@ def __init__(self, path="", extract_forceplat_data=False, ignore_bad_formatting= else: self.c3d_swig = ezc3d.c3d(path, ignore_bad_formatting) + rotations_info = ezc3d.RotationsInfo(self.c3d_swig) + self.extract_forceplat_data = extract_forceplat_data - self._storage["header"] = c3d.Header(self.c3d_swig.header()) + self._storage["header"] = c3d.Header(self.c3d_swig.header(), rotations_info) self._storage["parameters"] = c3d.Parameter(self.c3d_swig.parameters()) - self._storage["data"] = c3d.Data(self.c3d_swig, self.extract_forceplat_data) + self._storage["data"] = c3d.Data(self.c3d_swig, rotations_info, self.extract_forceplat_data) return - def __deepcopy__(self, memodict={}): + def __deepcopy__(self, memodict=None): + if memodict is None: + memodict = {} # Create a valid structure new = c3d() + rotations_info = ezc3d.RotationsInfo(self.c3d_swig) new.extract_forceplat_data = self.extract_forceplat_data - new._storage["header"] = c3d.Header(new.c3d_swig.header()) + + new._storage["header"] = c3d.Header(new.c3d_swig.header(), rotations_info) new._storage["parameters"] = c3d.Parameter(new.c3d_swig.parameters()) - new._storage["data"] = c3d.Data(new.c3d_swig, new.extract_forceplat_data) + new._storage["data"] = c3d.Data(new.c3d_swig, rotations_info, new.extract_forceplat_data) # Update the structure with a copy of all data for header_key in self["header"]: @@ -153,8 +159,8 @@ def __deepcopy__(self, memodict={}): return new class Header(C3dMapper): - def __init__(self, swig_header): - super(c3d.Header, self).__init__() + def __init__(self, swig_header, rotation_info): + super().__init__() # Interface to swig pointers self.header = swig_header @@ -171,6 +177,14 @@ def __init__(self, swig_header): "first_frame": self.header.nbAnalogByFrame() * self.header.firstFrame(), "last_frame": self.header.nbAnalogByFrame() * (self.header.lastFrame() + 1) - 1, } + if rotation_info.hasGroup(): + rotation_frame_rate = self.header.frameRate() * rotation_info.ratio() + self._storage["rotations"] = { + "size": rotation_info.used(), + "frame_rate": rotation_frame_rate, + "first_frame": rotation_frame_rate * self.header.firstFrame(), + "last_frame": rotation_frame_rate * (self.header.lastFrame() + 1) - 1, + } self._storage["events"] = { "size": len(self.header.eventsTime()), "events_time": self.header.eventsTime(), @@ -181,7 +195,7 @@ def __init__(self, swig_header): class Parameter(C3dMutableMapper): def __init__(self, swig_param): - super(c3d.Parameter, self).__init__() + super().__init__() # Interface to swig pointers self.parameters = swig_param @@ -222,6 +236,8 @@ def add_parameter(self, group_name, param_ezc3d): value = [] for element in table: value.append(element) + else: + raise RuntimeError("Data type not recognized") param["value"] = value param_name = param_ezc3d.name() @@ -231,7 +247,7 @@ def add_parameter(self, group_name, param_ezc3d): class PlatForm(C3dMapper): def __init__(self, swig_pf): - super(c3d.PlatForm, self).__init__() + super().__init__() self._storage["unit_force"] = swig_pf.forceUnit() self._storage["unit_moment"] = swig_pf.momentUnit() @@ -259,8 +275,8 @@ def __init__(self, swig_pf): self._storage["Tz"][:, i] = Tz[i].to_array()[:, 0] class Data(C3dMutableMapper): - def __init__(self, swig_c3d, extract_forceplat_data): - super(c3d.Data, self).__init__() + def __init__(self, swig_c3d, rotations_info, extract_forceplat_data): + super().__init__() # Interface to swig pointers self.data = swig_c3d.data() @@ -272,6 +288,9 @@ def __init__(self, swig_c3d, extract_forceplat_data): } self._storage["analogs"] = swig_c3d.get_analogs() + if rotations_info.hasGroup(): + self._storage["rotations"] = swig_c3d.get_rotations() + # Add the platform filer if required if extract_forceplat_data: all_pf = [] @@ -486,6 +505,21 @@ def write(self, path): "'c3d['parameters']['ANALOG']['LABELSX']' must have the same length as nAnalogs of the data. " ) + data_rotations = None + if "rotations" in self._storage["data"]: + data_rotations = self._storage["data"]["rotations"] + if len(data_rotations.shape) != 4: + raise TypeError("Rotations should be a numpy with exactly 4 dimensions (4 x 4 x nRotations x nFrames)") + if data_rotations.shape[0] != 4 or data_rotations.shape[1] != 4: + raise TypeError("Rotations should be a numpy with first and second dimension exactly equals to 4 element") + nb_rotations = data_rotations.shape[2] + nb_rotations_frames = data_rotations.shape[3] + + # Store the ratio + if nb_rotations_frames % nb_point_frames != 0: + raise ValueError("Number of rotations' frame should be an integer multiple of frames") + self.add_parameter("ROTATION", "RATIO", int(nb_rotations_frames / nb_point_frames)) + # Start from a fresh c3d new_c3d = ezc3d.c3d() @@ -581,7 +615,7 @@ def write(self, path): for i in range(nb_points): pts.point(pt) c = ezc3d.Channel() - subframe = ezc3d.SubFrame() + subframe = ezc3d.AnalogsSubframe() for i in range(nb_analogs): subframe.channel(c) analogs = ezc3d.Analogs() @@ -589,8 +623,12 @@ def write(self, path): analogs.subframe(subframe) # # Fill the data - new_c3d.import_numpy_data(data_points, data_meta_points["residuals"], data_meta_points["camera_masks"], data_analogs) + new_c3d.import_numpy_data( + data_points, data_meta_points["residuals"], data_meta_points["camera_masks"], data_analogs, data_rotations + ) # Write the file new_c3d.write(path) return + + diff --git a/binding/python3/ezc3d_python.i b/binding/python3/ezc3d_python.i index 552ecde5..37b53e73 100644 --- a/binding/python3/ezc3d_python.i +++ b/binding/python3/ezc3d_python.i @@ -5,6 +5,7 @@ #include "Header.h" #include "Data.h" #include "Parameters.h" +#include "RotationsInfo.h" %} %include "numpy.i" @@ -125,19 +126,61 @@ PyObject * _get_analogs(const ezc3d::c3d& c3d, const std::vector& analogs) // Get the data size_t nAnalogs(analogs.size()); size_t nFrames(c3d.data().nbFrames()); - int nSubframe(c3d.header().nbAnalogByFrame()); - double * data = new double[nAnalogs * nFrames * nSubframe]; + int nSubframes(c3d.header().nbAnalogByFrame()); + double * data = new double[nAnalogs * nFrames * nSubframes]; for (size_t f = 0; f < nFrames; ++f) - for (size_t sf = 0; sf < nSubframe; ++sf) + for (size_t sf = 0; sf < nSubframes; ++sf) for (int a = 0; a < nAnalogs; ++a) - data[nSubframe*nFrames*a + sf+nSubframe*f] = c3d.data().frame(f).analogs().subframe(sf).channel(a).data(); + data[nSubframes*nFrames*a + sf+nSubframes*f] = c3d.data().frame(f).analogs().subframe(sf).channel(analogs[a]).data(); // Export them to Python Object int nArraySize = 3; npy_intp * arraySizes = new npy_intp[nArraySize]; arraySizes[0] = 1; arraySizes[1] = nAnalogs; - arraySizes[2] = nFrames * nSubframe; + arraySizes[2] = nFrames * nSubframes; + PyArrayObject * c = (PyArrayObject *)PyArray_SimpleNewFromData(nArraySize,arraySizes,NPY_DOUBLE, data); + delete[] arraySizes; + + // Give ownership to Python so it will free the memory when needed + PyArray_ENABLEFLAGS(c, NPY_ARRAY_OWNDATA); + + return PyArray_Return(c); +} +%} + +%inline %{ +PyObject * _get_rotations( + const ezc3d::c3d& c3d, + const std::vector& rotations, + const ezc3d::DataNS::RotationNS::Info& rotationInfo + ) +{ + size_t nRotations(rotations.size()); + size_t nFrames(c3d.data().nbFrames()); + size_t nSubframes(rotationInfo.ratio()); + int cmp = 0; + double * data = new double[16 * nRotations * nFrames * nSubframes]; + for (size_t f = 0; f < nFrames; ++f) + for (size_t sf = 0; sf < nSubframes; ++sf) + for (size_t r = 0; r < nRotations; ++r){ + const ezc3d::DataNS::RotationNS::Rotation& currentData = + c3d.data().frame(f).rotations().subframe(sf).rotation(rotations[r]); + for (size_t i = 0; i<4; ++i){ + for (size_t j = 0; j<4; ++j){ + data[f + r*nFrames*nSubframes + j*nRotations*nFrames*nSubframes + i*4*nRotations*nFrames*nSubframes] = + currentData(i, j); + } + } + } + + // Export them to Python Object + int nArraySize = 4; + npy_intp * arraySizes = new npy_intp[nArraySize]; + arraySizes[0] = 4; + arraySizes[1] = 4; + arraySizes[2] = nRotations; + arraySizes[3] = nFrames * nSubframes; PyArrayObject * c = (PyArrayObject *)PyArray_SimpleNewFromData(nArraySize,arraySizes,NPY_DOUBLE, data); delete[] arraySizes; @@ -171,20 +214,39 @@ PyArrayObject *helper_getPyArrayObject( PyObject *input, int type) { %} %inline %{ - void _import_numpy_data(ezc3d::c3d *self, PyArrayObject *pointsData, PyArrayObject *residualsData, PyArrayObject* cameraMasksData, PyArrayObject *analogData){ + void _import_numpy_data( + ezc3d::c3d *self, + PyArrayObject *pointsData, + PyArrayObject *residualsData, + PyArrayObject *cameraMasksData, + PyArrayObject *analogData, + PyArrayObject *rotationData + ){ const size_t nbFrames = PyArray_DIM(pointsData, 2); const size_t nbPoints = PyArray_DIM(pointsData, 1); const size_t nbAnalog = PyArray_DIM(analogData, 1); const size_t nbAnalogFrames = PyArray_DIM(analogData, 2); const size_t nbAnalogSubframes = nbAnalogFrames / nbFrames; + size_t nbRotation; + size_t nbRotationFrames; + size_t nbRotationSubframes; + if (rotationData){ + nbRotation = PyArray_DIM(rotationData, 2); + nbRotationFrames = PyArray_DIM(rotationData, 3); + nbRotationSubframes = nbRotationFrames / nbFrames; + } ezc3d::DataNS::Points3dNS::Points pts; ezc3d::DataNS::Points3dNS::Point pt; ezc3d::DataNS::AnalogsNS::Channel c; - ezc3d::DataNS::AnalogsNS::SubFrame subframe; + ezc3d::DataNS::AnalogsNS::SubFrame analogsSubframe; ezc3d::DataNS::AnalogsNS::Analogs analogs; + ezc3d::DataNS::RotationNS::Rotation rot; + ezc3d::DataNS::RotationNS::SubFrame rotationsSubframe; + ezc3d::DataNS::RotationNS::Rotations rotations; + ezc3d::DataNS::Frame currFrame; for(size_t f = 0; f < nbFrames; ++f){ @@ -211,12 +273,49 @@ PyArrayObject *helper_getPyArrayObject( PyObject *input, int type) { for(size_t i = 0; i < nbAnalog; ++i){ double data = *static_cast(PyArray_GETPTR3(analogData, 0, i, nbAnalogSubframes * f + sf)); c.data(data); - subframe.channel(c, i); + analogsSubframe.channel(c, i); } - analogs.subframe(subframe, sf); + analogs.subframe(analogsSubframe, sf); } - currFrame.add(pts, analogs); + if (rotationData){ + for(size_t sf = 0; sf < nbRotationSubframes; ++sf){ + for(size_t r = 0; r < nbRotation; ++r){ + double elem00 = *static_cast(PyArray_GETPTR4(rotationData, 0, 0, r, nbRotationSubframes * f + sf)); + double elem10 = *static_cast(PyArray_GETPTR4(rotationData, 1, 0, r, nbRotationSubframes * f + sf)); + double elem20 = *static_cast(PyArray_GETPTR4(rotationData, 2, 0, r, nbRotationSubframes * f + sf)); + double elem30 = *static_cast(PyArray_GETPTR4(rotationData, 3, 0, r, nbRotationSubframes * f + sf)); + double elem01 = *static_cast(PyArray_GETPTR4(rotationData, 0, 1, r, nbRotationSubframes * f + sf)); + double elem11 = *static_cast(PyArray_GETPTR4(rotationData, 1, 1, r, nbRotationSubframes * f + sf)); + double elem21 = *static_cast(PyArray_GETPTR4(rotationData, 2, 1, r, nbRotationSubframes * f + sf)); + double elem31 = *static_cast(PyArray_GETPTR4(rotationData, 3, 1, r, nbRotationSubframes * f + sf)); + double elem02 = *static_cast(PyArray_GETPTR4(rotationData, 0, 2, r, nbRotationSubframes * f + sf)); + double elem12 = *static_cast(PyArray_GETPTR4(rotationData, 1, 2, r, nbRotationSubframes * f + sf)); + double elem22 = *static_cast(PyArray_GETPTR4(rotationData, 2, 2, r, nbRotationSubframes * f + sf)); + double elem32 = *static_cast(PyArray_GETPTR4(rotationData, 3, 2, r, nbRotationSubframes * f + sf)); + double elem03 = *static_cast(PyArray_GETPTR4(rotationData, 0, 3, r, nbRotationSubframes * f + sf)); + double elem13 = *static_cast(PyArray_GETPTR4(rotationData, 1, 3, r, nbRotationSubframes * f + sf)); + double elem23 = *static_cast(PyArray_GETPTR4(rotationData, 2, 3, r, nbRotationSubframes * f + sf)); + double elem33 = *static_cast(PyArray_GETPTR4(rotationData, 3, 3, r, nbRotationSubframes * f + sf)); + double reliability = + std::isnan(elem00 + elem01 + elem02 + elem03 + + elem10 + elem11 + elem12 + elem13 + + elem20 + elem21 + elem22 + elem23 + + elem30 + elem31 + elem32 + elem33) + ? -1. : 0.; + + rot.set(elem00, elem01, elem02, elem03, + elem10, elem11, elem12, elem13, + elem20, elem21, elem22, elem23, + elem30, elem31, elem32, elem33, + reliability); + rotationsSubframe.rotation(rot, r); + } + rotations.subframe(rotationsSubframe, sf); + } + } + + currFrame.add(pts, analogs, rotations); self->frame(currFrame); } } @@ -224,14 +323,22 @@ PyArrayObject *helper_getPyArrayObject( PyObject *input, int type) { %extend ezc3d::c3d { - // Extend c3d class to "import" data from numpy-arrays into object efficiently - void import_numpy_data(PyObject *pointsData, PyObject *residualsData, PyObject *cameraMasksData, PyObject *analogData){ + void import_numpy_data( + PyObject *pointsData, + PyObject *residualsData, + PyObject *cameraMasksData, + PyObject *analogData, + PyObject *rotationsData + ){ PyArrayObject *pointsDataArr = helper_getPyArrayObject(pointsData, NPY_DOUBLE); PyArrayObject *residualsDataArr = helper_getPyArrayObject(residualsData, NPY_DOUBLE); PyArrayObject *cameraMasksDataArr = helper_getPyArrayObject(cameraMasksData, NPY_DOUBLE); PyArrayObject *analogDataArr = helper_getPyArrayObject(analogData, NPY_DOUBLE); - _import_numpy_data(self, pointsDataArr, residualsDataArr, cameraMasksDataArr, analogDataArr); + PyArrayObject *rotationDataArr = nullptr; + if (rotationsData != Py_None) + rotationDataArr = helper_getPyArrayObject(rotationsData, NPY_DOUBLE); + _import_numpy_data(self, pointsDataArr, residualsDataArr, cameraMasksDataArr, analogDataArr, rotationDataArr); } // Extend c3d class to get an easy accessor to data points @@ -329,6 +436,34 @@ PyArrayObject *helper_getPyArrayObject( PyObject *input, int type) { channels.push_back(channel); return _get_analogs(*self, channels); } + + + // Extend c3d class to get an easy accessor to data points + PyObject * get_rotations(){ + // Get the data + ezc3d::DataNS::RotationNS::Info rotationInfo(*self); + std::vector rotations; + for (int i = 0; i < rotationInfo.used(); ++i) + rotations.push_back(i); + return _get_rotations(*self, rotations, rotationInfo); + } + + PyObject * get_rotations(int* rotations, int nRotations) + { + ezc3d::DataNS::RotationNS::Info rotationInfo(*self); + std::vector _rotations; + for (int i = 0; i < nRotations; ++i) + _rotations.push_back(rotations[i]); + return _get_rotations(*self, _rotations, rotationInfo); + } + + PyObject * get_rotations(int rotation) + { + ezc3d::DataNS::RotationNS::Info rotationInfo(*self); + std::vector rotations; + rotations.push_back(rotation); + return _get_rotations(*self, rotations, rotationInfo); + } } diff --git a/examples/create_and_read_example.cpp b/examples/create_and_read_example.cpp index 6b38ffbf..711c3b13 100644 --- a/examples/create_and_read_example.cpp +++ b/examples/create_and_read_example.cpp @@ -6,6 +6,11 @@ int main() { + { + ezc3d::c3d c3d2("/home/pariterre/Programmation/ezc3d/test/c3dFiles/ezc3d-testFiles-master/ezc3d-testFiles-master/C3DRotationExample.c3d"); + c3d2.print(); + } + // Create an empty fresh c3d ezc3d::c3d c3d_empty; ezc3d::ParametersNS::GroupNS::Parameter t("SCALE"); diff --git a/examples/force_plate_example.cpp b/examples/force_plate_example.cpp index 2405803e..027ba781 100644 --- a/examples/force_plate_example.cpp +++ b/examples/force_plate_example.cpp @@ -8,26 +8,26 @@ int main() auto& pf0(pf.forcePlatform(0)); // Show some metadata - std::cout << "Number of force platform (pf) = " << pf.forcePlatforms().size() << std::endl; - std::cout << std::endl; - std::cout << "Information for pf 0:" << std::endl; - std::cout << "Number of frames = " << pf0.nbFrames() << std::endl; - std::cout << "Type = " << pf0.type() << std::endl; - std::cout << "Force units = " << pf0.forceUnit() << std::endl; - std::cout << "Moment units = " << pf0.momentUnit() << std::endl; - std::cout << "Position units = " << pf0.positionUnit() << std::endl; - std::cout << "Calibration matrix = " << std::endl << pf0.calMatrix() << std::endl; - std::cout << "Corners = " << std::endl << pf0.corners() << std::endl; - std::cout << "Origin = " << std::endl << pf0.origin() << std::endl; - std::cout << std::endl; + std::cout << "Number of force platform (pf) = " << pf.forcePlatforms().size() << "\n"; + std::cout << "\n"; + std::cout << "Information for pf 0:" << "\n"; + std::cout << "Number of frames = " << pf0.nbFrames() << "\n"; + std::cout << "Type = " << pf0.type() << "\n"; + std::cout << "Force units = " << pf0.forceUnit() << "\n"; + std::cout << "Moment units = " << pf0.momentUnit() << "\n"; + std::cout << "Position units = " << pf0.positionUnit() << "\n"; + std::cout << "Calibration matrix = " << "\n" << pf0.calMatrix() << "\n"; + std::cout << "Corners = " << "\n" << pf0.corners() << "\n"; + std::cout << "Origin = " << "\n" << pf0.origin() << "\n"; + std::cout << "\n"; // Show some data - std::cout << "Data for 1st frame:" << std::endl; + std::cout << "Data for 1st frame:" << "\n"; // Values - std::cout << "Forces = " << pf0.forces()[0].T() << std::endl; - std::cout << "Moments = " << pf0.moments()[0].T() << std::endl; - std::cout << "CoP = " << pf0.CoP()[0].T() << std::endl; - std::cout << "Tz = " << pf0.Tz()[0].T() << std::endl; + std::cout << "Forces = " << pf0.forces()[0].T() << "\n"; + std::cout << "Moments = " << pf0.moments()[0].T() << "\n"; + std::cout << "CoP = " << pf0.CoP()[0].T() << "\n"; + std::cout << "Tz = " << pf0.Tz()[0].T() << "\n"; return 0; } diff --git a/include/Analogs.h b/include/Analogs.h index 4d04bac3..09cc9286 100644 --- a/include/Analogs.h +++ b/include/Analogs.h @@ -8,7 +8,7 @@ /// \date October 17th, 2018 /// -#include "Subframe.h" +#include "AnalogsSubframe.h" /// /// \brief Analog holder for C3D analogous data @@ -21,6 +21,16 @@ class EZC3D_API ezc3d::DataNS::AnalogsNS::Analogs{ /// Analogs(); + /// + /// \brief Create a filled Analogs class at a given frame from a given file + /// \param c3d Reference to the c3d to copy the data in + /// \param file File to copy the data from + /// \param info The information about the analogs + /// + Analogs( + ezc3d::c3d& c3d, + std::fstream& file, + const AnalogsNS::Info& info); //---- STREAM ----// public: diff --git a/include/AnalogsInfo.h b/include/AnalogsInfo.h new file mode 100644 index 00000000..1b64e21a --- /dev/null +++ b/include/AnalogsInfo.h @@ -0,0 +1,70 @@ +#ifndef ANALOGS_INFO_H +#define ANALOGS_INFO_H +/// +/// \file AnalogInfo.cpp +/// \brief Implementation of AnalogInfo class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "ezc3d.h" + +/// +/// \brief 3D rotation data +/// +class EZC3D_API ezc3d::DataNS::AnalogsNS::Info { + //---- CONSTRUCTORS ----// +public: + /// + /// \brief Reads and create a proper AnalogInfo class + /// c3d The c3d structure to read the values from + /// + Info(const ezc3d::c3d& c3d); + + //---- DATA ----// +protected: + PROCESSOR_TYPE _processorType; ///< The type of processor formatting + +public: + /// + /// \brief Returns the type of processor formatting + /// \return The type of processor formatting + /// + PROCESSOR_TYPE processorType() const; + + +protected: + std::vector _scaleFactors; ///< The scale factors by channel + +public: + /// + /// \brief Returns the scale factors by channel + /// \return The scale factors by channel + /// + const std::vector& scaleFactors() const; + + +protected: + double _generalFactor; ///< The general scale factor + +public: + /// + /// \brief Returns the general scale factor + /// \return The general scale factor + /// + double generalFactor() const; + + +protected: + std::vector _zeroOffset; ///< The offset of the analogs + +public: + /// + /// \brief Returns the zero offset + /// \return The zero offset + /// + const std::vector& zeroOffset() const; +}; + +#endif diff --git a/include/Subframe.h b/include/AnalogsSubframe.h similarity index 88% rename from include/Subframe.h rename to include/AnalogsSubframe.h index 529daf54..2f50d260 100644 --- a/include/Subframe.h +++ b/include/AnalogsSubframe.h @@ -1,7 +1,7 @@ -#ifndef SUBFRAME_H -#define SUBFRAME_H +#ifndef ANALOGS_SUBFRAME_H +#define ANALOGS_SUBFRAME_H /// -/// \file Subframe.h +/// \file AnalogsSubframe.h /// \brief Declaration of Subframe class /// \author Pariterre /// \version 1.0 @@ -21,6 +21,16 @@ class EZC3D_API ezc3d::DataNS::AnalogsNS::SubFrame{ /// SubFrame(); + /// + /// \brief Create a filled SubFrame class at a given frame from a given file + /// \param c3d Reference to the c3d to copy the data in + /// \param file File to copy the data from + /// \param info The information about the analogs + /// + SubFrame( + ezc3d::c3d& c3d, + std::fstream& file, + const AnalogsNS::Info& info); //---- STREAM ----// public: diff --git a/include/Channel.h b/include/Channel.h index dd4b5c4c..8987ea20 100644 --- a/include/Channel.h +++ b/include/Channel.h @@ -28,6 +28,18 @@ class EZC3D_API ezc3d::DataNS::AnalogsNS::Channel{ Channel( const ezc3d::DataNS::AnalogsNS::Channel &channel); + /// + /// \brief Create a filled Channel class at a given subframe from a given file + /// \param c3d Reference to the c3d to copy the data in + /// \param file File to copy the data from + /// \param info The information about the analogs + /// \param channelIndex The index of the channel currently created + /// + Channel( + ezc3d::c3d& c3d, + std::fstream& file, + const AnalogsNS::Info& info, + size_t channelIndex); //---- STREAM ----// public: diff --git a/include/Data.h b/include/Data.h index ed585342..e3ec01c9 100644 --- a/include/Data.h +++ b/include/Data.h @@ -60,6 +60,7 @@ class EZC3D_API ezc3d::DataNS::Data{ //---- FRAME ----// protected: std::vector _frames; ///< Storage of the data + public: /// /// \brief Get the number of frames in the data structure diff --git a/include/Frame.h b/include/Frame.h index c0733748..ccbe6f31 100644 --- a/include/Frame.h +++ b/include/Frame.h @@ -10,6 +10,7 @@ #include "Points.h" #include "Analogs.h" +#include "Rotations.h" /// /// \brief Frame holder for C3D data @@ -87,6 +88,26 @@ class EZC3D_API ezc3d::DataNS::Frame{ ezc3d::DataNS::AnalogsNS::Analogs& analogs(); + //---- ROTATIONS ----// +protected: + std::shared_ptr _rotations; ///< All the rotations for this frame + +public: + /// + /// \brief Return a reference to all the rotations + /// \return Reference to all the rotations + /// + const ezc3d::DataNS::RotationNS::Rotations& rotations() const; + + /// + /// \brief Return a reference to all the rotations in order to be modified by the caller + /// \return A non-const reference to all the rotations + /// + /// Get all the rotations in the form of a non-const reference. + /// The user can thereafter modify these rotations at will, but with the caution it requires. + /// + ezc3d::DataNS::RotationNS::Rotations& rotations(); + //---- ACCESSORS ----// public: /// @@ -110,6 +131,13 @@ class EZC3D_API ezc3d::DataNS::Frame{ void add( const ezc3d::DataNS::AnalogsNS::Analogs &analogs); + /// + /// \brief Add rotations to a frame + /// \param rotations The rotations data to add + /// + void add( + const ezc3d::DataNS::RotationNS::Rotations &rotations); + /// /// \brief Add points and analogs to a frame /// \param points The 3D points to add @@ -119,6 +147,17 @@ class EZC3D_API ezc3d::DataNS::Frame{ const ezc3d::DataNS::Points3dNS::Points &points, const ezc3d::DataNS::AnalogsNS::Analogs &analogs); + /// + /// \brief Add points and analogs to a frame + /// \param points The 3D points to add + /// \param analogs The analogous data to add + /// \param rotations The rotations data to add + /// + void add( + const ezc3d::DataNS::Points3dNS::Points &points, + const ezc3d::DataNS::AnalogsNS::Analogs &analogs, + const ezc3d::DataNS::RotationNS::Rotations &rotations); + /// /// \brief Return if the frame is empty /// \return if the frame is empty diff --git a/include/Header.h b/include/Header.h index 33819a37..58a5eee7 100644 --- a/include/Header.h +++ b/include/Header.h @@ -147,6 +147,24 @@ class EZC3D_API ezc3d::Header{ /// size_t nbAnalogsMeasurement() const; + +protected: + size_t _hasRotationalData; ///< This is a support for rotational data added by C-Motion + +public: + /// + /// \brief Get if rotational data are present in the c3d + /// \return If rotational data are in present in the c3d + /// + bool hasRotationalData() const; + + /// + /// \brief Set if rotational data are present in the c3d + /// \param hasRotationalData If rotational data are present in the c3d + /// + void hasRotationalData( + bool hasRotationalData); + protected: size_t _firstFrame; ///< Byte 4 ///< diff --git a/include/Point.h b/include/Point.h index 79f2430e..56362a10 100644 --- a/include/Point.h +++ b/include/Point.h @@ -29,6 +29,16 @@ class EZC3D_API ezc3d::DataNS::Points3dNS::Point : Point( const ezc3d::DataNS::Points3dNS::Point& point); + /// + /// \brief Create a filled Point class from a given file + /// \param c3d Reference to the c3d to copy the data in + /// \param file File to copy the data from + /// \param info The information about the points + /// + Point( + ezc3d::c3d& c3d, + std::fstream &file, + const Points3dNS::Info& info); //---- STREAM ----// public: diff --git a/include/Points.h b/include/Points.h index 4bf39769..6b48f3cb 100644 --- a/include/Points.h +++ b/include/Points.h @@ -28,6 +28,16 @@ class EZC3D_API ezc3d::DataNS::Points3dNS::Points{ Points( size_t nbPoints); + /// + /// \brief Create a filled Points class at a given frame from a given file + /// \param c3d Reference to the c3d to copy the data in + /// \param file File to copy the data from + /// \param info The information about the points + /// + Points( + ezc3d::c3d& c3d, + std::fstream& file, + const Points3dNS::Info& info); //---- STREAM ----// public: diff --git a/include/PointsInfo.h b/include/PointsInfo.h new file mode 100644 index 00000000..866f0e86 --- /dev/null +++ b/include/PointsInfo.h @@ -0,0 +1,47 @@ +#ifndef POINTS_INFO_H +#define POINTS_INFO_H +/// +/// \file PointsInfo.cpp +/// \brief Implementation of PointsInfo class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "ezc3d.h" + +/// +/// \brief 3D rotation data +/// +class EZC3D_API ezc3d::DataNS::Points3dNS::Info { + //---- CONSTRUCTORS ----// +public: + /// + /// \brief Reads and create a proper PointsInfo class + /// c3d The c3d structure to read the values from + /// + Info(const ezc3d::c3d& c3d); + + //---- DATA ----// +protected: + PROCESSOR_TYPE _processorType; ///< The type of processor formatting + +public: + /// + /// \brief Returns the type of processor formatting + /// \return The type of processor formatting + /// + PROCESSOR_TYPE processorType() const; + +protected: + double _scaleFactor; ///< The scale factor for all the points + +public: + /// + /// \brief Returns the scale factor for all the points + /// \return The scale factor for all the points + /// + double scaleFactor() const; +}; + +#endif diff --git a/include/Rotation.h b/include/Rotation.h new file mode 100644 index 00000000..5ad35c65 --- /dev/null +++ b/include/Rotation.h @@ -0,0 +1,172 @@ +#ifndef ROTATION_H +#define ROTATION_H +/// +/// \file Rotation.cpp +/// \brief Implementation of Rotation class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "math/Matrix44.h" + +/// +/// \brief 3D rotation data +/// +class EZC3D_API ezc3d::DataNS::RotationNS::Rotation : + public ezc3d::Matrix44 { + //---- CONSTRUCTORS ----// +public: + /// + /// \brief Create an empty rotation with memory allocated but not filled + /// + Rotation(); + + /// + /// \brief Create an empty rotation with memory allocated but not filled/// \param elem00 first col, first row + /// \param elem01 first col, second row + /// \param elem02 first col, third row + /// \param elem03 first col, fourth row + /// \param elem10 second col, first row + /// \param elem11 second col, second row + /// \param elem12 second col, third row + /// \param elem13 second col, fourth row + /// \param elem20 third col, first row + /// \param elem21 third col, second row + /// \param elem22 third col, third row + /// \param elem23 third col, fourth row + /// \param elem30 fourth col, first row + /// \param elem31 fourth col, second row + /// \param elem32 fourth col, third row + /// \param elem33 fourth col, fourth row + /// \param reliability The reliability + /// + Rotation( + double elem00, double elem01, double elem02, double elem03, + double elem10, double elem11, double elem12, double elem13, + double elem20, double elem21, double elem22, double elem23, + double elem30, double elem31, double elem32, double elem33, + double reliability); + + /// + /// \brief Create a filled rotation class at a given subframe from a given file + /// \param c3d Reference to the c3d to copy the data in + /// \param file File to copy the data from + /// \param info The information about the rotations + /// + Rotation( + ezc3d::c3d& c3d, + std::fstream& file, + const RotationNS::Info& info); + + /// + /// \brief Copy a rotation + /// \param rotation The rotation to copy + /// + Rotation( + const ezc3d::DataNS::RotationNS::Rotation& rotation); + + + //---- STREAM ----// +public: + /// + /// + /// \brief Print the rotation + /// + /// Print the rotation matrix to the console + /// + virtual void print() const override; + + /// + /// \brief Write the rotation to an opened file (scaleFactor is necessarily -1) + /// \param f Already opened fstream file with write access + /// + /// Write the values of the rotation to a file + /// + void write( + std::fstream &f) const; + + + //---- DATA ----// +protected: + double _reliability; ///< Reliability metric of the rotation + +public: + /// + /// \brief set All the values of the rotation at once + /// \param elem00 first col, first row + /// \param elem01 first col, second row + /// \param elem02 first col, third row + /// \param elem03 first col, fourth row + /// \param elem10 second col, first row + /// \param elem11 second col, second row + /// \param elem12 second col, third row + /// \param elem13 second col, fourth row + /// \param elem20 third col, first row + /// \param elem21 third col, second row + /// \param elem22 third col, third row + /// \param elem23 third col, fourth row + /// \param elem30 fourth col, first row + /// \param elem31 fourth col, second row + /// \param elem32 fourth col, third row + /// \param elem33 fourth col, fourth row + /// \param reliability The reliability of the rotation + /// + virtual void set( + double elem00, double elem01, double elem02, double elem03, + double elem10, double elem11, double elem12, double elem13, + double elem20, double elem21, double elem22, double elem23, + double elem30, double elem31, double elem32, double elem33, + double reliability); + + /// + /// \brief set All the values of the rotation at once. Don't change the reliability value + /// \param elem00 first col, first row + /// \param elem01 first col, second row + /// \param elem02 first col, third row + /// \param elem03 first col, fourth row + /// \param elem10 second col, first row + /// \param elem11 second col, second row + /// \param elem12 second col, third row + /// \param elem13 second col, fourth row + /// \param elem20 third col, first row + /// \param elem21 third col, second row + /// \param elem22 third col, third row + /// \param elem23 third col, fourth row + /// \param elem30 fourth col, first row + /// \param elem31 fourth col, second row + /// \param elem32 fourth col, third row + /// \param elem33 fourth col, fourth row + /// + virtual void set( + double elem00, double elem01, double elem02, double elem03, + double elem10, double elem11, double elem12, double elem13, + double elem20, double elem21, double elem22, double elem23, + double elem30, double elem31, double elem32, double elem33) override; + + /// + /// \brief Get the reliability component of the rotation + /// \return The reliability component of the rotation + /// + virtual double reliability() const; + + /// + /// \brief Set the reliability component of the rotation + /// \param reliability The reliability component of the rotation + /// + virtual void reliability( + double reliability); + + /// + /// \brief If the rotation is valid + /// + virtual bool isValid() const; + + /// + /// \brief Returns if the rotation is empty + /// \return If the rotation is empty + /// + virtual bool isEmpty() const; +}; + +#endif diff --git a/include/Rotations.h b/include/Rotations.h new file mode 100644 index 00000000..6bed6f5a --- /dev/null +++ b/include/Rotations.h @@ -0,0 +1,131 @@ +#ifndef ROTATIONS_H +#define ROTATIONS_H +/// +/// \file Rotations.cpp +/// \brief Implementation of Rotations class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "RotationsSubframe.h" + +/// +/// \brief Rotation holder for C3D Rotations data +/// base on documentation from https://www.c-motion.com/v3dwiki/index.php?title=ROTATION_DATA_TYPE +/// +class EZC3D_API ezc3d::DataNS::RotationNS::Rotations{ + //---- CONSTRUCTORS ----// +public: + /// + /// \brief Create an empty holder for Rotation data + /// + Rotations(); + + /// + /// \brief Create an empty holder for Rotation data preallocating the size of it + /// \param c3d Reference to the c3d to copy the data in + /// \param file File to copy the data from + /// \param info The information about the rotations + /// + Rotations( + ezc3d::c3d &c3d, + std::fstream &file, + const RotationNS::Info& info); + + + //---- STREAM ----// +public: + /// + /// + /// \brief Print the rotations + /// + /// Print the Rotations to the console by calling sequentially the print method for all the rotations + /// + void print() const; + + /// + /// \brief Write rotations to an opened file (scaleFactor is necessarily -1) + /// \param f Already opened fstream file with write access + /// + /// Write all the rotations to a file by calling sequentially the write method of each rotation + /// + void write( + std::fstream &f) const; + + + //---- ROTATION ----// +protected: + std::vector _subframe; ///< Holder of the 3D rotations at each frame + +public: + /// + /// \brief Get the number of subframes + /// \return The number of subframes + /// + size_t nbSubframes() const; + + /// + /// \brief Resize the number of subframes. Warning, this function drops data if subframes is downsized + /// \param nbSubframes The number of subframes to be in the holder + /// + void nbSubframes( + size_t nbSubframes); + + /// + /// \brief Get a particular subframe of index idx from the rotation data set + /// \param idx The index of the subframe + /// \return The Subframe + /// + /// Get a particular subframe of index idx from the rotation data set. + /// + /// Throw a std::out_of_range exception if idx is larger than the number of subframes + /// + const ezc3d::DataNS::RotationNS::SubFrame& subframe( + size_t idx) const; + + /// + /// \brief Get a particular subframe of index idx from the rotation data set in order to be modified by the caller + /// \param idx The index of the subframe + /// \return A non-const reference to the subframe + /// + /// Get a particular subframe of index idx from the rotation data in the form of a non-const reference. + /// The user can thereafter modify these points at will, but with the caution it requires. + /// + /// Throw a std::out_of_range exception if idx is larger than the number of subframes + /// + ezc3d::DataNS::RotationNS::SubFrame& subframe( + size_t idx); + + /// + /// \brief Add/replace a subframe to the rotation data set + /// \param subframe The subframe to add + /// \param idx The index of the subframe in the rotation data set + /// + /// Add or replace a subframe to the rotation data set. + /// + /// If no idx is sent, then the subframe is appended to the rotation data set. + /// If the idx correspond to a pre-existing subframe, it replaces it. + /// If idx is larger than the number of subframes, it resize the rotation data set accordingly and add the subframe + /// where it belongs but leaves the other created subframes empty. + /// + void subframe( + const ezc3d::DataNS::RotationNS::SubFrame& subframe, + size_t idx = SIZE_MAX); + + + /// + /// \brief Get all the subframes from the rotation data set + /// \return The subframes + /// + const std::vector& subframes() const; + + /// + /// \brief Return if the rotations are empty + /// \return if the rotations are empty + /// + bool isEmpty() const; + +}; + +#endif diff --git a/include/RotationsInfo.h b/include/RotationsInfo.h new file mode 100644 index 00000000..010ba9b3 --- /dev/null +++ b/include/RotationsInfo.h @@ -0,0 +1,81 @@ +#ifndef ROTATIONS_INFO_H +#define ROTATIONS_INFO_H +/// +/// \file RotationsInfo.cpp +/// \brief Implementation of RotationsInfo class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "ezc3d.h" + +/// +/// \brief 3D rotation data +/// +class EZC3D_API ezc3d::DataNS::RotationNS::Info { + //---- CONSTRUCTORS ----// +public: + /// + /// \brief Reads and create a proper RotationsInfo class + /// c3d The c3d structure to read the values from + /// + Info(const ezc3d::c3d& c3d); + + + //---- DATA ----// +protected: + bool _hasGroup; ///< If the group parameter is present + +public: + /// + /// \brief Returns If the group parameter is present + /// \return If the group parameter is present + /// + bool hasGroup() const; + + +protected: + size_t _dataStart; ///< The data start parameter + +public: + /// + /// \brief Returns the data start parameter + /// \return The data start parameter + /// + size_t dataStart() const; + +protected: + size_t _used; ///< The number of Rotations + +public: + /// + /// \brief Returns the number of Rotations + /// \return The number of Rotations + /// + size_t used() const; + +protected: + size_t _ratio; ///< The ratio to point data + +public: + /// + /// \brief Returns the ratio to point data + /// \return The ratio to point data + /// + size_t ratio() const; + +protected: + PROCESSOR_TYPE _processorType; ///< The type of processor formatting + +public: + /// + /// \brief Returns the type of processor formatting + /// \return The type of processor formatting + /// + PROCESSOR_TYPE processorType() const; + + +}; + +#endif diff --git a/include/RotationsSubframe.h b/include/RotationsSubframe.h new file mode 100644 index 00000000..3085e6f6 --- /dev/null +++ b/include/RotationsSubframe.h @@ -0,0 +1,121 @@ +#ifndef ROTATIONS_SUBFRAME_H +#define ROTATIONS_SUBFRAME_H +/// +/// \file RotationsSubframe.h +/// \brief Declaration of Subframe class +/// \author Pariterre +/// \version 1.0 +/// \date October 17th, 2018 +/// + +#include "Rotation.h" + +/// +/// \brief Subframe for the rotation data +/// +class EZC3D_API ezc3d::DataNS::RotationNS::SubFrame{ + //---- CONSTRUCTORS ----// +public: + /// + /// \brief Create an empty subframe for rotation data + /// + SubFrame(); + + /// + /// \brief Create a filled SubFrame class at a given frame from a given file + /// \param c3d Reference to the c3d to copy the data in + /// \param file File to copy the data from + /// \param info The information about the rotations + /// + SubFrame( + ezc3d::c3d& c3d, + std::fstream& file, + const RotationNS::Info& info); + + //---- STREAM ----// +public: + /// + /// + /// \brief Print the subframe + /// + /// Print the subframe to the console by calling sequentially the print method of all of the rotation + /// + void print() const; + + /// + /// \brief Write the subframe to an opened file + /// \param f Already opened fstream file with write access + /// + /// Write the subframe to a file by calling sequentially the write method of all of the rotation + /// + void write( + std::fstream &f) const; + + + //---- ROTATIONS ----// +protected: + std::vector _rotations; ///< Holder for the rotations +public: + /// + /// \brief Get the number of rotations + /// \return The number of rotations + /// + size_t nbRotations() const; + + /// + /// \brief Resize the number of rotations. Warning, this function drops data if rotations are downsized. + /// \param nRotations Number of rotations in the subframe + /// + void nbRotations( + size_t nRotations); + + /// + /// \brief Get a particular rotation of index idx from the rotations data + /// \param idx The index of the rotation + /// \return The rotation + /// + /// Throw a std::out_of_range exception if idx is larger than the number of frames + /// + const ezc3d::DataNS::RotationNS::Rotation& rotation( + size_t idx) const; + + /// + /// \brief Get a particular rotation of index idx from the rotations data + /// \param idx The index of the rotation + /// \return The rotation + /// + /// Throw a std::out_of_range exception if idx is larger than the number of frames + /// + ezc3d::DataNS::RotationNS::Rotation& rotation( + size_t idx); + + /// + /// \brief Add/replace a rotation to the analog subframe data set + /// \param rotation the rotation to add + /// \param idx the index of the rotation in the subframe data set + /// + /// Add or replace a particular rotation to the subframe data set. + /// + /// If no idx is sent, then the rotation is appended to the points data set. + /// If the idx correspond to a pre-existing rotation, it replaces it. + /// If idx is larger than the number of rotation, it resize the subframe accordingly and add the channel + /// where it belongs but leaves the other created rotations empty. + /// + void rotation( + const ezc3d::DataNS::RotationNS::Rotation& rotation, + size_t idx = SIZE_MAX); + + /// + /// \brief Get all the rotations from the 3D rotations data at a specific frame + /// \return The rotations + /// + const std::vector& rotations() const; + + /// + /// \brief Return if the subframe is empty + /// \return if the subframe is empty + /// + bool isEmpty() const; +}; + +#endif diff --git a/include/ezc3d.h b/include/ezc3d.h index 982abf22..0c1c29bf 100644 --- a/include/ezc3d.h +++ b/include/ezc3d.h @@ -107,6 +107,7 @@ namespace ezc3d { class c3d; class EZC3D_API Matrix; class EZC3D_API Matrix33; + class EZC3D_API Matrix44; class EZC3D_API Matrix66; class EZC3D_API Vector3d; class EZC3D_API Vector6d; @@ -140,6 +141,7 @@ namespace ezc3d { namespace Points3dNS { class EZC3D_API Points; class EZC3D_API Point; + class EZC3D_API Info; } /// /// \brief Namespace that holds the Analogs hierarchy @@ -148,9 +150,21 @@ namespace ezc3d { class EZC3D_API Analogs; class EZC3D_API SubFrame; class EZC3D_API Channel; + class EZC3D_API Info; + } + + /// + /// \brief Namespace that holds the Rotations hierarchy + /// + namespace RotationNS { + class EZC3D_API Rotations; + class EZC3D_API Rotation; + class EZC3D_API SubFrame; + class EZC3D_API Info; } } + /// /// \brief Namespace for all the analysis modules /// diff --git a/include/ezc3d_all.h b/include/ezc3d_all.h index 11277437..50f2b499 100644 --- a/include/ezc3d_all.h +++ b/include/ezc3d_all.h @@ -14,6 +14,10 @@ #include "Parameters.h" #include "Data.h" +#include "PointsInfo.h" +#include "AnalogsInfo.h" +#include "RotationsInfo.h" + #include "modules/ForcePlatforms.h" #endif diff --git a/include/math/Matrix.h b/include/math/Matrix.h index 2f57ad49..f62ece99 100644 --- a/include/math/Matrix.h +++ b/include/math/Matrix.h @@ -53,6 +53,7 @@ class EZC3D_API ezc3d::Matrix { // Declare Friendship friend class ezc3d::Matrix33; + friend class ezc3d::Matrix44; friend class ezc3d::Matrix66; friend class ezc3d::Vector3d; friend class ezc3d::Vector6d; @@ -75,6 +76,12 @@ class EZC3D_API ezc3d::Matrix { std::vector _data; ///< Value of the Matrix public: + /// + /// \brief The sum of all elements in the matrix + /// \return The sum of all elements in the matrix + /// + virtual double sum() const; + /// /// \brief Set all values to zero /// diff --git a/include/math/Matrix44.h b/include/math/Matrix44.h new file mode 100644 index 00000000..61a7926e --- /dev/null +++ b/include/math/Matrix44.h @@ -0,0 +1,129 @@ +#ifndef EZC3D_MATH_MATRIX44_H +#define EZC3D_MATH_MATRIX44_H +/// +/// \file Matrix44.cpp +/// \brief Implementation of Matrix44 class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "Matrix.h" + +/// +/// \brief Matrix of unknown dimension +/// +class EZC3D_API ezc3d::Matrix44 : public ezc3d::Matrix { + //---- CONSTRUCTORS ----// +public: + /// + /// \brief Create an 4x4 Matrix with the memory allocated + /// + Matrix44(); + + /// + /// \brief Create an 4x4 Matrix initalized with the 16 elements + /// \param elem00 first col, first row + /// \param elem01 first col, second row + /// \param elem02 first col, third row + /// \param elem03 first col, fourth row + /// \param elem10 second col, first row + /// \param elem11 second col, second row + /// \param elem12 second col, third row + /// \param elem13 second col, fourth row + /// \param elem20 third col, first row + /// \param elem21 third col, second row + /// \param elem22 third col, third row + /// \param elem23 third col, fourth row + /// \param elem30 fourth col, first row + /// \param elem31 fourth col, second row + /// \param elem32 fourth col, third row + /// \param elem33 fourth col, fourth row + /// + Matrix44( + double elem00, double elem01, double elem02, double elem03, + double elem10, double elem11, double elem12, double elem13, + double elem20, double elem21, double elem22, double elem23, + double elem30, double elem31, double elem32, double elem33); + + /// + /// \brief Copy a matrix into a Matrix44 + /// \param other The matrix to copy + /// + Matrix44( + const ezc3d::Matrix &other); + + //---- DATA ----// +public: + /// + /// \brief Return the number of element in the matrix (nbRows * nbCols = 16) + /// \return The number of element in the matrix (nbRows * nbCols = 16) + /// + size_t size() const override; + + /// + /// \brief Return the number of rows (4) + /// \return The number of rows (4) + /// + size_t nbRows() const override; + + /// + /// \brief Return the number of columns (4) + /// \return The number of columns (4) + /// + size_t nbCols() const override; + + /// + /// \brief Do nothing, it is not possible to resize a 4x4 matrix + /// + void resize( + size_t, + size_t) override; + + /// + /// \brief Set an 4x4 Matrix initalized with the 16 elements + /// \param elem00 first col, first row + /// \param elem01 first col, second row + /// \param elem02 first col, third row + /// \param elem03 first col, fourth row + /// \param elem10 second col, first row + /// \param elem11 second col, second row + /// \param elem12 second col, third row + /// \param elem13 second col, fourth row + /// \param elem20 third col, first row + /// \param elem21 third col, second row + /// \param elem22 third col, third row + /// \param elem23 third col, fourth row + /// \param elem30 fourth col, first row + /// \param elem31 fourth col, second row + /// \param elem32 fourth col, third row + /// \param elem33 fourth col, fourth row + /// + virtual void set( + double elem00, double elem01, double elem02, double elem03, + double elem10, double elem11, double elem12, double elem13, + double elem20, double elem21, double elem22, double elem23, + double elem30, double elem31, double elem32, double elem33); + + //---- OPERATIONS ----// +public: + + /// + /// \brief Defining matrix multiplication with a Vector3d + /// \param other The vector to multiply with + /// \return The vector multiplied + /// + virtual ezc3d::Vector3d operator*( + const ezc3d::Vector3d& other); + + /// + /// \brief Defining matrix multiplication with a Matrix44 + /// \param other The matrix to multiply with + /// \return The matrix multiplied + /// + virtual ezc3d::Matrix44 operator*( + const ezc3d::Matrix44& other); + +}; + +#endif diff --git a/include/math/ezc3dMath.h b/include/math/ezc3dMath.h index b7c91dda..f2728e17 100644 --- a/include/math/ezc3dMath.h +++ b/include/math/ezc3dMath.h @@ -10,6 +10,7 @@ #include "math/Matrix.h" #include "math/Matrix33.h" +#include "math/Matrix44.h" #include "math/Matrix66.h" #include "math/Vector3d.h" #include "math/Vector6d.h" diff --git a/src/Analogs.cpp b/src/Analogs.cpp index b8e0f664..ccdc5cf3 100644 --- a/src/Analogs.cpp +++ b/src/Analogs.cpp @@ -8,15 +8,28 @@ /// #include "Analogs.h" +#include "Header.h" ezc3d::DataNS::AnalogsNS::Analogs::Analogs() { + +} + +ezc3d::DataNS::AnalogsNS::Analogs::Analogs( + ezc3d::c3d &c3d, + std::fstream &file, + const AnalogsNS::Info& info) +{ + nbSubframes(c3d.header().nbAnalogByFrame()); + for (size_t k = 0; k < c3d.header().nbAnalogByFrame(); ++k){ + subframe(ezc3d::DataNS::AnalogsNS::SubFrame(c3d, file, info), k); + } } void ezc3d::DataNS::AnalogsNS::Analogs::print() const { for (size_t i = 0; i < nbSubframes(); ++i) { - std::cout << "Subframe = " << i << std::endl; + std::cout << "Subframe = " << i << "\n"; subframe(i).print(); - std::cout << std::endl; + std::cout << "\n"; } } diff --git a/src/AnalogsInfo.cpp b/src/AnalogsInfo.cpp new file mode 100644 index 00000000..98d0749f --- /dev/null +++ b/src/AnalogsInfo.cpp @@ -0,0 +1,56 @@ +#define EZC3D_API_EXPORTS +/// +/// \file AnalogsInfo.cpp +/// \brief Implementation of AnalogsInfo class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "AnalogsInfo.h" +#include "Header.h" +#include "Parameters.h" + + +ezc3d::DataNS::AnalogsNS::Info::Info( + const ezc3d::c3d &c3d) : + _processorType(ezc3d::PROCESSOR_TYPE::INTEL), + _scaleFactors(std::vector()), + _generalFactor(-1), + _zeroOffset(std::vector()) +{ + _processorType = c3d.parameters().processorType(); + + if (c3d.header().nbAnalogs()) + _scaleFactors = c3d.parameters() + .group("ANALOG").parameter("SCALE").valuesAsDouble(); + _generalFactor = c3d.parameters() + .group("ANALOG").parameter("GEN_SCALE").valuesAsDouble()[0]; + _zeroOffset = c3d.parameters() + .group("ANALOG").parameter("OFFSET").valuesAsInt(); + for (int& offset : _zeroOffset){ + offset = abs(offset); + } +} + +ezc3d::PROCESSOR_TYPE ezc3d::DataNS::AnalogsNS::Info::processorType() const +{ + return _processorType; +} + +const std::vector& ezc3d::DataNS::AnalogsNS::Info::scaleFactors() const +{ + return _scaleFactors; +} + +double ezc3d::DataNS::AnalogsNS::Info::generalFactor() const +{ + return _generalFactor; +} + +const std::vector& ezc3d::DataNS::AnalogsNS::Info::zeroOffset() const +{ + return _zeroOffset; +} + + diff --git a/src/Subframe.cpp b/src/AnalogsSubframe.cpp similarity index 85% rename from src/Subframe.cpp rename to src/AnalogsSubframe.cpp index 8f634410..33002963 100644 --- a/src/Subframe.cpp +++ b/src/AnalogsSubframe.cpp @@ -1,18 +1,30 @@ #define EZC3D_API_EXPORTS /// -/// \file Subframe.cpp +/// \file AnalogsSubframe.cpp /// \brief Implementation of Subframe class /// \author Pariterre /// \version 1.0 /// \date October 17th, 2018 /// -#include "Subframe.h" +#include "AnalogsSubframe.h" +#include "Header.h" ezc3d::DataNS::AnalogsNS::SubFrame::SubFrame() { } +ezc3d::DataNS::AnalogsNS::SubFrame::SubFrame( + ezc3d::c3d &c3d, + std::fstream &file, + const ezc3d::DataNS::AnalogsNS::Info &info) +{ + nbChannels(c3d.header().nbAnalogs()); + for (size_t i = 0; i < c3d.header().nbAnalogs(); ++i){ + channel(ezc3d::DataNS::AnalogsNS::Channel(c3d, file, info, i), i); + } +} + void ezc3d::DataNS::AnalogsNS::SubFrame::print() const { for (size_t i = 0; i < nbChannels(); ++i){ channel(i).print(); diff --git a/src/Channel.cpp b/src/Channel.cpp index 6c20de7b..faf2a4a8 100644 --- a/src/Channel.cpp +++ b/src/Channel.cpp @@ -8,6 +8,8 @@ /// #include "Channel.h" +#include "Header.h" +#include "AnalogsInfo.h" ezc3d::DataNS::AnalogsNS::Channel::Channel() { @@ -18,8 +20,26 @@ ezc3d::DataNS::AnalogsNS::Channel::Channel( _data(channel._data) { } +ezc3d::DataNS::AnalogsNS::Channel::Channel( + ezc3d::c3d &c3d, + std::fstream &file, + const ezc3d::DataNS::AnalogsNS::Info &info, + size_t channelIndex) +{ + if (c3d.header().scaleFactor() < 0) // if it is float + data( (c3d.readFloat(info.processorType(), file) + - info.zeroOffset()[channelIndex]) + * info.scaleFactors()[channelIndex] * info.generalFactor() ); + else + data( (static_cast( + c3d.readInt(info.processorType(), file, + ezc3d::DATA_TYPE::WORD)) + - info.zeroOffset()[channelIndex]) + * info.scaleFactors()[channelIndex] * info.generalFactor() ); +} + void ezc3d::DataNS::AnalogsNS::Channel::print() const { - std::cout << "Analog = " << data() << std::endl; + std::cout << "Analog = " << data() << "\n"; } void ezc3d::DataNS::AnalogsNS::Channel::write( diff --git a/src/Data.cpp b/src/Data.cpp index 59ddd6fd..feb52e12 100644 --- a/src/Data.cpp +++ b/src/Data.cpp @@ -10,6 +10,9 @@ #include "Data.h" #include "Header.h" #include "Parameters.h" +#include "AnalogsInfo.h" +#include "PointsInfo.h" +#include "RotationsInfo.h" ezc3d::DataNS::Data::Data() { } @@ -19,156 +22,43 @@ ezc3d::DataNS::Data::Data( // Firstly move the pointer to the data start position file.seekg(static_cast(c3d.header().dataStart()-1)*512, std::ios::beg); - // Get names of the data - std::vector pointNames; - if (c3d.header().nb3dPoints() > 0) - pointNames = c3d.parameters() - .group("POINT").parameter("LABELS") - .valuesAsString(); - std::vector analogNames; - if (c3d.header().nbAnalogs() > 0) - analogNames = c3d.parameters() - .group("ANALOG").parameter("LABELS") - .valuesAsString(); - // Read the data - PROCESSOR_TYPE processorType(c3d.parameters().processorType()); - double pointScaleFactor(-1); - if (c3d.header().nb3dPoints()) - pointScaleFactor = c3d.parameters() - .group("POINT").parameter("SCALE") - .valuesAsDouble()[0]; - std::vector analogScaleFactors; - if (c3d.header().nbAnalogs()) - analogScaleFactors = c3d.parameters() - .group("ANALOG").parameter("SCALE") - .valuesAsDouble(); - double analogGeneralFactor(c3d.parameters() - .group("ANALOG").parameter("GEN_SCALE") - .valuesAsDouble()[0]); - std::vector analogZeroOffset(c3d.parameters().group("ANALOG").parameter("OFFSET").valuesAsInt()); - for (int& offset : analogZeroOffset){ - offset = abs(offset); - } + ezc3d::DataNS::Points3dNS::Info pointsInfo(c3d); + ezc3d::DataNS::AnalogsNS::Info analogsInfo(c3d); + ezc3d::DataNS::RotationNS::Info rotationsInfo(c3d); + for (size_t j = 0; j < c3d.header().nbFrames(); ++j){ if (file.eof()) break; ezc3d::DataNS::Frame f; // Read point 3d - ezc3d::DataNS::Points3dNS::Points ptsAtAFrame( - c3d.header().nb3dPoints()); - for (size_t i = 0; i < c3d.header().nb3dPoints(); ++i){ - ezc3d::DataNS::Points3dNS::Point pt; - if (c3d.header().scaleFactor() < 0){ // if it is float - pt.x(c3d.readFloat(processorType, file)); - pt.y(c3d.readFloat(processorType, file)); - pt.z(c3d.readFloat(processorType, file)); - if (processorType == PROCESSOR_TYPE::INTEL){ - pt.cameraMask(c3d.readInt( - processorType, file, ezc3d::DATA_TYPE::WORD)); - pt.residual(static_cast(c3d.readInt( - processorType, file, ezc3d::DATA_TYPE::WORD)) - * -pointScaleFactor); - } - else if (processorType == PROCESSOR_TYPE::DEC){ - pt.residual(static_cast(c3d.readInt( - processorType, file, ezc3d::DATA_TYPE::WORD)) - * -pointScaleFactor); - pt.cameraMask(c3d.readInt( - processorType, file, ezc3d::DATA_TYPE::WORD)); - } - else if (processorType == PROCESSOR_TYPE::MIPS){ - throw std::runtime_error( - "MIPS processor type not supported yet, please open a " - "GitHub issue to report that you want this feature!"); - } - } else { - pt.x(static_cast( - c3d.readInt( - processorType, file, ezc3d::DATA_TYPE::WORD)) - * pointScaleFactor); - pt.y(static_cast( - c3d.readInt( - processorType, file, ezc3d::DATA_TYPE::WORD)) - * pointScaleFactor); - pt.z(static_cast( - c3d.readInt( - processorType, file, ezc3d::DATA_TYPE::WORD)) - * pointScaleFactor); - if (processorType == PROCESSOR_TYPE::INTEL){ - pt.cameraMask(c3d.readInt( - processorType, file, ezc3d::DATA_TYPE::BYTE)); - pt.residual(static_cast( - c3d.readInt(processorType, - file, ezc3d::DATA_TYPE::BYTE)) - * pointScaleFactor); - } - else if (processorType == PROCESSOR_TYPE::DEC){ - pt.cameraMask(c3d.readInt( - processorType, file, ezc3d::DATA_TYPE::BYTE)); - pt.residual(static_cast( - c3d.readInt(processorType, - file, ezc3d::DATA_TYPE::BYTE)) - * pointScaleFactor); - } - else if (processorType == PROCESSOR_TYPE::MIPS){ - throw std::runtime_error( - "MIPS processor type not supported yet, please open a " - "GitHub issue to report that you want this feature!"); - } - } - if (pt.residual() < 0){ - pt.set(NAN, NAN, NAN); - } - ptsAtAFrame.point(pt, i); - } - // modified by pts_tp which is an nonconst ref to internal points - f.add(ptsAtAFrame); + f.add(ezc3d::DataNS::Points3dNS::Points(c3d, file, pointsInfo)); // Read analogs - ezc3d::DataNS::AnalogsNS::Analogs analog; - analog.nbSubframes(c3d.header().nbAnalogByFrame()); - for (size_t k = 0; k < c3d.header().nbAnalogByFrame(); ++k){ - ezc3d::DataNS::AnalogsNS::SubFrame sub; - sub.nbChannels(c3d.header().nbAnalogs()); - for (size_t i = 0; i < c3d.header().nbAnalogs(); ++i){ - ezc3d::DataNS::AnalogsNS::Channel c; - if (c3d.header().scaleFactor() < 0) // if it is float - c.data( (c3d.readFloat(processorType, file) - - analogZeroOffset[i]) - * analogScaleFactors[i] * analogGeneralFactor ); - else - c.data( (static_cast( - c3d.readInt(processorType, file, - ezc3d::DATA_TYPE::WORD)) - - analogZeroOffset[i]) - * analogScaleFactors[i] * analogGeneralFactor ); - sub.channel(c, i); - } - analog.subframe(sub, k); - } - f.add(analog); + f.add(ezc3d::DataNS::AnalogsNS::Analogs(c3d, file, analogsInfo)); _frames.push_back(f); } - // remove the trailing empty frames if they exist - size_t nFrames(_frames.size()); - if (nFrames > 0) - for (size_t i=0; i(rotationsInfo.dataStart()-1)*512, std::ios::beg); + + for (size_t i = 0; i < c3d.header().nbFrames(); ++i){ + if (file.eof()) break; + + _frames[i].add(ezc3d::DataNS::RotationNS::Rotations(c3d, file, rotationsInfo)); } + } } void ezc3d::DataNS::Data::print() const { for (size_t i = 0; i < nbFrames(); ++i){ - std::cout << "Frame " << i << std::endl; + std::cout << "Frame " << i << "\n"; frame(i).print(); - std::cout << std::endl; + std::cout << "\n"; } } diff --git a/src/Frame.cpp b/src/Frame.cpp index 44271160..6061fb72 100644 --- a/src/Frame.cpp +++ b/src/Frame.cpp @@ -14,11 +14,14 @@ ezc3d::DataNS::Frame::Frame() { new ezc3d::DataNS::Points3dNS::Points()); _analogs = std::shared_ptr( new ezc3d::DataNS::AnalogsNS::Analogs()); + _rotations = std::shared_ptr( + new ezc3d::DataNS::RotationNS::Rotations()); } void ezc3d::DataNS::Frame::print() const { points().print(); analogs().print(); + rotations().print(); } void ezc3d::DataNS::Frame::write( @@ -27,6 +30,7 @@ void ezc3d::DataNS::Frame::write( std::vector analogScaleFactors) const { points().write(f, pointScaleFactor); analogs().write(f, analogScaleFactors); + rotations().write(f); } const ezc3d::DataNS::Points3dNS::Points& ezc3d::DataNS::Frame::points() const { @@ -45,9 +49,19 @@ ezc3d::DataNS::AnalogsNS::Analogs &ezc3d::DataNS::Frame::analogs() { return *_analogs; } +const ezc3d::DataNS::RotationNS::Rotations &ezc3d::DataNS::Frame::rotations() const +{ + return *_rotations; +} + +ezc3d::DataNS::RotationNS::Rotations &ezc3d::DataNS::Frame::rotations() +{ + return *_rotations; +} + void ezc3d::DataNS::Frame::add( const ezc3d::DataNS::Frame &frame) { - add(frame.points(), frame.analogs()); + add(frame.points(), frame.analogs(), frame.rotations()); } void ezc3d::DataNS::Frame::add( @@ -62,6 +76,13 @@ void ezc3d::DataNS::Frame::add( new ezc3d::DataNS::AnalogsNS::Analogs(analogs_frame)); } +void ezc3d::DataNS::Frame::add( + const ezc3d::DataNS::RotationNS::Rotations &rotations) +{ + _rotations = std::shared_ptr( + new ezc3d::DataNS::RotationNS::Rotations(rotations)); +} + void ezc3d::DataNS::Frame::add( const ezc3d::DataNS::Points3dNS::Points &point3d_frame, const ezc3d::DataNS::AnalogsNS::Analogs &analog_frame) { @@ -69,6 +90,16 @@ void ezc3d::DataNS::Frame::add( add(analog_frame); } +void ezc3d::DataNS::Frame::add( + const ezc3d::DataNS::Points3dNS::Points &points, + const ezc3d::DataNS::AnalogsNS::Analogs &analogs, + const ezc3d::DataNS::RotationNS::Rotations &rotations) +{ + add(points); + add(analogs); + add(rotations); +} + bool ezc3d::DataNS::Frame::isEmpty() const { return points().isEmpty() && analogs().isEmpty(); } diff --git a/src/Group.cpp b/src/Group.cpp index f6d55ec5..7504b7f4 100644 --- a/src/Group.cpp +++ b/src/Group.cpp @@ -20,12 +20,12 @@ ezc3d::ParametersNS::GroupNS::Group::Group( } void ezc3d::ParametersNS::GroupNS::Group::print() const { - std::cout << "groupName = " << name() << std::endl; - std::cout << "isLocked = " << isLocked() << std::endl; - std::cout << "desc = " << description() << std::endl; + std::cout << "groupName = " << name() << "\n"; + std::cout << "isLocked = " << isLocked() << "\n"; + std::cout << "desc = " << description() << "\n"; for (size_t i=0; i < nbParameters(); ++i){ - std::cout << "Parameter " << i << std::endl; + std::cout << "Parameter " << i << "\n"; parameter(i).print(); } } diff --git a/src/Header.cpp b/src/Header.cpp index 48a5672c..fca204e2 100644 --- a/src/Header.cpp +++ b/src/Header.cpp @@ -16,6 +16,7 @@ ezc3d::Header::Header(): _checksum(0x50), _nb3dPoints(0), _nbAnalogsMeasurement(0), + _hasRotationalData(false), _firstFrame(0), _lastFrame(0), _nbMaxInterpGap(10), @@ -44,6 +45,7 @@ ezc3d::Header::Header( _checksum(0), _nb3dPoints(0), _nbAnalogsMeasurement(0), + _hasRotationalData(false), _firstFrame(0), _lastFrame(0), _nbMaxInterpGap(10), @@ -66,33 +68,33 @@ ezc3d::Header::Header( } void ezc3d::Header::print() const { - std::cout << "HEADER" << std::endl; - std::cout << "nb3dPoints = " << nb3dPoints() << std::endl; - std::cout << "nbAnalogsMeasurement = " - << nbAnalogsMeasurement() << std::endl; - std::cout << "nbAnalogs = " << nbAnalogs() << std::endl; - std::cout << "firstFrame = " << firstFrame() << std::endl; - std::cout << "lastFrame = " << lastFrame() << std::endl; - std::cout << "nbFrames = " << nbFrames() << std::endl; - std::cout << "nbMaxInterpGap = " << nbMaxInterpGap() << std::endl; - std::cout << "scaleFactor = " << scaleFactor() << std::endl; - std::cout << "dataStart = " << dataStart() << std::endl; - std::cout << "nbAnalogByFrame = " << nbAnalogByFrame() << std::endl; - std::cout << "frameRate = " << frameRate() << std::endl; - std::cout << "keyLabelPresent = " << keyLabelPresent() << std::endl; - std::cout << "firstBlockKeyLabel = " << firstBlockKeyLabel() << std::endl; - std::cout << "fourCharPresent = " << fourCharPresent() << std::endl; - std::cout << "nbEvents = " << nbEvents() << std::endl; + std::cout << "HEADER" << "\n"; + std::cout << "nb3dPoints = " << nb3dPoints() << "\n"; + std::cout << "nbAnalogsMeasurement = " << nbAnalogsMeasurement() << "\n"; + std::cout << "nbAnalogs = " << nbAnalogs() << "\n"; + std::cout << "hasRotationalData = " << hasRotationalData() << "\n"; + std::cout << "firstFrame = " << firstFrame() << "\n"; + std::cout << "lastFrame = " << lastFrame() << "\n"; + std::cout << "nbFrames = " << nbFrames() << "\n"; + std::cout << "nbMaxInterpGap = " << nbMaxInterpGap() << "\n"; + std::cout << "scaleFactor = " << scaleFactor() << "\n"; + std::cout << "dataStart = " << dataStart() << "\n"; + std::cout << "nbAnalogByFrame = " << nbAnalogByFrame() << "\n"; + std::cout << "frameRate = " << frameRate() << "\n"; + std::cout << "keyLabelPresent = " << keyLabelPresent() << "\n"; + std::cout << "firstBlockKeyLabel = " << firstBlockKeyLabel() << "\n"; + std::cout << "fourCharPresent = " << fourCharPresent() << "\n"; + std::cout << "nbEvents = " << nbEvents() << "\n"; for (size_t i=0; i < eventsTime().size(); ++i) std::cout << "eventsTime[" << i << "] = " - << eventsTime(i) << std::endl; + << eventsTime(i) << "\n"; for (size_t i=0; i < eventsDisplay().size(); ++i) std::cout << "eventsDisplay[" << i << "] = " - << eventsDisplay(i) << std::endl; + << eventsDisplay(i) << "\n"; for (size_t i=0; i < eventsLabel().size(); ++i) std::cout << "eventsLabel[" << i << "] = " - << eventsLabel(i) << std::endl; - std::cout << std::endl; + << eventsLabel(i) << "\n"; + std::cout << "\n"; } void ezc3d::Header::write( @@ -325,8 +327,18 @@ size_t ezc3d::Header::nbAnalogsMeasurement() const { return _nbAnalogsMeasurement; } +bool ezc3d::Header::hasRotationalData() const +{ + return _hasRotationalData; +} + +void ezc3d::Header::hasRotationalData(bool value) +{ + _hasRotationalData = value; +} + size_t ezc3d::Header::nbFrames() const { - if (nb3dPoints() == 0 && nbAnalogs() == 0) + if (nb3dPoints() == 0 && nbAnalogs() == 0 && !hasRotationalData()) return 0; else return _lastFrame - _firstFrame + 1; diff --git a/src/Parameter.cpp b/src/Parameter.cpp index e70a0a42..c53293e4 100644 --- a/src/Parameter.cpp +++ b/src/Parameter.cpp @@ -22,28 +22,28 @@ ezc3d::ParametersNS::GroupNS::Parameter::Parameter( } void ezc3d::ParametersNS::GroupNS::Parameter::print() const { - std::cout << "parameterName = " << name() << std::endl; - std::cout << "isLocked = " << isLocked() << std::endl; + std::cout << "parameterName = " << name() << "\n"; + std::cout << "isLocked = " << isLocked() << "\n"; // Data are not separated according to _dimension, which could help to read if (_data_type == DATA_TYPE::CHAR) for (unsigned int i = 0; i < _param_data_string.size(); ++i) std::cout << "param_data_string[" << i << "] = " - << _param_data_string[i] << std::endl; + << _param_data_string[i] << "\n"; if (_data_type == DATA_TYPE::BYTE) for (unsigned int i = 0; i < _param_data_int.size(); ++i) std::cout << "param_data[" << i << "] = " - << _param_data_int[i] << std::endl; + << _param_data_int[i] << "\n"; if (_data_type == DATA_TYPE::INT) for (unsigned int i = 0; i < _param_data_int.size(); ++i) std::cout << "param_data[" << i << "] = " - << _param_data_int[i] << std::endl; + << _param_data_int[i] << "\n"; if (_data_type == DATA_TYPE::FLOAT) for (unsigned int i = 0; i < _param_data_double.size(); ++i) std::cout << "param_data[" << i << "] = " - << _param_data_double[i] << std::endl; + << _param_data_double[i] << "\n"; - std::cout << "description = " << _description << std::endl; + std::cout << "description = " << _description << "\n"; } void ezc3d::ParametersNS::GroupNS::Parameter::write( diff --git a/src/Parameters.cpp b/src/Parameters.cpp index a152f513..86b3e7e2 100644 --- a/src/Parameters.cpp +++ b/src/Parameters.cpp @@ -321,17 +321,17 @@ void ezc3d::ParametersNS::Parameters::setMandatoryParameters() { } void ezc3d::ParametersNS::Parameters::print() const { - std::cout << "Parameters header" << std::endl; - std::cout << "parametersStart = " << parametersStart() << std::endl; - std::cout << "nbParamBlock = " << nbParamBlock() << std::endl; - std::cout << "processorType = " << processorType() << std::endl; + std::cout << "Parameters header" << "\n"; + std::cout << "parametersStart = " << parametersStart() << "\n"; + std::cout << "nbParamBlock = " << nbParamBlock() << "\n"; + std::cout << "processorType = " << processorType() << "\n"; for (size_t i = 0; i < nbGroups(); ++i){ - std::cout << "Group " << i << std::endl; + std::cout << "Group " << i << "\n"; group(i).print(); - std::cout << std::endl; + std::cout << "\n"; } - std::cout << std::endl; + std::cout << "\n"; } ezc3d::ParametersNS::Parameters ezc3d::ParametersNS::Parameters::write( diff --git a/src/Point.cpp b/src/Point.cpp index 5ac453d8..0f3ddbbe 100644 --- a/src/Point.cpp +++ b/src/Point.cpp @@ -8,6 +8,8 @@ /// #include "Point.h" +#include "Header.h" +#include "PointsInfo.h" #include @@ -26,6 +28,76 @@ ezc3d::DataNS::Points3dNS::Point::Point( _cameraMasks = p._cameraMasks; } +ezc3d::DataNS::Points3dNS::Point::Point( + ezc3d::c3d &c3d, + std::fstream &file, + const ezc3d::DataNS::Points3dNS::Info& info) : + ezc3d::Vector3d(), + _residual(-1) +{ + _cameraMasks.resize(7, false); + if (info.scaleFactor() < 0){ // if it is float + x(c3d.readFloat(info.processorType(), file)); + y(c3d.readFloat(info.processorType(), file)); + z(c3d.readFloat(info.processorType(), file)); + if (info.processorType() == PROCESSOR_TYPE::INTEL){ + cameraMask(c3d.readInt(info.processorType(), file, ezc3d::DATA_TYPE::WORD)); + residual(static_cast(c3d.readInt( + info.processorType(), file, ezc3d::DATA_TYPE::WORD)) + * -info.scaleFactor()); + } + else if (info.processorType() == PROCESSOR_TYPE::DEC){ + residual(static_cast(c3d.readInt( + info.processorType(), file, ezc3d::DATA_TYPE::WORD)) + * -info.scaleFactor()); + cameraMask(c3d.readInt( + info.processorType(), file, ezc3d::DATA_TYPE::WORD)); + } + else if (info.processorType() == PROCESSOR_TYPE::MIPS){ + throw std::runtime_error( + "MIPS processor type not supported yet, please open a " + "GitHub issue to report that you want this feature!"); + } + } else { + x(static_cast( + c3d.readInt( + info.processorType(), file, ezc3d::DATA_TYPE::WORD)) + * info.scaleFactor()); + y(static_cast( + c3d.readInt( + info.processorType(), file, ezc3d::DATA_TYPE::WORD)) + * info.scaleFactor()); + z(static_cast( + c3d.readInt( + info.processorType(), file, ezc3d::DATA_TYPE::WORD)) + * info.scaleFactor()); + if (info.processorType() == PROCESSOR_TYPE::INTEL){ + cameraMask(c3d.readInt( + info.processorType(), file, ezc3d::DATA_TYPE::BYTE)); + residual(static_cast( + c3d.readInt(info.processorType(), + file, ezc3d::DATA_TYPE::BYTE)) + * info.scaleFactor()); + } + else if (info.processorType() == PROCESSOR_TYPE::DEC){ + cameraMask(c3d.readInt( + info.processorType(), file, ezc3d::DATA_TYPE::BYTE)); + residual(static_cast( + c3d.readInt(info.processorType(), + file, ezc3d::DATA_TYPE::BYTE)) + * info.scaleFactor()); + } + else if (info.processorType() == PROCESSOR_TYPE::MIPS){ + throw std::runtime_error( + "MIPS processor type not supported yet, please open a " + "GitHub issue to report that you want this feature!"); + } + } + if (residual() < 0){ + set(NAN, NAN, NAN); + } +} + void ezc3d::DataNS::Points3dNS::Point::print() const { ezc3d::Vector3d::print(); std::cout << "Residual = " << residual() << "; Masks = ["; @@ -35,7 +107,7 @@ void ezc3d::DataNS::Points3dNS::Point::print() const { if (_cameraMasks.size() > 0){ std::cout << _cameraMasks[_cameraMasks.size()-1] << "]"; } - std::cout << std::endl; + std::cout << "\n"; } void ezc3d::DataNS::Points3dNS::Point::write( diff --git a/src/Points.cpp b/src/Points.cpp index 959970dc..8dc0e0ef 100644 --- a/src/Points.cpp +++ b/src/Points.cpp @@ -8,6 +8,7 @@ /// #include "Points.h" +#include "Header.h" // Point3d data ezc3d::DataNS::Points3dNS::Points::Points() { @@ -18,6 +19,17 @@ ezc3d::DataNS::Points3dNS::Points::Points( _points.resize(nbPoints); } +ezc3d::DataNS::Points3dNS::Points::Points( + ezc3d::c3d &c3d, + std::fstream &file, + const ezc3d::DataNS::Points3dNS::Info& info) +{ + for (size_t i = 0; i < c3d.header().nb3dPoints(); ++i){ + ezc3d::DataNS::Points3dNS::Point pt(c3d, file, info); + point(pt, i); + } +} + void ezc3d::DataNS::Points3dNS::Points::print() const { for (size_t i = 0; i < nbPoints(); ++i) point(i).print(); diff --git a/src/PointsInfo.cpp b/src/PointsInfo.cpp new file mode 100644 index 00000000..9283f06e --- /dev/null +++ b/src/PointsInfo.cpp @@ -0,0 +1,37 @@ +#define EZC3D_API_EXPORTS +/// +/// \file PointsInfo.cpp +/// \brief Implementation of PointsInfo class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "PointsInfo.h" +#include "Header.h" +#include "Parameters.h" + + +ezc3d::DataNS::Points3dNS::Info::Info( + const ezc3d::c3d &c3d) : + _processorType(ezc3d::PROCESSOR_TYPE::INTEL), + _scaleFactor(-1) +{ + _processorType = c3d.parameters().processorType(); + + if (c3d.header().nb3dPoints()) + _scaleFactor = c3d.parameters() + .group("POINT").parameter("SCALE").valuesAsDouble()[0]; +} + +ezc3d::PROCESSOR_TYPE ezc3d::DataNS::Points3dNS::Info::processorType() const +{ + return _processorType; +} + +double ezc3d::DataNS::Points3dNS::Info::scaleFactor() const +{ + return _scaleFactor; +} + + diff --git a/src/Rotation.cpp b/src/Rotation.cpp new file mode 100644 index 00000000..ee19aaef --- /dev/null +++ b/src/Rotation.cpp @@ -0,0 +1,144 @@ +#define EZC3D_API_EXPORTS +/// +/// \file Rotation.cpp +/// \brief Implementation of Rotation class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "Rotation.h" +#include "RotationsInfo.h" + +#include + +ezc3d::DataNS::RotationNS::Rotation::Rotation() : + ezc3d::Matrix44(), + _reliability(-1) +{ + +} + +ezc3d::DataNS::RotationNS::Rotation::Rotation( + double elem00, double elem01, double elem02, double elem03, + double elem10, double elem11, double elem12, double elem13, + double elem20, double elem21, double elem22, double elem23, + double elem30, double elem31, double elem32, double elem33, + double reliability): + ezc3d::Matrix44(elem00, elem01, elem02, elem03, + elem10, elem11, elem12, elem13, + elem20, elem21, elem22, elem23, + elem30, elem31, elem32, elem33), + _reliability(reliability) +{ + +} + +ezc3d::DataNS::RotationNS::Rotation::Rotation( + ezc3d::c3d &c3d, + std::fstream &file, + const ezc3d::DataNS::RotationNS::Info &info) : + ezc3d::Matrix44() +{ + // Scale -1 is mandatory (Float) + double elem00 = c3d.readFloat(info.processorType(), file); + double elem10 = c3d.readFloat(info.processorType(), file); + double elem20 = c3d.readFloat(info.processorType(), file); + double elem30 = c3d.readFloat(info.processorType(), file); + double elem01 = c3d.readFloat(info.processorType(), file); + double elem11 = c3d.readFloat(info.processorType(), file); + double elem21 = c3d.readFloat(info.processorType(), file); + double elem31 = c3d.readFloat(info.processorType(), file); + double elem02 = c3d.readFloat(info.processorType(), file); + double elem12 = c3d.readFloat(info.processorType(), file); + double elem22 = c3d.readFloat(info.processorType(), file); + double elem32 = c3d.readFloat(info.processorType(), file); + double elem03 = c3d.readFloat(info.processorType(), file); + double elem13 = c3d.readFloat(info.processorType(), file); + double elem23 = c3d.readFloat(info.processorType(), file); + double elem33 = c3d.readFloat(info.processorType(), file); + set(elem00, elem01, elem02, elem03, + elem10, elem11, elem12, elem13, + elem20, elem21, elem22, elem23, + elem30, elem31, elem32, elem33); + _reliability = c3d.readFloat(info.processorType(), file); +} + +ezc3d::DataNS::RotationNS::Rotation::Rotation( + const ezc3d::DataNS::RotationNS::Rotation &r) : + ezc3d::Matrix44(r) +{ + reliability(r.reliability()); +} + +void ezc3d::DataNS::RotationNS::Rotation::print() const { + for (size_t i=0; i<_nbRows; ++i){ + for (size_t j=0; j<_nbCols; ++j){ + std::cout << operator ()(i, j); + if (j != _nbCols-1){ + std::cout << ", "; + } + } + std::cout << "\n"; + }; + std::cout << "Reliability = " << reliability() << "\n"; +} + +void ezc3d::DataNS::RotationNS::Rotation::write( + std::fstream &f) const { + for (size_t i = 0; i<16; ++i) { + float data(static_cast(_reliability < 0 ? NAN : _data[i])); + f.write(reinterpret_cast(&data), ezc3d::DATA_TYPE::FLOAT); + } + + float reliability(static_cast(_reliability)); + f.write(reinterpret_cast(&reliability), ezc3d::DATA_TYPE::FLOAT); +} + +void ezc3d::DataNS::RotationNS::Rotation::set( + double elem00, double elem01, double elem02, double elem03, + double elem10, double elem11, double elem12, double elem13, + double elem20, double elem21, double elem22, double elem23, + double elem30, double elem31, double elem32, double elem33, + double reliability) +{ + ezc3d::Matrix44::set( + elem00, elem01, elem02, elem03, + elem10, elem11, elem12, elem13, + elem20, elem21, elem22, elem23, + elem30, elem31, elem32, elem33); + _reliability = reliability; +} + +void ezc3d::DataNS::RotationNS::Rotation::set( + double elem00, double elem01, double elem02, double elem03, + double elem10, double elem11, double elem12, double elem13, + double elem20, double elem21, double elem22, double elem23, + double elem30, double elem31, double elem32, double elem33) +{ + ezc3d::Matrix44::set( + elem00, elem01, elem02, elem03, + elem10, elem11, elem12, elem13, + elem20, elem21, elem22, elem23, + elem30, elem31, elem32, elem33); + reliability(0); +} + +double ezc3d::DataNS::RotationNS::Rotation::reliability() const { + return _reliability; +} + +void ezc3d::DataNS::RotationNS::Rotation::reliability( + double reliability) { + _reliability = reliability; +} + +bool ezc3d::DataNS::RotationNS::Rotation::isValid() const +{ + return _reliability < 0 ? false : true; +} + +bool ezc3d::DataNS::RotationNS::Rotation::isEmpty() const +{ + return !isValid(); +} diff --git a/src/Rotations.cpp b/src/Rotations.cpp new file mode 100644 index 00000000..32721265 --- /dev/null +++ b/src/Rotations.cpp @@ -0,0 +1,112 @@ +#define EZC3D_API_EXPORTS +/// +/// \file Rotations.cpp +/// \brief Implementation of Rotations class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "Rotations.h" +#include "Header.h" +#include "Parameters.h" +#include "RotationsInfo.h" +#include "RotationsSubframe.h" + +// Rotations data +ezc3d::DataNS::RotationNS::Rotations::Rotations() +{ + +} + +ezc3d::DataNS::RotationNS::Rotations::Rotations( + ezc3d::c3d &c3d, + std::fstream &file, + const ezc3d::DataNS::RotationNS::Info& info) +{ + if (!c3d.header().hasRotationalData()) + return; + + size_t nbSubframes = info.ratio(); + for (size_t k = 0; k < nbSubframes; ++k){ + subframe(ezc3d::DataNS::RotationNS::SubFrame(c3d, file, info), k); + } +} + +void ezc3d::DataNS::RotationNS::Rotations::print() const { + for (size_t i = 0; i < nbSubframes(); ++i){ + std::cout << "Subframe = " << i << "\n"; + subframe(i).print(); + std::cout << "\n"; + } +} + +void ezc3d::DataNS::RotationNS::Rotations::write( + std::fstream &f) const { + for (size_t i = 0; i < nbSubframes(); ++i) { + subframe(i).write(f); + } +} + +size_t ezc3d::DataNS::RotationNS::Rotations::nbSubframes() const { + return _subframe.size(); +} + +void ezc3d::DataNS::RotationNS::Rotations::nbSubframes( + size_t nbSubframes) { + _subframe.resize(nbSubframes); +} + +const ezc3d::DataNS::RotationNS::SubFrame& +ezc3d::DataNS::RotationNS::Rotations::subframe( + size_t idx) const { + try { + return _subframe.at(idx); + } catch(std::out_of_range) { + throw std::out_of_range("Analogs::subframe method is trying to access the subframe " + + std::to_string(idx) + + " while the maximum number of subframes is " + + std::to_string(nbSubframes()) + "."); + } +} + +ezc3d::DataNS::RotationNS::SubFrame& +ezc3d::DataNS::RotationNS::Rotations::subframe( + size_t idx) { + try { + return _subframe.at(idx); + } catch(std::out_of_range) { + throw std::out_of_range("Analogs::subframe method is trying to access the subframe " + + std::to_string(idx) + + " while the maximum number of subframes is " + + std::to_string(nbSubframes()) + "."); + } +} + +void ezc3d::DataNS::RotationNS::Rotations::subframe( + const ezc3d::DataNS::RotationNS::SubFrame& subframe, + size_t idx) { + if (idx == SIZE_MAX) { + _subframe.push_back(subframe); + } + else { + if (idx >= nbSubframes()) { + _subframe.resize(idx+1); + } + _subframe[idx] = subframe; + } +} + +const std::vector& +ezc3d::DataNS::RotationNS::Rotations::subframes() const { + return _subframe; +} + +bool ezc3d::DataNS::RotationNS::Rotations::isEmpty() const { + for (SubFrame subframe : subframes()) { + if (!subframe.isEmpty()) { + return false; + } + } + return true; +} diff --git a/src/RotationsInfo.cpp b/src/RotationsInfo.cpp new file mode 100644 index 00000000..672e8457 --- /dev/null +++ b/src/RotationsInfo.cpp @@ -0,0 +1,74 @@ +#define EZC3D_API_EXPORTS +/// +/// \file RotationsInfo.cpp +/// \brief Implementation of RotationsInfo class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "RotationsInfo.h" +#include "Header.h" +#include "Parameters.h" + + +ezc3d::DataNS::RotationNS::Info::Info( + const ezc3d::c3d &c3d) +{ + if (!c3d.parameters().isGroup("ROTATION")){ + _hasGroup = false; + return; + } + _hasGroup = true; + + const ezc3d::ParametersNS::GroupNS::Group& group = + c3d.parameters().group("ROTATION"); + + // Do a sanity check before accessing + if (!group.isParameter("DATA_START")){ + throw std::runtime_error("DATA_START is not present in ROTATION."); + } + _dataStart = group.parameter("DATA_START").valuesAsInt()[0]; + + if (!group.isParameter("USED")){ + throw std::runtime_error("USED is not present in ROTATION."); + } + _used = group.parameter("USED").valuesAsInt()[0]; + + if (!group.isParameter("RATIO") && !group.isParameter("RATE")){ + throw std::runtime_error("RATIO or RATE must be present in ROTATION."); + } + _ratio = group.isParameter("RATIO") ? + group.parameter("RATIO").valuesAsInt()[0] : + group.parameter("RATE").valuesAsDouble()[0] / c3d.header().frameRate(); + + _processorType = c3d.parameters().processorType(); +} + +bool ezc3d::DataNS::RotationNS::Info::hasGroup() const +{ + return _hasGroup; +} + +size_t ezc3d::DataNS::RotationNS::Info::dataStart() const +{ + return _dataStart; +} + +size_t ezc3d::DataNS::RotationNS::Info::used() const +{ + return _used; +} + +size_t ezc3d::DataNS::RotationNS::Info::ratio() const +{ + return _ratio; +} + +ezc3d::PROCESSOR_TYPE ezc3d::DataNS::RotationNS::Info::processorType() const +{ + return _processorType; +} + + + diff --git a/src/RotationsSubframe.cpp b/src/RotationsSubframe.cpp new file mode 100644 index 00000000..48c851db --- /dev/null +++ b/src/RotationsSubframe.cpp @@ -0,0 +1,104 @@ +#define EZC3D_API_EXPORTS +/// +/// \file RotationsSubframe.cpp +/// \brief Implementation of Subframe class +/// \author Pariterre +/// \version 1.0 +/// \date October 17th, 2018 +/// + +#include "RotationsSubframe.h" +#include "Header.h" +#include "RotationsInfo.h" + +ezc3d::DataNS::RotationNS::SubFrame::SubFrame() { + +} + +ezc3d::DataNS::RotationNS::SubFrame::SubFrame( + ezc3d::c3d &c3d, + std::fstream &file, + const ezc3d::DataNS::RotationNS::Info &info) +{ + nbRotations(info.used()); + + // Read the rotations + for (size_t i = 0; i < nbRotations(); ++i){ + rotation(ezc3d::DataNS::RotationNS::Rotation(c3d, file, info), i); + } +} + +void ezc3d::DataNS::RotationNS::SubFrame::print() const { + for (size_t j = 0; j < nbRotations(); ++j){ + std::cout << "Rotation: " << j << "\n"; + rotation(j).print(); + } +} + +void ezc3d::DataNS::RotationNS::SubFrame::write( + std::fstream &f) const +{ + for (size_t i = 0; i < nbRotations(); ++i){ + rotation(i).write(f); + } +} + +size_t ezc3d::DataNS::RotationNS::SubFrame::nbRotations() const { + return _rotations.size(); +} + +void ezc3d::DataNS::RotationNS::SubFrame::nbRotations( + size_t nbRotations) { + _rotations.resize(nbRotations); +} + +const ezc3d::DataNS::RotationNS::Rotation& +ezc3d::DataNS::RotationNS::SubFrame::rotation(size_t idx) const { + try { + return _rotations.at(idx); + } catch(std::out_of_range) { + throw std::out_of_range( + "Subframe::rotation method is trying to access the rotation " + + std::to_string(idx) + + " while the maximum number of rotations is " + + std::to_string(nbRotations()) + "."); + } +} + +ezc3d::DataNS::RotationNS::Rotation& +ezc3d::DataNS::RotationNS::SubFrame::rotation( + size_t idx) { + try { + return _rotations.at(idx); + } catch(std::out_of_range) { + throw std::out_of_range( + "Subframe::rotation method is trying to access the rotation " + + std::to_string(idx) + + " while the maximum number of rotations is " + + std::to_string(nbRotations()) + "."); + } +} + +void ezc3d::DataNS::RotationNS::SubFrame::rotation( + const ezc3d::DataNS::RotationNS::Rotation &rotation, + size_t idx) { + if (idx == SIZE_MAX) + _rotations.push_back(rotation); + else{ + if (idx >= nbRotations()) + _rotations.resize(idx+1); + _rotations[idx] = rotation; + } +} + +const std::vector& +ezc3d::DataNS::RotationNS::SubFrame::rotations() const { + return _rotations; +} + +bool ezc3d::DataNS::RotationNS::SubFrame::isEmpty() const { + for (Rotation rotation : rotations()) + if (!rotation.isEmpty()) + return false; + return true; +} diff --git a/src/ezc3d.cpp b/src/ezc3d.cpp index 86a1217c..c3a1a31b 100644 --- a/src/ezc3d.cpp +++ b/src/ezc3d.cpp @@ -780,6 +780,9 @@ void ezc3d::c3d::updateHeader() { static_cast(parameters() .group("ANALOG").parameter("USED") .valuesAsInt()[0])); + + if (parameters().isGroup("ROTATION")) + _header->hasRotationalData(true); } void ezc3d::c3d::updateParameters( diff --git a/src/math/Matrix.cpp b/src/math/Matrix.cpp index 6314552a..77e7ca81 100644 --- a/src/math/Matrix.cpp +++ b/src/math/Matrix.cpp @@ -66,7 +66,7 @@ ezc3d::Matrix::Matrix( void ezc3d::Matrix::print() const { - std::cout << " Matrix = [" << std::endl; + std::cout << " Matrix = [" << "\n"; for (size_t i=0; i<_nbRows; ++i){ for (size_t j=0; j<_nbCols; ++j){ std::cout << operator ()(i, j); @@ -77,9 +77,17 @@ void ezc3d::Matrix::print() const if (i == _nbRows-1){ std::cout << "]"; } - std::cout << std::endl; + std::cout << "\n"; } - std::cout << std::endl; + std::cout << "\n"; +} + +double ezc3d::Matrix::sum() const +{ + double sum(0); + for (size_t i = 0; i < _data.size(); ++i) + sum += _data[i]; + return sum; } void ezc3d::Matrix::setZeros() @@ -370,7 +378,7 @@ std::ostream &operator<<( } } if (i < m.nbRows() - 1){ - out << std::endl; + out << "\n"; } } out << "]"; diff --git a/src/math/Matrix44.cpp b/src/math/Matrix44.cpp new file mode 100644 index 00000000..12118b2e --- /dev/null +++ b/src/math/Matrix44.cpp @@ -0,0 +1,137 @@ +#define EZC3D_API_EXPORTS +/// +/// \file Matrix44.cpp +/// \brief Implementation of Matrix44 class +/// \author Pariterre +/// \version 1.0 +/// \date April 30th, 2022 +/// + +#include "math/Matrix44.h" + +#include "math/Vector3d.h" + +ezc3d::Matrix44::Matrix44() : + ezc3d::Matrix(4, 4) +{ + +} + +ezc3d::Matrix44::Matrix44( + double elem00, double elem01, double elem02, double elem03, + double elem10, double elem11, double elem12, double elem13, + double elem20, double elem21, double elem22, double elem23, + double elem30, double elem31, double elem32, double elem33) : + ezc3d::Matrix(4, 4) +{ + _data[0] = elem00; + _data[1] = elem10; + _data[2] = elem20; + _data[3] = elem30; + _data[4] = elem01; + _data[5] = elem11; + _data[6] = elem21; + _data[7] = elem31; + _data[8] = elem02; + _data[9] = elem12; + _data[10] = elem22; + _data[11] = elem32; + _data[12] = elem03; + _data[13] = elem13; + _data[14] = elem23; + _data[15] = elem33; +} + +ezc3d::Matrix44::Matrix44( + const ezc3d::Matrix &other) : + ezc3d::Matrix(other) +{ +#ifndef USE_MATRIX_FAST_ACCESSOR + if (_nbRows != 4 || _nbCols != 4){ + throw std::runtime_error("Size of the matrix must be 4x4 to be casted" + "as a Matrix44"); + } +#endif +} + +size_t ezc3d::Matrix44::size() const +{ + return 16; +} + +size_t ezc3d::Matrix44::nbRows() const +{ + return 4; +} + +size_t ezc3d::Matrix44::nbCols() const +{ + return 4; +} + +void ezc3d::Matrix44::resize( + size_t, + size_t) +{ + throw std::runtime_error("Matrix44 cannot be resized"); +} + +void ezc3d::Matrix44::set( + double elem00, double elem01, double elem02, double elem03, + double elem10, double elem11, double elem12, double elem13, + double elem20, double elem21, double elem22, double elem23, + double elem30, double elem31, double elem32, double elem33) +{ + _data[0] = elem00; + _data[1] = elem10; + _data[2] = elem20; + _data[3] = elem30; + _data[4] = elem01; + _data[5] = elem11; + _data[6] = elem21; + _data[7] = elem31; + _data[8] = elem02; + _data[9] = elem12; + _data[10] = elem22; + _data[11] = elem32; + _data[12] = elem03; + _data[13] = elem13; + _data[14] = elem23; + _data[15] = elem33; +} + +ezc3d::Vector3d ezc3d::Matrix44::operator*( + const ezc3d::Vector3d& other) +{ + return ezc3d::Vector3d( + _data[0] * other._data[0] + _data[4] * other._data[1] + _data[8] * other._data[2] + _data[12], + _data[1] * other._data[0] + _data[5] * other._data[1] + _data[9] * other._data[2] + _data[13], + _data[2] * other._data[0] + _data[6] * other._data[1] + _data[10] * other._data[2] + _data[14] + ); +} + +ezc3d::Matrix44 ezc3d::Matrix44::operator*( + const ezc3d::Matrix44& other) +{ + return ezc3d::Matrix44( + _data[0] * other._data[0] + _data[4] * other._data[1] + _data[8] * other._data[2] + _data[12] * other._data[3], + _data[0] * other._data[4] + _data[4] * other._data[5] + _data[8] * other._data[6] + _data[12] * other._data[7], + _data[0] * other._data[8] + _data[4] * other._data[9] + _data[8] * other._data[10] + _data[12] * other._data[11], + _data[0] * other._data[12] + _data[4] * other._data[13] + _data[8] * other._data[14] + _data[12] * other._data[15], + + _data[1] * other._data[0] + _data[5] * other._data[1] + _data[9] * other._data[2] + _data[13] * other._data[3], + _data[1] * other._data[4] + _data[5] * other._data[5] + _data[9] * other._data[6] + _data[13] * other._data[7], + _data[1] * other._data[8] + _data[5] * other._data[9] + _data[9] * other._data[10] + _data[13] * other._data[11], + _data[1] * other._data[12] + _data[5] * other._data[13] + _data[9] * other._data[14] + _data[13] * other._data[15], + + _data[2] * other._data[0] + _data[6] * other._data[1] + _data[10] * other._data[2] + _data[14] * other._data[3], + _data[2] * other._data[4] + _data[6] * other._data[5] + _data[10] * other._data[6] + _data[14] * other._data[7], + _data[2] * other._data[8] + _data[6] * other._data[9] + _data[10] * other._data[10] + _data[14] * other._data[11], + _data[2] * other._data[12] + _data[6] * other._data[13] + _data[10] * other._data[14] + _data[14] * other._data[15], + + _data[3] * other._data[0] + _data[7] * other._data[1] + _data[11] * other._data[2] + _data[15] * other._data[3], + _data[3] * other._data[4] + _data[7] * other._data[5] + _data[11] * other._data[6] + _data[15] * other._data[7], + _data[3] * other._data[8] + _data[7] * other._data[9] + _data[11] * other._data[10] + _data[15] * other._data[11], + _data[3] * other._data[12] + _data[7] * other._data[13] + _data[11] * other._data[14] + _data[15] * other._data[15] + ); +} diff --git a/src/math/Vector3d.cpp b/src/math/Vector3d.cpp index b5a0d7e0..b0514015 100644 --- a/src/math/Vector3d.cpp +++ b/src/math/Vector3d.cpp @@ -42,7 +42,7 @@ void ezc3d::Vector3d::print() const << x() << ", " << y() << ", " << z() << "];" - << std::endl; + << "\n"; } void ezc3d::Vector3d::resize( diff --git a/src/math/Vector6d.cpp b/src/math/Vector6d.cpp index 41271a14..17bfa294 100644 --- a/src/math/Vector6d.cpp +++ b/src/math/Vector6d.cpp @@ -53,7 +53,7 @@ void ezc3d::Vector6d::print() const << _data[3] << ", " << _data[4] << ", " << _data[5] << "];" - << std::endl; + << "\n"; } ezc3d::Vector6d& ezc3d::Vector6d::operator=( diff --git a/test/python3/test_binder_python.py b/test/python3/test_binder_python.py index 28694a2d..179e753e 100644 --- a/test/python3/test_binder_python.py +++ b/test/python3/test_binder_python.py @@ -460,6 +460,26 @@ def test_force_platform_filter(): np.testing.assert_array_almost_equal(all_pf[1]["Tz"][:, [0, 1000, -1]], expected_Tz, decimal=3) +def test_rotations(): + c3d = ezc3d.c3d("test/c3dTestFiles/C3DRotationExample.c3d") + array = c3d["data"]["rotations"] + decimal = 6 + + np.testing.assert_array_equal(x=array.shape, y=(4, 4, 21, 340), err_msg="Shape does not match") + raveled = array.ravel() + np.testing.assert_array_almost_equal( + x=array[2, 3, 2, 5], + y=931.6382446289062, + decimal=decimal, + ) + np.testing.assert_array_almost_equal( + x=raveled[-1], + y=1.0, + decimal=decimal, + ) + np.testing.assert_array_almost_equal(x=np.nansum(array), y=9367125.137371363, decimal=decimal) + + @pytest.fixture(scope="module", params=["BTS", "Optotrak", "Qualisys", "Vicon", "Label2"]) def c3d_build_rebuild_all(request): base_folder = Path("test/c3dTestFiles") @@ -475,7 +495,7 @@ def c3d_build_rebuild_all(request): Path.unlink(rebuild_file) -@pytest.fixture(scope="module", params=["BTS", "Optotrak", "Qualisys", "Vicon"]) +@pytest.fixture(scope="module", params=["BTS", "Optotrak", "Qualisys", "Vicon", "C3DRotationExample"]) def c3d_build_rebuild_reduced(request): base_folder = Path("test/c3dTestFiles") orig_file = Path(base_folder / (request.param + ".c3d")) diff --git a/test/test_ezc3d.cpp b/test/test_ezc3d.cpp index b351abc7..383e04c3 100644 --- a/test/test_ezc3d.cpp +++ b/test/test_ezc3d.cpp @@ -297,6 +297,14 @@ void defaultParametersTest(const ezc3d::c3d& new_c3d, PARAMETER_TYPE type){ } } +void testPrintingCall(const ezc3d::c3d& c3d){ + std::streambuf *old = std::cout.rdbuf(); // Save cout direction + std::stringstream ss; // create a redirection + std::cout.rdbuf (ss.rdbuf()); // <-- redirect to null + c3d.print(); + std::cout.rdbuf (old); // <-- restore the old direction +} + TEST(String, unittest){ EXPECT_STREQ(ezc3d::toUpper("toUpper").c_str(), "TOUPPER"); } @@ -1672,6 +1680,108 @@ TEST(c3dFileIO, CreateWriteAndReadBackWithNan){ EXPECT_TRUE(std::isnan(channel.data())); } +TEST(c3dFileIO, readC3DWithRotation){ + ezc3d::c3d c3d("c3dTestFiles/C3DRotationExample.c3d"); + + // Header test + // Point stuff + EXPECT_EQ(c3d.header().nb3dPoints(), 0); + EXPECT_EQ(c3d.header().nbMaxInterpGap(), 10); + EXPECT_FLOAT_EQ(c3d.header().scaleFactor(), static_cast(-1)); + EXPECT_FLOAT_EQ(c3d.header().frameRate(), 85); + + // Analog stuff + EXPECT_EQ(c3d.header().nbAnalogsMeasurement(), 0); + EXPECT_EQ(c3d.header().nbAnalogByFrame(), 0); + EXPECT_EQ(c3d.header().nbAnalogs(), 0); + + // Parameter tests + EXPECT_EQ(c3d.parameters().checksum(), 80); + EXPECT_EQ(c3d.parameters().nbGroups(), 9); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("USED").type(), ezc3d::INT); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("USED").valuesAsInt().size(), 1); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("USED").valuesAsInt()[0], 21); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("DATA_START").type(), ezc3d::INT); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("DATA_START").valuesAsInt().size(), 1); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("DATA_START").valuesAsInt()[0], 6); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("RATIO").type(), ezc3d::INT); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("RATIO").valuesAsInt().size(), 1); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("RATIO").valuesAsInt()[0], 1); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("LABELS").type(), ezc3d::CHAR); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("LABELS").valuesAsString().size(), 21); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("DESCRIPTIONS").type(), ezc3d::CHAR); + EXPECT_EQ(c3d.parameters().group("ROTATION").parameter("DESCRIPTIONS").valuesAsString().size(), 21); + + // DATA + EXPECT_EQ(c3d.data().nbFrames(), 340); + EXPECT_EQ(c3d.data().frame(0).rotations().subframe(0).nbRotations(), 21); + + // Test some values randomly + EXPECT_FLOAT_EQ(c3d.data().frame(5).rotations().subframe(0).rotation(2)(2, 3), 931.63824); + + // Test sum of all values + double sumValues(0); + for (const auto& frame : c3d.data().frames()){ + for (const auto& subframe : frame.rotations().subframes()){ + for (const auto& rot : subframe.rotations()){ + if (rot.isValid()){ + sumValues += rot.sum(); + } + } + } + } + EXPECT_FLOAT_EQ(sumValues, 9367047.137371358); + + // Test printing of rotations + testPrintingCall(c3d); + + // Write the file and read it back to test the rotations structure + std::string savePath("temporary.c3d"); + c3d.write(savePath); + ezc3d::c3d c3dCopy(savePath); + remove(savePath.c_str()); + + // Header test + + EXPECT_EQ(c3dCopy.header().nb3dPoints(), 0); + EXPECT_EQ(c3dCopy.header().nbMaxInterpGap(), 10); + EXPECT_FLOAT_EQ(c3dCopy.header().scaleFactor(), static_cast(-1)); + EXPECT_FLOAT_EQ(c3dCopy.header().frameRate(), 85); + + // Analog stuff + EXPECT_EQ(c3dCopy.header().nbAnalogsMeasurement(), 0); + EXPECT_EQ(c3dCopy.header().nbAnalogByFrame(), 0); + EXPECT_EQ(c3dCopy.header().nbAnalogs(), 0); + + // Parameter tests + EXPECT_EQ(c3dCopy.parameters().checksum(), 80); + EXPECT_EQ(c3dCopy.parameters().nbGroups(), 10); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("USED").type(), ezc3d::INT); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("USED").valuesAsInt().size(), 1); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("USED").valuesAsInt()[0], 21); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("DATA_START").type(), ezc3d::INT); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("DATA_START").valuesAsInt().size(), 1); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("DATA_START").valuesAsInt()[0], 6); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("RATIO").type(), ezc3d::INT); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("RATIO").valuesAsInt().size(), 1); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("RATIO").valuesAsInt()[0], 1); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("LABELS").type(), ezc3d::CHAR); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("LABELS").valuesAsString().size(), 21); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("DESCRIPTIONS").type(), ezc3d::CHAR); + EXPECT_EQ(c3dCopy.parameters().group("ROTATION").parameter("DESCRIPTIONS").valuesAsString().size(), 21); + + double sumValuesCopy(0); + for (const auto& frame : c3dCopy.data().frames()){ + for (const auto& subframe : frame.rotations().subframes()){ + for (const auto& rot : subframe.rotations()){ + if (rot.isValid()){ + sumValuesCopy += rot.sum(); + } + } + } + } + EXPECT_FLOAT_EQ(sumValues, sumValuesCopy); +} TEST(c3dFileIO, readViconC3D){ ezc3d::c3d Vicon("c3dTestFiles/Vicon.c3d"); @@ -2252,5 +2362,5 @@ TEST(c3dShow, printIt){ c3dTestStruct new_c3d; fillC3D(new_c3d, true, true); - EXPECT_NO_THROW(new_c3d.c3d.print()); + EXPECT_NO_THROW(testPrintingCall(new_c3d.c3d)); } diff --git a/test/test_math.cpp b/test/test_math.cpp index 7c0a37ad..509e23a3 100644 --- a/test/test_math.cpp +++ b/test/test_math.cpp @@ -3,6 +3,15 @@ #include "ezc3d_all.h" +void testPrintingCall(const ezc3d::Matrix& m){ + std::streambuf *old = std::cout.rdbuf(); // Save cout direction + std::stringstream ss; // create a redirection + std::cout.rdbuf (ss.rdbuf()); // <-- redirect to null + m.print(); + std::cout << "Another way to print : " << "\n" << m << "\n"; + std::cout.rdbuf (old); // <-- restore the old direction +} + TEST(Matrix, create){ ezc3d::Matrix m1(2, 3); EXPECT_EQ(m1.nbRows(), 2); @@ -15,8 +24,8 @@ TEST(Matrix, create){ m1(1,0) = 4.8; m1(1,1) = 6.0; m1(1,2) = 7.2; - m1.print(); - std::cout << "Another way to print : " << std::endl << m1 << std::endl; + + testPrintingCall(m1); EXPECT_EQ(m1(0,0), 1.2); EXPECT_EQ(m1(0,1), 2.4); @@ -133,6 +142,9 @@ TEST(Matrix, unittest){ m3(2,0) = 11.1; m3(2,1) = 12.2; + // Sum of all elements + EXPECT_DOUBLE_EQ(m1.sum(), 23.1); + // Transpose ezc3d::Matrix m_transp(m1.T()); EXPECT_EQ(m_transp.nbRows(), 3); @@ -322,6 +334,122 @@ TEST(Matrix33, unittest){ #endif } +TEST(Matrix44, unittest){ + ezc3d::Matrix44 m_toFill; + EXPECT_EQ(m_toFill.nbRows(), 4); + EXPECT_EQ(m_toFill.nbCols(), 4); + EXPECT_EQ(m_toFill.size(), 16); + EXPECT_THROW(m_toFill.resize(0, 0), std::runtime_error); + m_toFill(0, 0) = 2.; + m_toFill(0, 1) = 3.; + m_toFill(0, 2) = 4.; + m_toFill(0, 3) = 5.; + m_toFill(1, 0) = 6.; + m_toFill(1, 1) = 7.; + m_toFill(1, 2) = 8.; + m_toFill(1, 3) = 9.; + m_toFill(2, 0) = 10.; + m_toFill(2, 1) = 11.; + m_toFill(2, 2) = 12.; + m_toFill(2, 3) = 13.; + m_toFill(3, 0) = 14.; + m_toFill(3, 1) = 15.; + m_toFill(3, 2) = 16.; + m_toFill(3, 3) = 17.; + EXPECT_DOUBLE_EQ(m_toFill(0, 0), 2.); + EXPECT_DOUBLE_EQ(m_toFill(0, 1), 3.); + EXPECT_DOUBLE_EQ(m_toFill(0, 2), 4.); + EXPECT_DOUBLE_EQ(m_toFill(0, 3), 5.); + EXPECT_DOUBLE_EQ(m_toFill(1, 0), 6.); + EXPECT_DOUBLE_EQ(m_toFill(1, 1), 7.); + EXPECT_DOUBLE_EQ(m_toFill(1, 2), 8.); + EXPECT_DOUBLE_EQ(m_toFill(1, 3), 9.); + EXPECT_DOUBLE_EQ(m_toFill(2, 0), 10.); + EXPECT_DOUBLE_EQ(m_toFill(2, 1), 11.); + EXPECT_DOUBLE_EQ(m_toFill(2, 2), 12.); + EXPECT_DOUBLE_EQ(m_toFill(2, 3), 13.); + EXPECT_DOUBLE_EQ(m_toFill(3, 0), 14.); + EXPECT_DOUBLE_EQ(m_toFill(3, 1), 15.); + EXPECT_DOUBLE_EQ(m_toFill(3, 2), 16.); + EXPECT_DOUBLE_EQ(m_toFill(3, 3), 17.); + + ezc3d::Matrix44 m1( + 1., 2., 3., 4., + 5., 6., 7., 8., + 9., 10., 11., 12., + 13., 14., 15., 16.); + EXPECT_EQ(m1.nbRows(), 4); + EXPECT_EQ(m1.nbCols(), 4); + EXPECT_EQ(m1.size(), 16); + + EXPECT_DOUBLE_EQ(m1(0, 0), 1.); + EXPECT_DOUBLE_EQ(m1(0, 1), 2.); + EXPECT_DOUBLE_EQ(m1(0, 2), 3.); + EXPECT_DOUBLE_EQ(m1(0, 3), 4.); + EXPECT_DOUBLE_EQ(m1(1, 0), 5.); + EXPECT_DOUBLE_EQ(m1(1, 1), 6.); + EXPECT_DOUBLE_EQ(m1(1, 2), 7.); + EXPECT_DOUBLE_EQ(m1(1, 3), 8.); + EXPECT_DOUBLE_EQ(m1(2, 0), 9.); + EXPECT_DOUBLE_EQ(m1(2, 1), 10.); + EXPECT_DOUBLE_EQ(m1(2, 2), 11.); + EXPECT_DOUBLE_EQ(m1(2, 3), 12.); + EXPECT_DOUBLE_EQ(m1(3, 0), 13.); + EXPECT_DOUBLE_EQ(m1(3, 1), 14.); + EXPECT_DOUBLE_EQ(m1(3, 2), 15.); + EXPECT_DOUBLE_EQ(m1(3, 3), 16.); + + ezc3d::Matrix44 m1_copy(m1); + EXPECT_EQ(m1_copy.nbRows(), 4); + EXPECT_EQ(m1_copy.nbCols(), 4); + EXPECT_EQ(m1_copy.size(), 16); + EXPECT_DOUBLE_EQ(m1_copy(0, 0), 1.); + EXPECT_DOUBLE_EQ(m1_copy(0, 1), 2.); + EXPECT_DOUBLE_EQ(m1_copy(0, 2), 3.); + EXPECT_DOUBLE_EQ(m1_copy(0, 3), 4.); + EXPECT_DOUBLE_EQ(m1_copy(1, 0), 5.); + EXPECT_DOUBLE_EQ(m1_copy(1, 1), 6.); + EXPECT_DOUBLE_EQ(m1_copy(1, 2), 7.); + EXPECT_DOUBLE_EQ(m1_copy(1, 3), 8.); + EXPECT_DOUBLE_EQ(m1_copy(2, 0), 9.); + EXPECT_DOUBLE_EQ(m1_copy(2, 1), 10.); + EXPECT_DOUBLE_EQ(m1_copy(2, 2), 11.); + EXPECT_DOUBLE_EQ(m1_copy(2, 3), 12.); + EXPECT_DOUBLE_EQ(m1_copy(3, 0), 13.); + EXPECT_DOUBLE_EQ(m1_copy(3, 1), 14.); + EXPECT_DOUBLE_EQ(m1_copy(3, 2), 15.); + EXPECT_DOUBLE_EQ(m1_copy(3, 3), 16.); + + ezc3d::Matrix44 m_fromMult(m1 * m_toFill); + EXPECT_DOUBLE_EQ(m_fromMult(0, 0), 100.); + EXPECT_DOUBLE_EQ(m_fromMult(0, 1), 110.); + EXPECT_DOUBLE_EQ(m_fromMult(0, 2), 120.); + EXPECT_DOUBLE_EQ(m_fromMult(0, 3), 130.); + EXPECT_DOUBLE_EQ(m_fromMult(1, 0), 228.); + EXPECT_DOUBLE_EQ(m_fromMult(1, 1), 254.); + EXPECT_DOUBLE_EQ(m_fromMult(1, 2), 280.); + EXPECT_DOUBLE_EQ(m_fromMult(1, 3), 306.); + EXPECT_DOUBLE_EQ(m_fromMult(2, 0), 356.); + EXPECT_DOUBLE_EQ(m_fromMult(2, 1), 398.); + EXPECT_DOUBLE_EQ(m_fromMult(2, 2), 440.); + EXPECT_DOUBLE_EQ(m_fromMult(2, 3), 482.); + EXPECT_DOUBLE_EQ(m_fromMult(3, 0), 484.); + EXPECT_DOUBLE_EQ(m_fromMult(3, 1), 542.); + EXPECT_DOUBLE_EQ(m_fromMult(3, 2), 600.); + EXPECT_DOUBLE_EQ(m_fromMult(3, 3), 658.); + + ezc3d::Vector3d vec(3, 4, 5); + ezc3d::Vector3d v_fromMult(m_toFill * vec); + EXPECT_DOUBLE_EQ(v_fromMult(0), 43); + EXPECT_DOUBLE_EQ(v_fromMult(1), 95); + EXPECT_DOUBLE_EQ(v_fromMult(2), 147); + +#ifndef USE_MATRIX_FAST_ACCESSOR + EXPECT_THROW(ezc3d::Matrix44(ezc3d::Matrix(2, 4)), std::runtime_error); + EXPECT_THROW(ezc3d::Matrix44(ezc3d::Matrix(4, 2)), std::runtime_error); +#endif +} + TEST(Matrix66, unittest){ ezc3d::Matrix66 m_toFill; EXPECT_EQ(m_toFill.nbRows(), 6); @@ -559,7 +687,7 @@ TEST(Vector6d, unittest){ EXPECT_EQ(random.size(), 6); EXPECT_THROW(random.resize(0, 0), std::runtime_error); - random.print(); + testPrintingCall(random); #ifndef USE_MATRIX_FAST_ACCESSOR EXPECT_THROW(random(6), std::runtime_error); EXPECT_THROW(random(6) = 0, std::runtime_error);