pupyC3D.c3dfile

  1# pupyC3D (c) by Antoine MARIN antoine.marin@univ-rennes2.fr
  2#
  3# pupyC3D is licensed under a
  4# Creative Commons Attribution-NonCommercial 4.0 International License.
  5#
  6# You should have received a copy of the license along with this
  7# work. If not, see <https://creativecommons.org/licenses/by-nc/4.0/>.
  8
  9import os
 10import warnings
 11import struct
 12import math
 13import numpy as np
 14from .decoder import *
 15
 16class Metadata:
 17    """Base class for Parameters and ParameterGroups.
 18    
 19    Handles common functionality for C3D metadata elements including
 20    name, description, and group ID management.
 21    """
 22
 23    def __init__(self, name:str, group_id:int):
 24        """Initialize metadata object.
 25        
 26        Args:
 27            name (str): Name of the metadata element
 28            group_id (int): ID of the parameter group
 29        """
 30        self.name = name
 31        self.description = ''
 32        self.group_id = group_id
 33
 34    def read_from_buffer(self, buffer:ProcStream):
 35        """Read metadata from binary buffer.
 36        
 37        Args:
 38            buffer (ProcStream): Binary data stream
 39            
 40        Returns:
 41            int: Number of bytes read
 42        """
 43        return 0
 44
 45    def write_to_buffer(self, buffer: ProcStream):
 46        """Write metadata to binary buffer.
 47        
 48        Args:
 49            buffer (ProcStream): Binary data stream to write to
 50        """
 51        buffer.write_int8(len(self.name))
 52        buffer.write_int8(self.group_id)
 53        buffer.write_string(self.name)
 54
 55    def _get_offset(self):
 56        """Calculate byte offset for metadata structure.
 57        
 58        Returns:
 59            int: Offset in bytes
 60        """
 61        return 2 + len(self.description) + 1
 62
 63    def get_size(self):
 64        """Calculate total size of metadata structure.
 65        
 66        Returns:
 67            int: Total size in bytes
 68        """
 69        return 2 + len(self.name) + self._get_offset()
 70
 71
 72class Parameter(Metadata):
 73    """Represents a C3D parameter with typed data.
 74    
 75    Parameters can store scalar values or multi-dimensional arrays
 76    of various types (int8, uint16, float, string).
 77    """
 78
 79    def __init__(self, name:str, group_id:int):
 80        """Initialize parameter.
 81        
 82        Args:
 83            name (str): Parameter name
 84            group_id (int): ID of the parameter group
 85        """
 86        super(Parameter, self).__init__(name, group_id)
 87        self.data_type = 0
 88        self.value = None
 89
 90    def read_from_buffer(self, buffer:ProcStream):
 91        """Read parameter data from binary buffer.
 92        
 93        Supports scalar and multi-dimensional array data of types:
 94        - int8 (data_type=1)
 95        - uint16 (data_type=2) 
 96        - float (data_type=4)
 97        - string (data_type=-1)
 98        
 99        Args:
100            buffer (ProcStream): Binary data stream
101            
102        Returns:
103            int: Number of bytes read
104        """
105        offset = 0
106        self.data_type = buffer.get_int8()
107        offset += 1
108        n_dim = buffer.get_int8()
109        offset += 1
110        if n_dim == 0:
111            if self.data_type == 1:
112                self.value = buffer.get_int8()
113            elif self.data_type == 2:
114                self.value = buffer.get_uint16()
115            elif self.data_type == 4:
116                self.value = buffer.get_float()
117            data_size = abs(self.data_type)
118        else:
119            dims = []
120            for i in range(n_dim):
121                dims.append(buffer.get_uint8())
122                offset += 1
123            prod = math.prod(dims[:n_dim])
124            if self.data_type == -1:
125                if len(dims) >= 2:
126                    row = 1
127                    inc2 = 1
128                    while inc2 < n_dim:
129                        row *= dims[inc2]
130                        inc2 += 1
131                    data = []
132                    for i in range(row):
133                        data.append(buffer.get_string(dims[0]).strip())
134                    data = np.array(data)
135                    if data.size == 0:
136                        data = np.empty((dims[:]), str)
137                else:
138                    data = np.array([buffer.get_string(prod).strip()])
139            elif self.data_type == 1:
140                data = np.array([buffer.get_int8() for _ in range(prod)]).reshape(dims)
141            elif self.data_type == 2:
142                data = np.array([buffer.get_uint16() for _ in range(prod)]).reshape(dims)
143            elif self.data_type == 4:
144                data = np.array([buffer.get_float() for _ in range(prod)]).reshape(dims)
145            else:
146                data = np.array([])
147
148            self.value = data
149            data_size = prod * abs(self.data_type)
150        offset += data_size
151        desc_len = buffer.get_uint8()
152        self.description = buffer.get_string(desc_len)
153        offset += desc_len
154        return offset
155
156    def write_to_buffer(self, buffer: ProcStream, last_entry=False):
157        super(Parameter, self).write_to_buffer(buffer)
158        if last_entry:
159            offset = 0
160        else:
161            offset = self._get_offset()
162        buffer.write_uint16(offset)
163        buffer.write_int8(self.data_type)
164
165        dims = self.__get_dim()
166        n_dim = len(dims)
167        buffer.write_int8(n_dim)
168        if n_dim == 0:
169            if self.data_type == 1:
170                buffer.write_int8(self.value)
171            elif self.data_type == 2:
172                buffer.write_uint16(self.value)
173            elif self.data_type == 4:
174                buffer.write_float(self.value)
175        else:
176            for each in dims:
177                buffer.write_uint8(each)
178            data = self.value.flatten()
179            for each in data:
180                if self.data_type == 1:
181                    buffer.write_int8(each)
182                elif self.data_type == 2:
183                    buffer.write_uint16(each)
184                elif self.data_type == 4:
185                    buffer.write_float(each)
186                elif self.data_type == -1:
187                    buffer.write_string(each.ljust(dims[0]))
188
189        buffer.write_uint8(len(self.description))
190        buffer.write_string(self.description)
191
192    def _get_offset(self):
193        offset = 2 + 1 + 1 #offset, data_type, dim
194        if not isinstance(self.value, np.ndarray):
195            offset += self.data_type#value
196        else:
197            # n_dimension
198            dims = self.__get_dim()
199            n_dim = len(dims)
200            offset += n_dim
201            # data size
202            prod = math.prod(dims[:n_dim])
203            data_size = prod * abs(self.data_type)
204            offset += data_size
205
206        offset +=1 # desc length
207        offset += len(self.description) #desc
208        return offset
209
210    def __get_dim(self):
211        if not isinstance(self.value, np.ndarray):
212            dims  = []
213        else:
214            dims = self.value.shape
215            if self.data_type == -1:
216                if dims[0] == 1:
217                    dims = [len(self.value[0])]
218                else:
219                    if self.value.size > 0:
220                        d1 = max([len(x) for x in self.value])
221                        dims = [d1, dims[0]]
222        return dims
223
224
225class ParameterGroup(Metadata):
226    """Represents a group of related C3D parameters.
227    
228    Parameter groups organize parameters by functionality
229    (e.g., POINT, ANALOG, TRIAL groups).
230    """
231
232    def __init__(self, name:str, group_id:int):
233        """Initialize parameter group.
234        
235        Args:
236            name (str): Group name
237            group_id (int): Unique group identifier
238        """
239        super(ParameterGroup, self).__init__(name, group_id)
240        self.parameters = dict()
241
242    def add_parameter(self, name)->Parameter:
243        """Add a new parameter to this group.
244        
245        Args:
246            name (str): Parameter name
247            
248        Returns:
249            Parameter: New or existing parameter
250        """
251        if name not in self.parameters:
252            param = Parameter(name, -self.group_id)
253            self.parameters[param.name] = param
254            return param
255        warnings.warn('Parameter %s already exists' %name)
256        return self.parameters[name]
257
258    def remove_parameter(self, name):
259        """Remove a parameter from this group.
260        
261        Args:
262            name (str): Parameter name to remove
263            
264        Returns:
265            bool: True if parameter was removed, False if not found
266        """
267        if name in self.parameters:
268            self.parameters.pop(name)
269            return True
270        return False
271
272    def get_parameter(self, name):
273        """Get a parameter by name.
274        
275        Args:
276            name (str): Parameter name
277            
278        Returns:
279            Parameter or None: Parameter object if found
280        """
281        if name not in self.parameters:
282            return None
283        return self.parameters[name]
284
285    def read_from_buffer(self, buffer:ProcStream):
286        desc_len = buffer.get_uint8()
287        offset = 1
288        desc = buffer.get_string(desc_len)
289        offset += desc_len
290        self.description = desc
291        return offset
292
293    def write_to_buffer(self, buffer: ProcStream, last_entry=False):
294        super(ParameterGroup, self).write_to_buffer(buffer)
295        # offset = 2 + len(self.description) + 1
296        offset = self._get_offset()
297        buffer.write_uint16(offset)
298        buffer.write_uint8(len(self.description))
299        buffer.write_string(self.description)
300
301
302class C3DFile:
303    """Main class for reading and writing C3D files.
304    
305    C3D files contain 3D coordinate data, parameters, and metadata
306    commonly used in biomechanics and motion capture applications.
307    
308    Attributes:
309        filename (str): Path to the C3D file
310        header (dict): File header information
311        groups (dict): Parameter groups indexed by group ID
312        data (dict): Point and analog data
313    """
314
315    def __init__(self, filename:str=''):
316        """Initialize C3DFile object.
317        
318        Args:
319            filename (str, optional): Path to C3D file. If file exists,
320                it will be automatically loaded.
321        """
322        self.filename = filename
323        self.__decoder = None
324        self.header = dict()
325        self.groups = dict()
326        self.data = dict()
327        if os.path.exists(filename):
328            self.read_file()
329
330    def read_file(self):
331        """
332        Read the C3D file associated with self.filename
333        """
334        if os.path.exists(self.filename):
335            with open(self.filename, 'rb') as handle:
336                self.__read_header(handle)
337                self.__read_parameters(handle)
338                self.__read_data(handle)
339        else:
340            raise FileNotFoundError(self.filename)
341
342    def write(self, filename:str='', **kwargs):
343        """Write C3D data to file.
344        
345        Args:
346            filename (str, optional): Output file path. Defaults to self.filename.
347            **kwargs: Additional options:
348                overwrite (bool): Allow overwriting existing file. Defaults to False.
349                
350        Raises:
351            Warning: If file exists and overwrite=False
352        """
353        overwrite = kwargs.get('overwrite', False)
354        if filename == '':
355            filename = self.filename
356        if os.path.exists(filename) and not overwrite:
357            warnings.warn('File %s already exist. If you wish to overwrite it set ''overwrite'' argument to True' %filename)
358            return
359        with open(filename, 'wb') as handle:
360            self.__write_header(handle)
361            self.__write_parameters(handle)
362            self.__write_data(handle)
363
364    def add_parameter_group(self, name, gid=0)->ParameterGroup:
365        """Add a new parameter group.
366        
367        Args:
368            name (str): Group name
369            gid (int, optional): Group ID. If 0, auto-assigned. Defaults to 0.
370            
371        Returns:
372            ParameterGroup or None: New group if created, None if already exists
373        """
374        assert(gid <= 0)
375        if gid == 0:
376            g = self.get_parameter_group(name)
377        else:
378            g = self.get_parameter_group(gid)
379        if g is not None:
380            return None
381        if gid == 0:
382            ids = list(self.groups.keys())
383            gid = ids[0]
384            # find first missing id
385            for number in ids:
386                if number != gid:
387                    break
388                gid -= 1
389        self.groups[gid] = ParameterGroup(name, gid)
390        return self.groups[gid]
391
392    def remove_parameter_group(self, group)->bool:
393        """Remove a parameter group.
394        
395        Args:
396            group (int or str): Group ID or name
397            
398        Returns:
399            bool: True if group was removed, False if not found
400        """
401        g = self.get_parameter_group(group)
402        if g is not None:
403            self.groups.pop(g.group_id)
404            return True
405        return False
406
407    def get_parameter_group(self, group_id)->ParameterGroup:
408        """Get a parameter group by ID or name.
409        
410        Args:
411            group_id (int or str): Group ID or name
412            
413        Returns:
414            ParameterGroup or None: Parameter group if found
415            
416        Raises:
417            TypeError: If group_id is neither int nor str
418        """
419        if isinstance(group_id, int):
420            return self.groups.get(group_id, None)
421        elif isinstance(group_id, str):
422            g = {v.name: v for v in self.groups.values()}
423            return g.get(group_id, None)
424        else:
425            raise TypeError('Argument ''group_id'' should be either str or int')
426
427    def add_parameter(self, name, gid):
428        group = self.get_parameter_group(gid)
429        if group is not None:
430            param = group.add_parameter(name)
431            return param
432        return None
433
434    def remove_parameter(self, name, group_id):
435        g = self.get_parameter_group(group_id)
436        if g is not None:
437            return g.remove_parameter(name)
438        return False
439
440    def get_parameter(self, group_id, param_name):
441        g = self.get_parameter_group(group_id)
442        if g is not None:
443            return g.parameters.get(param_name, None)
444        return None
445
446    def get_point_data(self, name: str):
447        """
448        Return trajectory of a point
449        :param name: the point name in string
450        :return: trajectory as np.ndarray (NbFrames X 3)
451        """
452        if name not in self.data['POINTS']:
453            raise ValueError('Point %s does not exists!' %name)
454        return self.data['POINTS'][name]
455
456    def get_points_data(self, name_list:list[str]):
457        """
458        Get several points trajectories
459        :param name_list: list of the points names
460        :return: dict of name:trajectories data
461        """
462        return {name: self.get_point_data(name) for name in name_list}
463
464    def get_point_names(self):
465        """
466        Get a list of all points in the c3d
467        :return: List[str] with all points labels
468        """
469        return list(self.data['POINTS'].keys())
470
471    def get_analog_data(self, name: str):
472        """
473        Return values for an analog chanel
474        :param name: analog name in string
475        :return: analog chanel values as np.ndarray(NbAnalogFrames X 1)
476        """
477        if name not in self.data['ANALOGS']:
478            raise ValueError('Chanel %s does not exists!' %name)
479        return self.data['ANALOGS'][name]
480
481    def get_analogs_data(self, name_list:list[str]):
482        """
483        Get several analog channels
484        :param name_list: list of the analogs names
485        :return: dict of name:values data
486        """
487        return {name: self.get_analog_data(name) for name in name_list}
488
489    def get_analog_names(self):
490        """
491        Get a list of all analogs chanels in the c3d
492        :return: List[str] with all analogs labels
493        """
494        return list(self.data['ANALOGS'].keys())
495
496    def get_rotation_data(self, name:str):
497        """
498        Return rotation data (mainly for c3d created by theia markerless software)
499        :param name: Joint name
500        :return: rotation and position data np.ndarray(NbFrames X 4 X 4)
501        """
502        if name not in self.data['ROTATIONS']:
503            raise ValueError('Rotation %s does not exists!' %name)
504        return self.data['ROTATIONS'][name]
505
506    def get_rotations_data(self, name_list:list[str]):
507        """
508        Get several rotations data
509        :param name_list: list of the rotations
510        :return: dict of name:values data
511        """
512        return {name: self.get_rotation_data(name) for name in name_list}
513
514    def get_rotation_names(self):
515        """
516        Get a list of all rotation data in the c3d
517        :return: List[str] with all rotations labels
518        """
519        return list(self.data['ROTATIONS'].keys())
520
521    @property
522    def point_count(self):
523        """
524        Get the number of 3D points in the c3d
525        :return: int
526        """
527        p = self.get_parameter('POINT', 'USED')
528        return p.value
529
530    @property
531    def analog_count(self):
532        """
533        Get the number of analog channels in the c3d
534        :return: int
535        """
536        p = self.get_parameter('ANALOG', 'USED')
537        if p is not None:
538            return p.value
539        return 0
540
541    @property
542    def frame_count(self):
543        """
544        Get the number of 3D points frames
545        :return: int
546        """
547        return self.header['last_frame'] - self.header['first_frame'] + 1
548
549    @property
550    def analog_frame_count(self):
551        """
552        Get the number of analog frames
553        :return: int
554        """
555        if self.analog_count > 0:
556            return self.frame_count * self.header['analog_per_frame']
557        return 0
558
559    @property
560    def frame_rate(self):
561        """
562        Get the 3D points frame rate
563        :return: float
564        """
565        p = self.get_parameter('POINT', 'RATE')
566        return p.value
567
568    @property
569    def analog_rate(self):
570        """
571        Get the analog channels frame rate
572        :return: float
573        """
574        p = self.get_parameter('ANALOG', 'RATE')
575        if p is not None:
576            return p.value
577        return 0
578
579    @property
580    def point_unit(self):
581        """
582        Get the 3D points unit (usually mm or m)
583        :return: str
584        """
585        p = self.get_parameter('POINT', 'UNITS')
586        return p.value
587
588    @property
589    def analog_unit(self):
590        """
591        Get the analog channels unit (mV, V, ...)
592        :return: str
593        """
594        p = self.get_parameter('ANALOG', 'UNITS')
595        if p is not None:
596            return p.value
597        return []
598
599    def __read_header(self, handle):
600        handle.seek(0)
601        # check if it's a c3d file
602        self.header['parameter_block'], magic = struct.unpack('BB', handle.read(2))
603        if magic != 80:
604            warnings.warn('%s is not a c3d file!' %self.filename)
605            return
606        # go to the start of the parameter block
607        handle.seek((self.header['parameter_block'] - 1) * 512 + 3)
608        # find the good encoder
609        processor = struct.unpack('B', handle.read(1))[0]
610        if processor == PROCESSOR_INTEL:
611            self.__decoder = DecoderIntel(handle)
612        elif processor == PROCESSOR_DEC:
613            self.__decoder = DecoderDec(handle)
614        elif processor == PROCESSOR_MIPS:
615            self.__decoder = DecoderMips(handle)
616
617        #  start reading header
618        handle.seek(2)
619        self.header['point_count'] = self.__decoder.get_uint16()
620        self.header['analog_count'] = self.__decoder.get_uint16()
621        self.header['first_frame'] = self.__decoder.get_uint16()
622        self.header['last_frame'] = self.__decoder.get_uint16()
623        self.header['max_gap'] = self.__decoder.get_uint16()
624        self.header['scale_factor'] = self.__decoder.get_float()
625        self.header['data_block'] = self.__decoder.get_uint16()
626        self.header['analog_per_frame'] = self.__decoder.get_uint16()
627        if self.header['analog_per_frame'] > 0 and self.header['analog_count'] > 0:
628            self.header['analog_count'] /= self.header['analog_per_frame']
629            self.header['analog_count'] = int(self.header['analog_count'])
630        if self.header['analog_per_frame'] == 0:
631            self.header['analog_per_frame'] = 1
632        self.header['frame_rate'] = self.__decoder.get_float()
633
634        self.header['events'] = dict()
635        handle.read(270)
636        self.header['events']['label_range_section'] = self.__decoder.get_uint16()
637        self.header['events']['label_first_block'] = self.__decoder.get_uint16()
638        self.header['events']['label_event_fmt'] = self.__decoder.get_uint16()
639        if self.header['events']['label_event_fmt'] == 12345:
640            self.header['events']['long_event_labels'] = True
641        else:
642            self.header['events']['long_event_labels'] = False
643        self.header['events']['num_events'] = self.__decoder.get_uint16()
644        if self.header['events']['num_events'] > 0:
645            self.header['events']['data'] = {'labels': [], 'time': [], 'display': []}
646            handle.read(2)
647            for i in range(self.header['events']['num_events']):
648                self.header['events']['data']['time'].append(self.__decoder.get_float())
649            handle.seek(198)
650            print(handle.tell())
651            for i in range(self.header['events']['num_events']):
652                self.header['events']['data']['display'].append(self.__decoder.get_uint8())
653                # print(self.header['events']['data']['display'][i])
654            handle.seek(198 * 2)
655            if self.header['events']['long_event_labels']:
656                num_char = 4
657            else:
658                num_char = 2
659            for i in range(self.header['events']['num_events']):
660                name = self.__decoder.get_string(num_char)
661                self.header['events']['data']['labels'].append(name)
662
663    def __write_header(self, handle):
664        if self.__decoder == 0:
665            self.__decoder = DecoderIntel(handle)
666        else:
667            self.__decoder.handle = handle
668
669        handle.seek(0)
670        self.__decoder.write_uint8(self.header['parameter_block'])
671        self.__decoder.write_uint8(80)
672
673        if isinstance(self.__decoder, DecoderIntel):
674            processor = PROCESSOR_INTEL
675        elif isinstance(self.__decoder, DecoderDec):
676            processor = PROCESSOR_DEC
677        elif isinstance(self.__decoder, DecoderMips):
678            processor = PROCESSOR_MIPS
679        else:
680            raise ValueError('Processor not supported')
681
682        handle.seek((self.header['parameter_block'] - 1) * 512 + 3)
683        self.__decoder.write_uint8(processor)
684
685        handle.seek(2)
686        p = self.get_parameter('POINT', 'USED')
687        self.__decoder.write_uint16(p.value)
688        if self.header['analog_per_frame'] > 0 and self.header['analog_count'] > 0:
689            analog_count = self.header['analog_count']* self.header['analog_per_frame']
690        else:
691            analog_count = self.header['analog_count']
692        self.__decoder.write_uint16(analog_count)
693        self.__decoder.write_uint16(self.header['first_frame'])
694        self.__decoder.write_uint16(self.header['last_frame'])
695        self.__decoder.write_uint16(self.header['max_gap'])
696        self.__decoder.write_float(self.header['scale_factor'])
697        data_block = self.__get_parameters_blocknum() + 2
698        self.__decoder.write_uint16(data_block)
699        if self.header['analog_per_frame'] == 1:
700            analog_per_frame = 0
701        else:
702            analog_per_frame = self.header['analog_per_frame']
703        self.__decoder.write_uint16(analog_per_frame)
704        self.__decoder.write_float(self.header['frame_rate'])
705
706        handle.seek(handle.tell() + 270)
707        self.__decoder.write_uint16(self.header['events']['label_range_section'])
708        self.__decoder.write_uint16(self.header['events']['label_first_block'])
709        self.__decoder.write_uint16(self.header['events']['label_event_fmt'])
710        self.__decoder.write_uint16(self.header['events']['num_events'])
711        if self.header['events']['num_events'] > 0:
712            handle.seek(handle.tell() + 2)
713            for i in range(self.header['events']['num_events']):
714                self.__decoder.write_float(self.header['events']['data']['time'][i])
715            handle.seek(198)
716            print(handle.tell())
717            for i in range(self.header['events']['num_events']):
718                self.__decoder.write_uint8(self.header['events']['data']['display'][i])
719            handle.seek(198 * 2)
720            if self.header['events']['long_event_labels']:
721                num_char = 4
722            else:
723                num_char = 2
724            for i in range(self.header['events']['num_events']):
725                name = self.header['events']['data']['labels'][i].ljust(num_char)
726                self.__decoder.write_string(name)
727
728    def __read_parameters(self, handle):
729        handle.seek((self.header['parameter_block'] - 1) * 512)
730        handle.read(4)
731        last_entry = False
732        while not last_entry:
733            nb_char_label = self.__decoder.get_int8()
734            if nb_char_label == 0:
735                break
736            group_id = self.__decoder.get_int8()
737            name = self.__decoder.get_string(abs(nb_char_label))
738            if group_id < 0:#group
739                current = self.add_parameter_group(name, group_id)
740            else:#parameter
741                group = self.get_parameter_group(-group_id)
742                if group is not None:
743                    current = group.add_parameter(name)
744                else :
745                    current = Parameter(name, group_id)
746
747            offset = self.__decoder.get_uint16()
748            if offset == 0:
749                last_entry = True
750            offset -= 2
751            offset -= current.read_from_buffer(self.__decoder)
752
753    def __write_parameters(self, handle):
754        handle.seek((self.header['parameter_block'] - 1) * 512 + 2)
755        self.__decoder.write_uint8(self.__get_parameters_blocknum())
756        handle.seek((self.header['parameter_block'] - 1) * 512+4)
757        groups = list(self.groups.values())
758        for g in groups:
759            g.write_to_buffer(self.__decoder)
760        for group in groups:
761            params = list(group.parameters.values())
762            for param in params:
763                if group == groups[-1] and param == params[-1]:
764                    param.write_to_buffer(self.__decoder, True)
765                else:
766                    param.write_to_buffer(self.__decoder)
767
768    def __get_parameters_blocknum(self):
769        parameters_size = 0
770        for g in self.groups.values():
771            parameters_size += g.get_size()
772            for p in g.parameters.values():
773                parameters_size += p.get_size()
774        return math.ceil(parameters_size/512)
775
776    def __read_data(self, handle):
777        handle.seek((512 * (self.header['data_block'] - 1)))
778        nb_frames = self.header['last_frame'] - self.header['first_frame'] + 1
779        point_used = self.header['point_count']
780        scale = abs(self.header['scale_factor'])
781        is_float = self.header['scale_factor'] < 0
782        if point_used > 0:
783            point_scale = [scale, 1][is_float]
784            names_param = self.get_parameter('POINT', 'LABELS')
785            marker_names = names_param.value.tolist()
786            self.data['POINTS'] = dict()
787            for each in marker_names:
788                self.data['POINTS'][each] = np.zeros([nb_frames, 4])
789
790        analog_used = self.header['analog_count']
791        if analog_used > 0:
792            offsets = np.zeros((analog_used,), int)
793            param = self.get_parameter('ANALOG', 'OFFSET')
794            if param is not None:
795                offsets = param.value
796
797            scales = np.ones((analog_used,), float)
798            param = self.get_parameter('ANALOG', 'SCALE')
799            if param is not None:
800                scales = param.value
801
802            gen_scale = 1.
803            param = self.get_parameter('ANALOG', 'GEN_SCALE')
804            if param is not None:
805                gen_scale = param.value
806            names_param = self.get_parameter('ANALOG', 'LABELS')
807            if names_param is not None:
808                analog_names = names_param.value.tolist()
809
810            self.data['ANALOGS'] = dict()
811            for each in analog_names:
812                self.data['ANALOGS'][each] = np.zeros([nb_frames*self.header['analog_per_frame'], 1])
813
814        rot_param = self.get_parameter_group('ROTATION')
815        if rot_param is not None:
816            names_param = self.get_parameter('ROTATION', 'LABELS')
817            rotation_names = names_param.value.tolist()
818            self.data['ROTATIONS'] = dict()
819            for each in rotation_names:
820                self.data['ROTATIONS'][each] = np.zeros([nb_frames, 4, 4])
821
822        for i in range(nb_frames):
823            if point_used > 0:
824                self.__read_point_frame(i, is_float, point_scale, marker_names)
825            if rot_param is not None:
826                self.__read_rotation_frame(i, rotation_names)
827            if analog_used >0:
828                self.__read_analog_frame(i, is_float, self.header['analog_per_frame'], offsets, scales, gen_scale, analog_names)
829
830    def __read_point_frame(self, frame_num, is_float, point_scale, marker_names):
831        for j, m in enumerate(marker_names):
832            if is_float:
833                p = np.array([self.__decoder.get_float(), self.__decoder.get_float(), self.__decoder.get_float(),
834                              self.__decoder.get_float()])
835            else:
836                p = np.array([self.__decoder.get_uint16(), self.__decoder.get_uint16(), self.__decoder.get_uint16(),
837                              self.__decoder.get_uint16()])
838            p = p * point_scale
839            self.data['POINTS'][m][frame_num, :] = p
840
841    def __read_analog_frame(self, frame_num, is_float, sub_frames, offsets, scales, gen_scale, analog_names):
842        if frame_num == 315:
843            stop = 1
844        for j in range(int(sub_frames)):
845            for k, analog in enumerate(analog_names):
846                if is_float:
847                    c = self.__decoder.get_float()
848                else:
849                    c = self.__decoder.get_uint16()
850                self.data['ANALOGS'][analog][frame_num*sub_frames+j] = (c-offsets[k]) * scales[k]*gen_scale
851
852    def __read_rotation_frame(self, frame_num, rotation_names):
853        for i, name in enumerate(rotation_names):
854            p = np.array([[self.__decoder.get_float(), self.__decoder.get_float(), self.__decoder.get_float(),
855                           self.__decoder.get_float()],
856                          [self.__decoder.get_float(), self.__decoder.get_float(), self.__decoder.get_float(),
857                           self.__decoder.get_float()],
858                          [self.__decoder.get_float(), self.__decoder.get_float(), self.__decoder.get_float(),
859                           self.__decoder.get_float()],
860                          [self.__decoder.get_float(), self.__decoder.get_float(), self.__decoder.get_float(),
861                           self.__decoder.get_float()]])
862            self.data['ROTATIONS'][name][frame_num, :, :] = p.transpose()
863
864    def __write_data(self, handle):
865        handle.seek((512 * (self.header['data_block'] - 1)))
866        point_used = self.header['point_count']>0
867        if point_used:
868            scale = abs(self.header['scale_factor'])
869            is_float = self.header['scale_factor'] < 0
870            point_scale = [scale, 1][is_float]
871            names_param = self.get_parameter('POINT', 'LABELS')
872            marker_names = names_param.value.tolist()
873
874        analog_used = self.header['analog_count']
875        if analog_used > 0:
876            offsets = np.zeros((analog_used,), int)
877            param = self.get_parameter('ANALOG', 'OFFSET')
878            if param is not None:
879                offsets = param.value
880
881            scales = np.ones((analog_used,), float)
882            param = self.get_parameter('ANALOG', 'SCALE')
883            if param is not None:
884                scales = param.value
885
886            gen_scale = 1.
887            param = self.get_parameter('ANALOG', 'GEN_SCALE')
888            if param is not None:
889                gen_scale = param.value
890            names_param = self.get_parameter('ANALOG', 'LABELS')
891            if names_param is not None:
892                analog_names = names_param.value.tolist()
893
894        rot_param = self.get_parameter_group('ROTATION')
895        if rot_param is not None:
896            names_param = self.get_parameter('ROTATION', 'LABELS')
897            rotation_names = names_param.value.tolist()
898
899        nb_frames = self.header['last_frame'] - self.header['first_frame'] + 1
900        for i in range(nb_frames):
901            if point_used:
902                self.__write_point_frame(marker_names, i, is_float, point_scale)
903            if rot_param is not None:
904                self.__write_rotation_frame(i, rotation_names)
905            if analog_used:
906                self.__write_analog_frame(i, self.header['analog_per_frame'], analog_names, is_float, scales, gen_scale, offsets)
907
908    def __write_point_frame(self, marker_names, frame_num, is_float, point_scale):
909        for j, m in enumerate(marker_names):
910            p = self.data['POINTS'][m][frame_num, :]/point_scale
911            for i in range(4):
912                if is_float:
913                    self.__decoder.write_float(p[i])
914                else:
915                    self.__decoder.write_uint16(p[i])
916
917    def __write_analog_frame(self, frame_num, sub_frames, analog_names, is_float, scales, gen_scale, offsets):
918        for j in range(int(sub_frames)):
919            for k, analog in enumerate(analog_names):
920                data = self.data['ANALOGS'][analog][frame_num*sub_frames+j]
921                c = ((data/gen_scale/scales[k]))+offsets[k]
922                if is_float:
923                    self.__decoder.write_float(c)
924                else:
925                    self.__decoder.write_uint16(c)
926
927    def __write_rotation_frame(self, frame_num, rotation_names):
928        for i, name in enumerate(rotation_names):
929            r = self.data['ROTATIONS'][name][frame_num, :, :].transpose()
930            for j in range(4):
931                for k in range(4):
932                    self.__decoder.write_float(r[j,k])