Merge pull request #241 from pariterre/dev

Added support for Rotation (C++ and Python)

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
 )
 

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));

binding/ezc3d.i

@@ -27,7 +27,8 @@ namespace std {
     %template(VecPoints) vector<ezc3d::DataNS::Points3dNS::Point>;
     %template(VecAnalogSubFrames) vector<ezc3d::DataNS::AnalogsNS::SubFrame>;
     %template(VecAnalogChannels) vector<ezc3d::DataNS::AnalogsNS::Channel>;
-
+    %template(VecRotationSubFrames) vector<ezc3d::DataNS::RotationNS::SubFrame>;
+    %template(VecRotations) vector<ezc3d::DataNS::RotationNS::Rotation>;
 }
 
 // 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 {

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,16 +615,20 @@ 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()
         for i in range(nb_analog_subframes):
             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
+
+