Added the first public version of C3D importer addon.

2 files changed, 501 insertions, 0 deletions

diff --git a/io_anim_c3d/__init__.py b/io_anim_c3d/__init__.py
new file mode 100644
index 00000000..f8510fd2
--- /dev/null
+++ b/io_anim_c3d/__init__.py
@@ -0,0 +1,239 @@
+# ##### BEGIN GPL LICENSE BLOCK #####
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 3
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software Foundation,
+#  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+#
+# ##### END GPL LICENSE BLOCK #####
+
+# <pep8-80 compliant>
+
+# This script was developed with financial support from the Foundation for
+# Science and Technology of Portugal, under the grant SFRH/BD/66452/2009.
+
+
+bl_info = {
+    'name': "C3D Graphics Lab Motion Capture file (.c3d)",
+    'author': "Daniel Monteiro Basso <daniel@basso.inf.br>",
+    'version': (2011, 11, 2, 1),
+    'blender': (2, 6, 0),
+    'api': 41226,
+    'location': "File > Import",
+    'description': "Imports C3D Graphics Lab Motion Capture files",
+    'wiki_url': "http://wiki.blender.org/index.php/Extensions:2.5/Py/"\
+        "Scripts/Import-Export/C3D_Importer",
+    'tracker_url': "http://projects.blender.org/tracker/?func=detail&atid=467"\
+        "&aid=29061&group_id=153",
+    'category': 'Import-Export'}
+
+
+import bpy
+import math
+import time
+from bpy.props import StringProperty, BoolProperty, FloatProperty, IntProperty
+from mathutils import Vector as vec
+from . import c3d
+
+
+class C3DAnimateCloud(bpy.types.Operator):
+    """
+        Animate the Marker Cloud
+    """
+    bl_idname = "import_anim.c3danim"
+    bl_label = "Animate C3D"
+
+    markerset = None
+    uname = None
+    curframe = 0
+    fskip = 0
+    scale = 0
+    timer = None
+
+    def modal(self, context, event):
+        if event.type == 'ESC':
+            return self.cancel(context)
+        if event.type == 'TIMER':
+            if self.curframe > self.markerset.endFrame:
+                return self.cancel(context)
+            fno = self.curframe
+            if not self.useFrameNo:
+                fno = (self.curframe - self.markerset.startFrame) / self.fskip
+            for i in range(self.fskip):
+                self.markerset.readNextFrameData()
+            for ml in self.markerset.markerLabels:
+                name = self.unames[self.prefix + ml]
+                o = bpy.context.scene.objects[name]
+                m = self.markerset.getMarker(ml, self.curframe)
+                o.location = vec(m.position) * self.scale
+                if m.confidence >= self.confidence:
+                    o.keyframe_insert('location', frame=fno)
+            self.curframe += self.fskip
+        return {'PASS_THROUGH'}
+
+    def execute(self, context):
+        context.window_manager.modal_handler_add(self)
+        self.timer = context.window_manager.\
+            event_timer_add(0.001, context.window)
+        return {'RUNNING_MODAL'}
+
+    def cancel(self, context):
+        bpy.context.scene.frame_set(bpy.context.scene.frame_current)
+        context.window_manager.event_timer_remove(self.timer)
+        return {'CANCELLED'}
+
+
+class C3DImporter(bpy.types.Operator):
+    """
+        Load a C3D Marker Cloud
+    """
+    bl_idname = "import_anim.c3d"
+    bl_label = "Import C3D"
+
+    filepath = StringProperty(name="File Path", maxlen=1024, default="",
+                          description="Path to the C3D file")
+    from_inches = BoolProperty(name="Convert from inches to metric",
+                          default=False, description="Scale by 2.54/100")
+    scale = FloatProperty(name="Scale", default=1.,
+                          description="Scale the positions by this value",
+                          min=0.0001, max=1000000.0,
+                          soft_min=0.001, soft_max=100.0)
+    auto_scale = BoolProperty(name="Adjust scale automatically", default=False,
+                          description="Guess correct scale factor")
+    auto_magnitude = BoolProperty(name="Adjust scale magnitude", default=True,
+                          description="Automatically adjust scale magnitude")
+    size = FloatProperty(name="Empty Size", default=.03,
+                          description="The size of each empty",
+                          min=0.0001, max=1000000.0,
+                          soft_min=0.001, soft_max=100.0)
+    x_ray = BoolProperty(name="Use X-Ray", default=True,
+                          description="Show the empties over other objects")
+    frame_skip = IntProperty(name="Fps divisor", default=4,
+    # usually the sample rate is 120, so the default 4 gives you 30fps
+                          description="Frame supersampling factor", min=1)
+    useFrameNo = BoolProperty(name="Use frame numbers", default=False,
+              description="Offset start of animation according to the source")
+    show_names = BoolProperty(name="Show Names", default=False,
+                          description="Show the markers' name")
+    prefix = StringProperty(name="Name Prefix", maxlen=1024, default="",
+                          description="Prefix object names with this")
+    confidence = FloatProperty(name="Minimum Confidence Level", default=0,
+                          description="Only consider markers with at least "
+                                      "this confidence level",
+                          min=-1., max=1000000.0,
+                          soft_min=-1., soft_max=100.0)
+    filter_glob = StringProperty(default="*.c3d;*.csv", options={'HIDDEN'})
+
+    def find_height(self, ms):
+        """
+            Heuristic to find the height of the subject in the markerset
+            (only works for standing poses)
+        """
+        zmin = None
+        for ml in ms.markerLabels:
+            if 'LTOE' in ml:
+                hd = ml.replace('LTOE', 'LFHD')
+                if hd not in ms.markerLabels:
+                    break
+                pmin_idx = ms.markerLabels.index(ml)
+                pmax_idx = ms.markerLabels.index(hd)
+                zmin = ms.frames[0][pmin_idx].position[2]
+                zmax = ms.frames[0][pmax_idx].position[2]
+        if zmin is None:  # could not find named markers, get extremes
+            allz = [m.position[2] for m in ms.frames[0]]
+            zmin, zmax = min(allz), max(allz)
+        return abs(zmax - zmin)
+
+    def adjust_scale_magnitude(self, height, scale):
+        mag = math.log10(height * scale)
+        #print('mag',mag, 'scale',scale)
+        return scale * math.pow(10, -int(mag))
+
+    def adjust_scale(self, height, scale):
+        factor = height * scale / 1.75  # normalize
+        if factor < .5:
+            scale /= 10
+            factor *= 10
+        cmu_factors = [(1.0, 1.0), (1.1, 1.45), (1.6, 1.6), (2.54, 2.54)]
+        sqerr, fix = min(((cf[0] - factor) ** 2, 1 / cf[1])
+            for cf in cmu_factors)
+        #print('height * scale: {:.2f}'.format(height * scale))
+        #print(factor, fix)
+        return scale * fix
+
+    def execute(self, context):
+        s = self.properties.size
+        empty_size = (s, s, s)
+        ms = c3d.read(self.properties.filepath, onlyHeader=True)
+        ms.readNextFrameData()
+        #print(ms.fileName)
+
+        # determine the final scale
+        height = self.find_height(ms)
+        #print('h', height)
+        scale = 1.0 if not self.properties.from_inches else 2.54
+        scale *= ms.scale
+        if self.properties.auto_magnitude:
+            scale = self.adjust_scale_magnitude(height, scale)
+            #print('scale',scale)
+        if self.properties.auto_scale:
+            scale = self.adjust_scale(height, scale)
+        scale *= self.properties.scale
+
+        # create the empties and get their collision-free names
+        unames = {}
+        for ml in ms.markerLabels:
+            bpy.ops.object.add()
+            bpy.ops.transform.resize(value=empty_size)
+            name = self.properties.prefix + ml
+            bpy.context.active_object.name = name
+            unames[name] = bpy.context.active_object.name
+            bpy.context.active_object.show_name = self.properties.show_names
+            bpy.context.active_object.show_x_ray = self.properties.x_ray
+        for name in unames.values():
+            bpy.context.scene.objects[name].select = True
+
+        # start animating the empties
+        C3DAnimateCloud.markerset = ms
+        C3DAnimateCloud.unames = unames
+        C3DAnimateCloud.scale = scale
+        C3DAnimateCloud.fskip = self.properties.frame_skip
+        C3DAnimateCloud.prefix = self.properties.prefix
+        C3DAnimateCloud.useFrameNo = self.properties.useFrameNo
+        C3DAnimateCloud.confidence = self.properties.confidence
+        C3DAnimateCloud.curframe = ms.startFrame
+        bpy.ops.import_anim.c3danim()
+        return {'FINISHED'}
+
+    def invoke(self, context, event):
+        wm = context.window_manager
+        wm.fileselect_add(self)
+        return {'RUNNING_MODAL'}
+
+
+def menu_func(self, context):
+    self.layout.operator(C3DImporter.bl_idname,
+                         text="Graphics Lab Motion Capture (.c3d)")
+
+
+def register():
+    bpy.utils.register_module(__name__)
+    bpy.types.INFO_MT_file_import.append(menu_func)
+
+
+def unregister():
+    bpy.utils.unregister_module(__name__)
+    bpy.types.INFO_MT_file_import.remove(menu_func)
+
+
+if __name__ == "__main__":
+    register()
diff --git a/io_anim_c3d/c3d.py b/io_anim_c3d/c3d.py
new file mode 100644
index 00000000..a28c5dce
--- /dev/null
+++ b/io_anim_c3d/c3d.py
@@ -0,0 +1,262 @@
+# ##### BEGIN GPL LICENSE BLOCK #####
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 3
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software Foundation,
+#  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+#
+# ##### END GPL LICENSE BLOCK #####
+
+# <pep8-80 compliant>
+
+# By Daniel Monteiro Basso, April-November 2011.
+
+# This script was developed with financial support from the Foundation for
+# Science and Technology of Portugal, under the grant SFRH/BD/66452/2009.
+
+# Complete rewrite, but based on the original importer for Blender
+# 2.39, developed by Jean-Baptiste PERIN (jb_perin(at)yahoo.fr), which was
+# based on the MATLAB C3D loader from Alan Morris, Toronto, October 1998
+# and Jaap Harlaar, Amsterdam, april 2002
+
+
+import struct
+try:
+    from numpy import array as vec  # would be nice to have NumPy in Blender
+except:
+    from mathutils import Vector as vec
+
+
+class Marker:
+    position = (0., 0., 0.)
+    confidence = -1.
+
+
+class Parameter:
+    def __init__(self, infile):
+        (nameLength, self.paramIdx) = struct.unpack('bb', infile.read(2))
+        if not nameLength:
+            self.name = ''
+            return
+        if nameLength < 0 or nameLength > 64:
+            raise ValueError
+        self.name = infile.read(nameLength).decode('ascii')
+        (offset, b) = struct.unpack('hb', infile.read(3))
+        if self.paramIdx > 0:
+            self.isGroup = False
+            self.data = infile.read(offset - 3)
+        else:
+            self.isGroup = True
+            self.paramIdx *= -1
+            self.description = infile.read(b)
+            self.params = {}
+
+    def collect(self, infile):
+        while True:
+            p = Parameter(infile)
+            if not p.name or p.isGroup:
+                return p
+            self.params[p.name] = p
+
+    def decode(self):
+        # for now only decode labels
+        l, c = struct.unpack('BB', self.data[1:3])
+        return [self.data[3 + i:3 + i + l].strip().decode('ascii')
+                for i in range(0, l * c, l)]
+
+
+class MarkerSet:
+    def __init__(self, fileName, scale=1., stripPrefix=True, onlyHeader=False):
+        self.fileName = fileName
+        if fileName.endswith('.csv'):
+            with open(fileName, 'rt') as infile:
+                self.readCSV(infile)
+            return
+        if onlyHeader:
+            self.infile = open(fileName, 'rb')
+            self.readHeader(self.infile, scale)
+            self.identifyMarkerPrefix(stripPrefix)
+            self.infile.seek(512 * (self.dataBlock - 1))
+            self.frames = []
+            return
+        with open(fileName, 'rb') as infile:
+            self.readHeader(infile, scale)
+            self.identifyMarkerPrefix(stripPrefix)
+            self.readFrameData(infile)
+
+    def readCSV(self, infile):
+        import csv
+        csvr = csv.reader(infile)
+        header = next(csvr)
+        if 0 != len(header) % 3:
+            raise Exception('Incorrect data format in CSV file')
+        self.markerLabels = [label[:-2] for label in header[::3]]
+        self.frames = []
+        for framerow in csvr:
+            newFrame = []
+            for c in range(0, len(framerow), 3):
+                m = Marker()
+                try:
+                    m.position = vec([float(v) for v in framerow[c:c + 3]])
+                    m.confidence = 1.
+                except:
+                    pass
+                newFrame.append(m)
+            self.frames.append(newFrame)
+        self.startFrame = 0
+        self.endFrame = len(self.frames) - 1
+        self.scale = 1.
+
+    def writeCSV(self, fileName, applyScale=True, mfilter=[]):
+        import csv
+        with open(fileName, 'w') as fo:
+            o = csv.writer(fo)
+            appxyz = lambda m: [m + a for a in ('_X', '_Y', '_Z')]
+            explabels = (appxyz(m) for m in self.markerLabels
+                         if not mfilter or m in mfilter)
+            o.writerow(sum(explabels, []))
+            fmt = lambda m: tuple('{0:.4f}'.format(
+                a * (self.scale if applyScale else 1.))
+                for a in m.position)
+            nan = ('NaN', 'NaN', 'NaN')
+            if mfilter:
+                mfilter = [self.markerLabels.index(m)
+                            for m in self.markerLabels if m in mfilter]
+            for f in self.frames:
+                F = f
+                if mfilter:
+                    F = [m for i, m in enumerate(f) if i in mfilter]
+                expmarkers = (m.confidence < 0 and nan or fmt(m) for m in F)
+                o.writerow(sum(expmarkers, ()))
+
+    def identifyMarkerPrefix(self, stripPrefix):
+        prefix = self.markerLabels[0]
+        for ml in self.markerLabels[1:]:
+            if len(ml) < len(prefix):
+                prefix = prefix[:len(ml)]
+            if not prefix:
+                break
+            for i in range(len(prefix)):
+                if prefix[i] != ml[i]:
+                    prefix = prefix[:i]
+                    break
+        self.prefix = prefix
+        if stripPrefix:
+            p = len(self.prefix)
+            self.markerLabels = [ml[p:] for ml in self.markerLabels]
+
+    def readHeader(self, infile, scale):
+        (self.firstParameterBlock, key, self.markerCount, bogus,
+         self.startFrame, self.endFrame,
+         bogus) = struct.unpack('BBhhhhh', infile.read(12))
+        if key != 80:
+            raise Exception('Not a C3D file.')
+        self.readParameters(infile)
+        infile.seek(12)
+        td = infile.read(12)
+        if self.procType == 2:
+            td = td[2:4] + td[:2] + td[4:8] + td[10:] + td[8:10]
+        (self.scale, self.dataBlock, bogus,
+         self.frameRate) = struct.unpack('fhhf', td)
+        self.scale *= scale
+        if self.scale < 0:
+            self.readMarker = self.readFloatMarker
+            self.scale *= -1
+        else:
+            self.readMarker = self.readShortMarker
+
+    def readParameters(self, infile):
+        infile.seek(512 * (self.firstParameterBlock - 1))
+        (ig, ig, pointIdx,
+         self.procType) = struct.unpack('BBBB', infile.read(4))
+        self.procType -= 83
+        if self.procType not in (1, 2):
+            # 1(INTEL-PC); 2(DEC-VAX); 3(MIPS-SUN/SGI)
+            print('Warning: importer was not tested for files from '
+                  'architectures other than Intel-PC and DEC-VAX')
+            print('Type: {0}'.format(self.procType))
+        self.paramGroups = {}
+        g = Parameter(infile)
+        self.paramGroups[g.name] = g
+        while(True):
+            g = g.collect(infile)
+            if not g.name:
+                break
+            self.paramGroups[g.name] = g
+        self.markerLabels = self.paramGroups['POINT'].params['LABELS'].decode()
+
+    def readMarker(self, infile):
+        pass  # ...
+
+    def readFloatMarker(self, infile):
+        m = Marker()
+        x, y, z, m.confidence = struct.unpack('ffff', infile.read(16))
+        m.position = (x * self.scale, y * self.scale, z * self.scale)
+        return m
+
+    def readShortMarker(self, infile):
+        m = Marker()
+        x, y, z, m.confidence = struct.unpack('hhhh', infile.read(8))
+        m.position = (x * self.scale, y * self.scale, z * self.scale)
+        return m
+
+    def readFrameData(self, infile):
+        infile.seek(512 * (self.dataBlock - 1))
+        self.frames = []
+        for f in range(self.startFrame, self.endFrame + 1):
+            frame = [self.readMarker(infile) for m in range(self.markerCount)]
+            self.frames.append(frame)
+
+    def readNextFrameData(self):
+        if len(self.frames) < (self.endFrame - self.startFrame + 1):
+            frame = [self.readMarker(self.infile)
+                for m in range(self.markerCount)]
+            self.frames.append(frame)
+        return self.frames[-1]
+
+    def getFramesByMarker(self, marker):
+        if type(marker) == int:
+            idx = marker
+        else:
+            idx = self.markerLabels.index(marker)
+        fcnt = self.endFrame - self.startFrame + 1
+        return [self.frames[f][idx] for f in range(fcnt)]
+
+    def getMarker(self, marker, frame):
+        idx = self.markerLabels.index(marker)
+        return self.frames[frame - self.startFrame][idx]
+
+
+def read(filename, *a, **kw):
+    return MarkerSet(filename, *a, **kw)
+
+# -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
+
+
+if __name__ == '__main__':
+    import os
+    import sys
+
+    sys.argv.pop(0)
+    if not sys.argv:
+        print("Convert C3D to CSV.\n"
+              "Please specify at least one C3D input file.")
+        raise SystemExit
+    while sys.argv:
+        fname = sys.argv.pop(0)
+        markerset = read(fname)
+        print("frameRate={0.frameRate}\t"
+              "scale={0.scale:.2f}\t"
+              "markers={0.markerCount}\t"
+              "startFrame={0.startFrame}\t"
+              "endFrame={0.endFrame}".format(markerset))
+        markerset.writeCSV(fname.lower().replace(".c3d", ".csv"))