change python scripts so modules which register with blender have a register() functi...
[blender.git] / release / scripts / io / import_anim_bvh.py
1 # ##### BEGIN GPL LICENSE BLOCK #####
2 #
3 #  This program is free software; you can redistribute it and/or
4 #  modify it under the terms of the GNU General Public License
5 #  as published by the Free Software Foundation; either version 2
6 #  of the License, or (at your option) any later version.
7 #
8 #  This program is distributed in the hope that it will be useful,
9 #  but WITHOUT ANY WARRANTY; without even the implied warranty of
10 #  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11 #  GNU General Public License for more details.
12 #
13 #  You should have received a copy of the GNU General Public License
14 #  along with this program; if not, write to the Free Software Foundation,
15 #  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
16 #
17 # ##### END GPL LICENSE BLOCK #####
18
19 # <pep8 compliant>
20
21 import math
22
23 # import Blender
24 import bpy
25 # import BPyMessages
26 import Mathutils
27 Vector = Mathutils.Vector
28 Euler = Mathutils.Euler
29 Matrix = Mathutils.Matrix
30 RotationMatrix = Mathutils.RotationMatrix
31 TranslationMatrix = Mathutils.TranslationMatrix
32
33 # NASTY GLOBAL
34 ROT_STYLE = 'QUAT'
35
36 DEG2RAD = 0.017453292519943295
37
38 class bvh_node_class(object):
39     __slots__=(\
40     'name',# bvh joint name
41     'parent',# bvh_node_class type or None for no parent
42     'children',# a list of children of this type.
43     'rest_head_world',# worldspace rest location for the head of this node
44     'rest_head_local',# localspace rest location for the head of this node
45     'rest_tail_world',# # worldspace rest location for the tail of this node
46     'rest_tail_local',# # worldspace rest location for the tail of this node
47     'channels',# list of 6 ints, -1 for an unused channel, otherwise an index for the BVH motion data lines, lock triple then rot triple
48     'rot_order',# a triple of indicies as to the order rotation is applied. [0,1,2] is x/y/z - [None, None, None] if no rotation.
49     'anim_data',# a list one tuple's one for each frame. (locx, locy, locz, rotx, roty, rotz)
50     'has_loc',# Conveinience function, bool, same as (channels[0]!=-1 or channels[1]!=-1 channels[2]!=-1)
51     'has_rot',# Conveinience function, bool, same as (channels[3]!=-1 or channels[4]!=-1 channels[5]!=-1)
52     'temp')# use this for whatever you want
53
54     def __init__(self, name, rest_head_world, rest_head_local, parent, channels, rot_order):
55         self.name = name
56         self.rest_head_world = rest_head_world
57         self.rest_head_local = rest_head_local
58         self.rest_tail_world = None
59         self.rest_tail_local = None
60         self.parent = parent
61         self.channels = channels
62         self.rot_order = rot_order
63
64         # convenience functions
65         self.has_loc = channels[0] != -1 or channels[1] != -1 or channels[2] != -1
66         self.has_rot = channels[3] != -1 or channels[4] != -1 or channels[5] != -1
67
68
69         self.children = []
70
71         # list of 6 length tuples: (lx,ly,lz, rx,ry,rz)
72         # even if the channels arnt used they will just be zero
73         #
74         self.anim_data = [(0, 0, 0, 0, 0, 0)]
75
76
77     def __repr__(self):
78         return 'BVH name:"%s", rest_loc:(%.3f,%.3f,%.3f), rest_tail:(%.3f,%.3f,%.3f)' %\
79         (self.name,\
80         self.rest_head_world.x, self.rest_head_world.y, self.rest_head_world.z,\
81         self.rest_head_world.x, self.rest_head_world.y, self.rest_head_world.z)
82
83
84
85 # Change the order rotation is applied.
86 MATRIX_IDENTITY_3x3 = Matrix([1,0,0],[0,1,0],[0,0,1])
87 MATRIX_IDENTITY_4x4 = Matrix([1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1])
88
89 def eulerRotate(x,y,z, rot_order):
90
91     # Clamp all values between 0 and 360, values outside this raise an error.
92     mats=[RotationMatrix(math.radians(x % 360), 3, 'X'), RotationMatrix(math.radians(y % 360),3,'Y'), RotationMatrix(math.radians(z % 360), 3, 'Z')]
93     # print rot_order
94     # Standard BVH multiplication order, apply the rotation in the order Z,X,Y
95
96     #XXX, order changes???
97     #eul = (mats[rot_order[2]]*(mats[rot_order[1]]* (mats[rot_order[0]]* MATRIX_IDENTITY_3x3))).to_euler()
98     eul = (MATRIX_IDENTITY_3x3*mats[rot_order[0]]*(mats[rot_order[1]]* (mats[rot_order[2]]))).to_euler()
99
100     eul = math.degrees(eul.x), math.degrees(eul.y), math.degrees(eul.z)
101
102     return eul
103
104 def read_bvh(context, file_path, GLOBAL_SCALE=1.0):
105     # File loading stuff
106     # Open the file for importing
107     file = open(file_path, 'rU')
108
109     # Seperate into a list of lists, each line a list of words.
110     file_lines = file.readlines()
111     # Non standard carrage returns?
112     if len(file_lines) == 1:
113         file_lines = file_lines[0].split('\r')
114
115     # Split by whitespace.
116     file_lines =[ll for ll in [ l.split() for l in file_lines] if ll]
117
118
119     # Create Hirachy as empties
120
121     if file_lines[0][0].lower() == 'hierarchy':
122         #print 'Importing the BVH Hierarchy for:', file_path
123         pass
124     else:
125         raise 'ERROR: This is not a BVH file'
126
127     bvh_nodes = {None: None}
128     bvh_nodes_serial = [None]
129
130     channelIndex = -1
131
132
133     lineIdx = 0 # An index for the file.
134     while lineIdx < len(file_lines) -1:
135         #...
136         if file_lines[lineIdx][0].lower() == 'root' or file_lines[lineIdx][0].lower() == 'joint':
137
138             # Join spaces into 1 word with underscores joining it.
139             if len(file_lines[lineIdx]) > 2:
140                 file_lines[lineIdx][1] = '_'.join(file_lines[lineIdx][1:])
141                 file_lines[lineIdx] = file_lines[lineIdx][:2]
142
143             # MAY NEED TO SUPPORT MULTIPLE ROOT's HERE!!!, Still unsure weather multiple roots are possible.??
144
145             # Make sure the names are unique- Object names will match joint names exactly and both will be unique.
146             name = file_lines[lineIdx][1]
147
148             #print '%snode: %s, parent: %s' % (len(bvh_nodes_serial) * '  ', name,  bvh_nodes_serial[-1])
149
150             lineIdx += 2 # Incriment to the next line (Offset)
151             rest_head_local = Vector( GLOBAL_SCALE * float(file_lines[lineIdx][1]), GLOBAL_SCALE * float(file_lines[lineIdx][2]), GLOBAL_SCALE * float(file_lines[lineIdx][3]))
152             lineIdx += 1 # Incriment to the next line (Channels)
153
154             # newChannel[Xposition, Yposition, Zposition, Xrotation, Yrotation, Zrotation]
155             # newChannel references indecies to the motiondata,
156             # if not assigned then -1 refers to the last value that will be added on loading at a value of zero, this is appended
157             # We'll add a zero value onto the end of the MotionDATA so this is always refers to a value.
158             my_channel = [-1, -1, -1, -1, -1, -1]
159             my_rot_order = [None, None, None]
160             rot_count = 0
161             for channel in file_lines[lineIdx][2:]:
162                 channel = channel.lower()
163                 channelIndex += 1 # So the index points to the right channel
164                 if   channel == 'xposition':    my_channel[0] = channelIndex
165                 elif channel == 'yposition':    my_channel[1] = channelIndex
166                 elif channel == 'zposition':    my_channel[2] = channelIndex
167
168                 elif channel == 'xrotation':
169                     my_channel[3] = channelIndex
170                     my_rot_order[rot_count] = 0
171                     rot_count += 1
172                 elif channel == 'yrotation':
173                     my_channel[4] = channelIndex
174                     my_rot_order[rot_count] = 1
175                     rot_count += 1
176                 elif channel == 'zrotation':
177                     my_channel[5] = channelIndex
178                     my_rot_order[rot_count] = 2
179                     rot_count += 1
180
181             channels = file_lines[lineIdx][2:]
182
183             my_parent = bvh_nodes_serial[-1] # account for none
184
185
186             # Apply the parents offset accumletivly
187             if my_parent==None:
188                 rest_head_world = Vector(rest_head_local)
189             else:
190                 rest_head_world = my_parent.rest_head_world + rest_head_local
191
192             bvh_node = bvh_nodes[name] = bvh_node_class(name, rest_head_world, rest_head_local, my_parent, my_channel, my_rot_order)
193
194             # If we have another child then we can call ourselves a parent, else
195             bvh_nodes_serial.append(bvh_node)
196
197         # Account for an end node
198         if file_lines[lineIdx][0].lower() == 'end' and file_lines[lineIdx][1].lower() == 'site': # There is somtimes a name after 'End Site' but we will ignore it.
199             lineIdx += 2 # Incriment to the next line (Offset)
200             rest_tail = Vector( GLOBAL_SCALE*float(file_lines[lineIdx][1]), GLOBAL_SCALE*float(file_lines[lineIdx][2]), GLOBAL_SCALE*float(file_lines[lineIdx][3]))
201
202             bvh_nodes_serial[-1].rest_tail_world = bvh_nodes_serial[-1].rest_head_world + rest_tail
203             bvh_nodes_serial[-1].rest_tail_local = rest_tail
204
205
206             # Just so we can remove the Parents in a uniform way- End end never has kids
207             # so this is a placeholder
208             bvh_nodes_serial.append(None)
209
210         if len(file_lines[lineIdx]) == 1 and file_lines[lineIdx][0] == '}': # == ['}']
211             bvh_nodes_serial.pop() # Remove the last item
212
213         if len(file_lines[lineIdx]) == 1 and file_lines[lineIdx][0].lower() == 'motion':
214             #print '\nImporting motion data'
215             lineIdx += 3 # Set the cursor to the first frame
216             break
217
218         lineIdx += 1
219
220
221     # Remove the None value used for easy parent reference
222     del bvh_nodes[None]
223     # Dont use anymore
224     del bvh_nodes_serial
225
226     bvh_nodes_list = bvh_nodes.values()
227
228     while lineIdx < len(file_lines):
229         line = file_lines[lineIdx]
230         for bvh_node in bvh_nodes_list:
231             #for bvh_node in bvh_nodes_serial:
232             lx = ly = lz = rx = ry = rz = 0.0
233             channels = bvh_node.channels
234             anim_data = bvh_node.anim_data
235             if channels[0] != -1:
236                 lx = GLOBAL_SCALE * float(line[channels[0]])
237
238             if channels[1] != -1:
239                 ly = GLOBAL_SCALE * float(line[channels[1]])
240
241             if channels[2] != -1:
242                 lz = GLOBAL_SCALE * float(line[channels[2]])
243
244             if channels[3] != -1 or channels[4] != -1 or channels[5] != -1:
245                 rx, ry, rz = float(line[channels[3]]), float(line[channels[4]]), float(line[channels[5]])
246
247                 if ROT_STYLE != 'NATIVE':
248                     rx, ry, rz = eulerRotate(rx, ry, rz, bvh_node.rot_order)
249
250                 # Make interpolation not cross between 180d, thjis fixes sub frame interpolation and time scaling.
251                 # Will go from (355d to 365d) rather then to (355d to 5d) - inbetween these 2 there will now be a correct interpolation.
252
253                 while anim_data[-1][3] - rx >  180: rx+=360
254                 while anim_data[-1][3] - rx < -180: rx-=360
255
256                 while anim_data[-1][4] - ry >  180: ry+=360
257                 while anim_data[-1][4] - ry < -180: ry-=360
258
259                 while anim_data[-1][5] - rz >  180: rz+=360
260                 while anim_data[-1][5] - rz < -180: rz-=360
261
262             # Done importing motion data #
263             anim_data.append( (lx, ly, lz, rx, ry, rz) )
264         lineIdx += 1
265
266     # Assign children
267     for bvh_node in bvh_nodes.values():
268         bvh_node_parent = bvh_node.parent
269         if bvh_node_parent:
270             bvh_node_parent.children.append(bvh_node)
271
272     # Now set the tip of each bvh_node
273     for bvh_node in bvh_nodes.values():
274
275         if not bvh_node.rest_tail_world:
276             if len(bvh_node.children)==0:
277                 # could just fail here, but rare BVH files have childless nodes
278                 bvh_node.rest_tail_world = Vector(bvh_node.rest_head_world)
279                 bvh_node.rest_tail_local = Vector(bvh_node.rest_head_local)
280             elif len(bvh_node.children)==1:
281                 bvh_node.rest_tail_world = Vector(bvh_node.children[0].rest_head_world)
282                 bvh_node.rest_tail_local = Vector(bvh_node.children[0].rest_head_local)
283             else:
284                 # allow this, see above
285                 #if not bvh_node.children:
286                 #       raise 'error, bvh node has no end and no children. bad file'
287
288                 # Removed temp for now
289                 rest_tail_world = Vector(0, 0, 0)
290                 rest_tail_local = Vector(0, 0, 0)
291                 for bvh_node_child in bvh_node.children:
292                     rest_tail_world += bvh_node_child.rest_head_world
293                     rest_tail_local += bvh_node_child.rest_head_local
294
295                 bvh_node.rest_tail_world = rest_tail_world * (1.0 / len(bvh_node.children))
296                 bvh_node.rest_tail_local = rest_tail_local * (1.0 / len(bvh_node.children))
297
298         # Make sure tail isnt the same location as the head.
299         if (bvh_node.rest_tail_local-bvh_node.rest_head_local).length <= 0.001*GLOBAL_SCALE:
300
301             bvh_node.rest_tail_local.y = bvh_node.rest_tail_local.y + GLOBAL_SCALE/10
302             bvh_node.rest_tail_world.y = bvh_node.rest_tail_world.y + GLOBAL_SCALE/10
303
304
305
306     return bvh_nodes
307
308
309
310 def bvh_node_dict2objects(context, bvh_nodes, IMPORT_START_FRAME= 1, IMPORT_LOOP= False):
311
312     if IMPORT_START_FRAME<1:
313         IMPORT_START_FRAME= 1
314
315     scn= context.scene
316     scn.objects.selected = []
317
318     objects= []
319
320     def add_ob(name):
321         ob = scn.objects.new('Empty')
322         objects.append(ob)
323         return ob
324
325     # Add objects
326     for name, bvh_node in bvh_nodes.items():
327         bvh_node.temp= add_ob(name)
328
329     # Parent the objects
330     for bvh_node in bvh_nodes.values():
331         bvh_node.temp.makeParent([ bvh_node_child.temp for bvh_node_child in bvh_node.children ], 1, 0) # ojbs, noninverse, 1 = not fast.
332
333     # Offset
334     for bvh_node in bvh_nodes.values():
335         # Make relative to parents offset
336         bvh_node.temp.loc= bvh_node.rest_head_local
337
338     # Add tail objects
339     for name, bvh_node in bvh_nodes.items():
340         if not bvh_node.children:
341             ob_end= add_ob(name + '_end')
342             bvh_node.temp.makeParent([ob_end], 1, 0) # ojbs, noninverse, 1 = not fast.
343             ob_end.loc= bvh_node.rest_tail_local
344
345
346     # Animate the data, the last used bvh_node will do since they all have the same number of frames
347     for current_frame in range(len(bvh_node.anim_data)):
348         Blender.Set('curframe', current_frame+IMPORT_START_FRAME)
349
350         for bvh_node in bvh_nodes.values():
351             lx,ly,lz,rx,ry,rz= bvh_node.anim_data[current_frame]
352
353             rest_head_local= bvh_node.rest_head_local
354             bvh_node.temp.loc= rest_head_local.x+lx, rest_head_local.y+ly, rest_head_local.z+lz
355
356             bvh_node.temp.rot= rx*DEG2RAD,ry*DEG2RAD,rz*DEG2RAD
357
358             bvh_node.temp.insertIpoKey(Blender.Object.IpoKeyTypes.LOCROT) # XXX invalid
359
360     scn.update(1)
361     return objects
362
363
364
365 def bvh_node_dict2armature(context, bvh_nodes, IMPORT_START_FRAME= 1, IMPORT_LOOP= False):
366
367     if IMPORT_START_FRAME<1:
368         IMPORT_START_FRAME= 1
369
370
371     # Add the new armature,
372     scn = context.scene
373 #XXX    scn.objects.selected = []
374     for ob in scn.objects:
375         ob.selected = False
376
377
378 #XXX    arm_data= bpy.data.armatures.new()
379 #XXX    arm_ob = scn.objects.new(arm_data)
380     bpy.ops.object.armature_add()
381     arm_ob= scn.objects[-1]
382     arm_data= arm_ob.data
383
384
385
386
387 #XXX    scn.objects.context = [arm_ob]
388 #XXX    scn.objects.active = arm_ob
389     arm_ob.selected= True
390     scn.objects.active= arm_ob
391     print(scn.objects.active)
392
393
394     # Put us into editmode
395 #XXX    arm_data.makeEditable()
396
397     bpy.ops.object.mode_set(mode='OBJECT', toggle=False)
398     bpy.ops.object.mode_set(mode='EDIT', toggle=False)
399
400
401
402     # Get the average bone length for zero length bones, we may not use this.
403     average_bone_length= 0.0
404     nonzero_count= 0
405     for bvh_node in bvh_nodes.values():
406         l= (bvh_node.rest_head_local-bvh_node.rest_tail_local).length
407         if l:
408             average_bone_length+= l
409             nonzero_count+=1
410
411     # Very rare cases all bones couldbe zero length???
412     if not average_bone_length:
413         average_bone_length = 0.1
414     else:
415         # Normal operation
416         average_bone_length = average_bone_length/nonzero_count
417
418
419 #XXX - sloppy operator code
420
421     bpy.ops.armature.delete()
422     bpy.ops.armature.select_all()
423     bpy.ops.armature.delete()
424
425     ZERO_AREA_BONES= []
426     for name, bvh_node in bvh_nodes.items():
427         # New editbone
428         bpy.ops.armature.bone_primitive_add(name="Bone")
429
430 #XXX            bone= bvh_node.temp= Blender.Armature.Editbone()
431         bone= bvh_node.temp= arm_data.edit_bones[-1]
432
433         bone.name= name
434 #               arm_data.bones[name]= bone
435
436         bone.head= bvh_node.rest_head_world
437         bone.tail= bvh_node.rest_tail_world
438
439         # ZERO AREA BONES.
440         if (bone.head-bone.tail).length < 0.001:
441             if bvh_node.parent:
442                 ofs= bvh_node.parent.rest_head_local- bvh_node.parent.rest_tail_local
443                 if ofs.length: # is our parent zero length also?? unlikely
444                     bone.tail= bone.tail+ofs
445                 else:
446                     bone.tail.y= bone.tail.y+average_bone_length
447             else:
448                 bone.tail.y= bone.tail.y+average_bone_length
449
450             ZERO_AREA_BONES.append(bone.name)
451
452
453     for bvh_node in bvh_nodes.values():
454         if bvh_node.parent:
455             # bvh_node.temp is the Editbone
456
457             # Set the bone parent
458             bvh_node.temp.parent= bvh_node.parent.temp
459
460             # Set the connection state
461             if not bvh_node.has_loc and\
462             bvh_node.parent and\
463             bvh_node.parent.temp.name not in ZERO_AREA_BONES and\
464             bvh_node.parent.rest_tail_local == bvh_node.rest_head_local:
465                 bvh_node.temp.connected= True
466
467     # Replace the editbone with the editbone name,
468     # to avoid memory errors accessing the editbone outside editmode
469     for bvh_node in bvh_nodes.values():
470         bvh_node.temp= bvh_node.temp.name
471
472 #XXX    arm_data.update()
473
474     # Now Apply the animation to the armature
475
476     # Get armature animation data
477     bpy.ops.object.mode_set(mode='OBJECT', toggle=False)
478     bpy.ops.object.mode_set(mode='POSE', toggle=False)
479
480     pose= arm_ob.pose
481     pose_bones= pose.bones
482
483
484     if ROT_STYLE=='NATIVE':
485         eul_order_lookup = {\
486             (0,1,2):'XYZ',
487             (0,2,1):'XZY',
488             (1,0,2):'YXZ',
489             (1,2,0):'YZX',
490             (2,0,1):'ZXY',
491             (2,1,0):'ZYZ'}
492
493         for bvh_node in bvh_nodes.values():
494             bone_name= bvh_node.temp # may not be the same name as the bvh_node, could have been shortened.
495             pose_bone= pose_bones[bone_name]
496             pose_bone.rotation_mode  = eul_order_lookup[tuple(bvh_node.rot_order)]
497
498     elif ROT_STYLE=='XYZ':
499         for pose_bone in pose_bones:
500             pose_bone.rotation_mode  = 'XYZ'
501     else:
502         # Quats default
503         pass
504
505
506     bpy.ops.pose.select_all() # set
507     bpy.ops.anim.keyframe_insert_menu(type=-4) # XXX -     -4 ???
508
509
510
511
512
513     #for p in pose_bones:
514     #   print(p)
515
516
517 #XXX    action = Blender.Armature.NLA.NewAction("Action")
518 #XXX    action.setActive(arm_ob)
519
520     #bpy.ops.action.new()
521     #action = bpy.data.actions[-1]
522
523     # arm_ob.animation_data.action = action
524     action = arm_ob.animation_data.action
525
526
527
528
529     #xformConstants= [ Blender.Object.Pose.LOC, Blender.Object.Pose.ROT ]
530
531     # Replace the bvh_node.temp (currently an editbone)
532     # With a tuple  (pose_bone, armature_bone, bone_rest_matrix, bone_rest_matrix_inv)
533     for bvh_node in bvh_nodes.values():
534         bone_name= bvh_node.temp # may not be the same name as the bvh_node, could have been shortened.
535         pose_bone= pose_bones[bone_name]
536         rest_bone= arm_data.bones[bone_name]
537 #XXX            bone_rest_matrix = rest_bone.matrix['ARMATURESPACE'].rotation_part()
538         bone_rest_matrix = rest_bone.matrix.rotation_part()
539
540
541         bone_rest_matrix_inv= Matrix(bone_rest_matrix)
542         bone_rest_matrix_inv.invert()
543
544         bone_rest_matrix_inv.resize4x4()
545         bone_rest_matrix.resize4x4()
546         bvh_node.temp= (pose_bone, bone, bone_rest_matrix, bone_rest_matrix_inv)
547
548
549     # Make a dict for fast access without rebuilding a list all the time.
550     '''
551     xformConstants_dict={
552     (True,True):        [Blender.Object.Pose.LOC, Blender.Object.Pose.ROT],\
553     (False,True):       [Blender.Object.Pose.ROT],\
554     (True,False):       [Blender.Object.Pose.LOC],\
555     (False,False):      [],\
556     }
557     '''
558
559     # KEYFRAME METHOD, SLOW, USE IPOS DIRECT
560     # TODO: use f-point samples instead (Aligorith)
561
562     # Animate the data, the last used bvh_node will do since they all have the same number of frames
563     for current_frame in range(len(bvh_node.anim_data)-1): # skip the first frame (rest frame)
564         # print current_frame
565
566         #if current_frame==150: # debugging
567         #       break
568
569         # Dont neet to set the current frame
570         for bvh_node in bvh_nodes.values():
571             pose_bone, bone, bone_rest_matrix, bone_rest_matrix_inv= bvh_node.temp
572             lx,ly,lz,rx,ry,rz= bvh_node.anim_data[current_frame+1]
573
574             if bvh_node.has_rot:
575
576                 if ROT_STYLE=='QUAT':
577                     # Set the rotation, not so simple
578                     bone_rotation_matrix= Euler(math.radians(rx), math.radians(ry), math.radians(rz)).to_matrix()
579
580                     bone_rotation_matrix.resize4x4()
581                     #XXX ORDER CHANGE???
582                     #pose_bone.rotation_quaternion= (bone_rest_matrix * bone_rotation_matrix * bone_rest_matrix_inv).to_quat() # ORIGINAL
583                     # pose_bone.rotation_quaternion= (bone_rest_matrix_inv * bone_rotation_matrix * bone_rest_matrix).to_quat()
584                     # pose_bone.rotation_quaternion= (bone_rotation_matrix * bone_rest_matrix).to_quat() # BAD
585                     # pose_bone.rotation_quaternion= bone_rotation_matrix.to_quat() # NOT GOOD
586                     # pose_bone.rotation_quaternion= bone_rotation_matrix.to_quat() # NOT GOOD
587
588                     #pose_bone.rotation_quaternion= (bone_rotation_matrix * bone_rest_matrix_inv * bone_rest_matrix).to_quat()
589                     #pose_bone.rotation_quaternion= (bone_rest_matrix_inv * bone_rest_matrix * bone_rotation_matrix).to_quat()
590                     #pose_bone.rotation_quaternion= (bone_rest_matrix * bone_rotation_matrix * bone_rest_matrix_inv).to_quat()
591
592                     #pose_bone.rotation_quaternion= ( bone_rest_matrix* bone_rest_matrix_inv * bone_rotation_matrix).to_quat()
593                     #pose_bone.rotation_quaternion= (bone_rotation_matrix * bone_rest_matrix  * bone_rest_matrix_inv).to_quat()
594                     #pose_bone.rotation_quaternion= (bone_rest_matrix_inv * bone_rotation_matrix  * bone_rest_matrix ).to_quat()
595
596                     pose_bone.rotation_quaternion= (bone_rest_matrix_inv * bone_rotation_matrix * bone_rest_matrix).to_quat()
597
598                 else:
599                     bone_rotation_matrix= Euler(math.radians(rx), math.radians(ry), math.radians(rz)).to_matrix()
600                     bone_rotation_matrix.resize4x4()
601
602                     eul= (bone_rest_matrix * bone_rotation_matrix * bone_rest_matrix_inv).to_euler()
603
604                     #pose_bone.rotation_euler = math.radians(rx), math.radians(ry), math.radians(rz)
605                     pose_bone.rotation_euler = eul
606
607                 print("ROTATION" + str(Euler(math.radians(rx), math.radians(ry), math.radians(rz))))
608
609             if bvh_node.has_loc:
610                 # Set the Location, simple too
611
612                 #XXX ORDER CHANGE
613                 # pose_bone.location= (TranslationMatrix(Vector(lx, ly, lz) - bvh_node.rest_head_local ) * bone_rest_matrix_inv).translation_part() # WHY * 10? - just how pose works
614                 # pose_bone.location= (bone_rest_matrix_inv * TranslationMatrix(Vector(lx, ly, lz) - bvh_node.rest_head_local )).translation_part()
615                 # pose_bone.location= lx, ly, lz
616                 pose_bone.location= Vector(lx, ly, lz) - bvh_node.rest_head_local
617
618
619 #XXX            # TODO- add in 2.5
620             if 0:
621                 # Get the transform
622                 xformConstants = xformConstants_dict[bvh_node.has_loc, bvh_node.has_rot]
623
624                 if xformConstants:
625                     # Insert the keyframe from the loc/quat
626                     pose_bone.insertKey(arm_ob, current_frame + IMPORT_START_FRAME, xformConstants, True)
627             else:
628
629                 if bvh_node.has_loc:
630                     pose_bone.keyframe_insert("location")
631                 if bvh_node.has_rot:
632                     if ROT_STYLE == 'QUAT':
633                         pose_bone.keyframe_insert("rotation_quaternion")
634                     else:
635                         pose_bone.keyframe_insert("rotation_euler")
636
637
638         # bpy.ops.anim.keyframe_insert_menu(type=-4) # XXX -     -4 ???
639         bpy.ops.screen.frame_offset(delta=1)
640
641         # First time, set the IPO's to linear
642 #XXX    #TODO
643         if 0:
644             if current_frame == 0:
645                 for ipo in action.getAllChannelIpos().values():
646                     if ipo:
647                         for cur in ipo:
648                             cur.interpolation = Blender.IpoCurve.InterpTypes.LINEAR
649                             if IMPORT_LOOP:
650                                 cur.extend = Blender.IpoCurve.ExtendTypes.CYCLIC
651
652
653         else:
654             for cu in action.fcurves:
655                 for bez in cu.keyframe_points:
656                     bez.interpolation = 'CONSTANT'
657
658     # END KEYFRAME METHOD
659
660
661     """
662     # IPO KEYFRAME SETTING
663     # Add in the IPOs by adding keyframes, AFAIK theres no way to add IPOs to an action so I do this :/
664     for bvh_node in bvh_nodes.values():
665         pose_bone, bone, bone_rest_matrix, bone_rest_matrix_inv= bvh_node.temp
666
667         # Get the transform
668         xformConstants= xformConstants_dict[bvh_node.has_loc, bvh_node.has_rot]
669         if xformConstants:
670             pose_bone.loc[:]= 0,0,0
671             pose_bone.quat[:]= 0,0,1,0
672             # Insert the keyframe from the loc/quat
673             pose_bone.insertKey(arm_ob, IMPORT_START_FRAME, xformConstants)
674
675
676     action_ipos= action.getAllChannelIpos()
677
678
679     for bvh_node in bvh_nodes.values():
680         has_loc= bvh_node.has_loc
681         has_rot= bvh_node.has_rot
682
683         if not has_rot and not has_loc:
684             # No animation data
685             continue
686
687         ipo= action_ipos[bvh_node.temp[0].name] # posebones name as key
688
689         if has_loc:
690             curve_xloc= ipo[Blender.Ipo.PO_LOCX]
691             curve_yloc= ipo[Blender.Ipo.PO_LOCY]
692             curve_zloc= ipo[Blender.Ipo.PO_LOCZ]
693
694             curve_xloc.interpolation= \
695             curve_yloc.interpolation= \
696             curve_zloc.interpolation= \
697             Blender.IpoCurve.InterpTypes.LINEAR
698
699
700         if has_rot:
701             curve_wquat= ipo[Blender.Ipo.PO_QUATW]
702             curve_xquat= ipo[Blender.Ipo.PO_QUATX]
703             curve_yquat= ipo[Blender.Ipo.PO_QUATY]
704             curve_zquat= ipo[Blender.Ipo.PO_QUATZ]
705
706             curve_wquat.interpolation= \
707             curve_xquat.interpolation= \
708             curve_yquat.interpolation= \
709             curve_zquat.interpolation= \
710             Blender.IpoCurve.InterpTypes.LINEAR
711
712         # Get the bone
713         pose_bone, bone, bone_rest_matrix, bone_rest_matrix_inv= bvh_node.temp
714
715
716         def pose_rot(anim_data):
717             bone_rotation_matrix= Euler(anim_data[3], anim_data[4], anim_data[5]).to_matrix()
718             bone_rotation_matrix.resize4x4()
719             return tuple((bone_rest_matrix * bone_rotation_matrix * bone_rest_matrix_inv).to_quat()) # qw,qx,qy,qz
720
721         def pose_loc(anim_data):
722             return tuple((TranslationMatrix(Vector(anim_data[0], anim_data[1], anim_data[2])) * bone_rest_matrix_inv).translation_part())
723
724
725         last_frame= len(bvh_node.anim_data)+IMPORT_START_FRAME-1
726
727         if has_loc:
728             pose_locations= [pose_loc(anim_key) for anim_key in bvh_node.anim_data]
729
730             # Add the start at the end, we know the start is just 0,0,0 anyway
731             curve_xloc.append((last_frame, pose_locations[-1][0]))
732             curve_yloc.append((last_frame, pose_locations[-1][1]))
733             curve_zloc.append((last_frame, pose_locations[-1][2]))
734
735             if len(pose_locations) > 1:
736                 ox,oy,oz= pose_locations[0]
737                 x,y,z= pose_locations[1]
738
739                 for i in range(1, len(pose_locations)-1): # from second frame to second last frame
740
741                     nx,ny,nz= pose_locations[i+1]
742                     xset= yset= zset= True # we set all these by default
743                     if abs((ox+nx)/2 - x) < 0.00001:    xset= False
744                     if abs((oy+ny)/2 - y) < 0.00001:    yset= False
745                     if abs((oz+nz)/2 - z) < 0.00001:    zset= False
746
747                     if xset: curve_xloc.append((i+IMPORT_START_FRAME, x))
748                     if yset: curve_yloc.append((i+IMPORT_START_FRAME, y))
749                     if zset: curve_zloc.append((i+IMPORT_START_FRAME, z))
750
751                     # Set the old and use the new
752                     ox,oy,oz=   x,y,z
753                     x,y,z=              nx,ny,nz
754
755
756         if has_rot:
757             pose_rotations= [pose_rot(anim_key) for anim_key in bvh_node.anim_data]
758
759             # Add the start at the end, we know the start is just 0,0,0 anyway
760             curve_wquat.append((last_frame, pose_rotations[-1][0]))
761             curve_xquat.append((last_frame, pose_rotations[-1][1]))
762             curve_yquat.append((last_frame, pose_rotations[-1][2]))
763             curve_zquat.append((last_frame, pose_rotations[-1][3]))
764
765
766             if len(pose_rotations) > 1:
767                 ow,ox,oy,oz= pose_rotations[0]
768                 w,x,y,z= pose_rotations[1]
769
770                 for i in range(1, len(pose_rotations)-1): # from second frame to second last frame
771
772                     nw, nx,ny,nz= pose_rotations[i+1]
773                     wset= xset= yset= zset= True # we set all these by default
774                     if abs((ow+nw)/2 - w) < 0.00001:    wset= False
775                     if abs((ox+nx)/2 - x) < 0.00001:    xset= False
776                     if abs((oy+ny)/2 - y) < 0.00001:    yset= False
777                     if abs((oz+nz)/2 - z) < 0.00001:    zset= False
778
779                     if wset: curve_wquat.append((i+IMPORT_START_FRAME, w))
780                     if xset: curve_xquat.append((i+IMPORT_START_FRAME, x))
781                     if yset: curve_yquat.append((i+IMPORT_START_FRAME, y))
782                     if zset: curve_zquat.append((i+IMPORT_START_FRAME, z))
783
784                     # Set the old and use the new
785                     ow,ox,oy,oz=        w,x,y,z
786                     w,x,y,z=            nw,nx,ny,nz
787
788     # IPO KEYFRAME SETTING
789     """
790
791 # XXX NOT NEEDED NOW?
792     # pose.update()
793     return arm_ob
794
795
796 #=============#
797 # TESTING     #
798 #=============#
799
800 #('/metavr/mocap/bvh/boxer.bvh')
801 #('/d/staggered_walk.bvh')
802 #('/metavr/mocap/bvh/dg-306-g.bvh') # Incompleate EOF
803 #('/metavr/mocap/bvh/wa8lk.bvh') # duplicate joint names, \r line endings.
804 #('/metavr/mocap/bvh/walk4.bvh') # 0 channels
805
806 '''
807 import os
808 DIR = '/metavr/mocap/bvh/'
809 for f in ('/d/staggered_walk.bvh',):
810     #for f in os.listdir(DIR)[5:6]:
811     #for f in os.listdir(DIR):
812     if f.endswith('.bvh'):
813         s = Blender.Scene.New(f)
814         s.makeCurrent()
815         #file= DIR + f
816         file= f
817         print f
818         bvh_nodes= read_bvh(file, 1.0)
819         bvh_node_dict2armature(bvh_nodes, 1)
820 '''
821
822
823 def load_bvh_ui(context, file, PREF_UI=False):
824
825 #XXX    if BPyMessages.Error_NoFile(file):
826 #XXX            return
827
828 #XXX    Draw= Blender.Draw
829
830     IMPORT_SCALE = 0.1
831     IMPORT_START_FRAME = 1
832     IMPORT_AS_ARMATURE = 1
833     IMPORT_AS_EMPTIES = 0
834     IMPORT_LOOP = 0
835
836     # Get USER Options
837     if PREF_UI:
838         pup_block = [\
839         ('As Armature', IMPORT_AS_ARMATURE, 'Imports the BVH as an armature'),\
840         ('As Empties', IMPORT_AS_EMPTIES, 'Imports the BVH as empties'),\
841         ('Scale: ', IMPORT_SCALE, 0.001, 100.0, 'Scale the BVH, Use 0.01 when 1.0 is 1 metre'),\
842         ('Start Frame: ', IMPORT_START_FRAME, 1, 30000, 'Frame to start BVH motion'),\
843         ('Loop Animation', IMPORT_LOOP, 'Enable cyclic IPOs'),\
844         ]
845
846 #XXX            if not Draw.PupBlock('BVH Import...', pup_block):
847 #XXX                    return
848
849     # print('Attempting import BVH', file)
850
851     if not IMPORT_AS_ARMATURE and not IMPORT_AS_EMPTIES:
852         raise('No import option selected')
853
854 #XXX    Blender.Window.WaitCursor(1)
855     # Get the BVH data and act on it.
856     import time
857     t1 = time.time()
858     print('\tparsing bvh...', end="")
859     bvh_nodes = read_bvh(context, file, IMPORT_SCALE)
860     print('%.4f' % (time.time() - t1))
861     t1 = time.time()
862     print('\timporting to blender...', end="")
863     if IMPORT_AS_ARMATURE:
864         bvh_node_dict2armature(context, bvh_nodes, IMPORT_START_FRAME, IMPORT_LOOP)
865     if IMPORT_AS_EMPTIES:
866         bvh_node_dict2objects(context, bvh_nodes, IMPORT_START_FRAME, IMPORT_LOOP)
867
868     print('Done in %.4f\n' % (time.time() - t1))
869 #XXX    Blender.Window.WaitCursor(0)
870
871
872 def main():
873     Blender.Window.FileSelector(load_bvh_ui, 'Import BVH', '*.bvh')
874
875 from bpy.props import *
876
877
878 class BvhImporter(bpy.types.Operator):
879     '''Load a Wavefront OBJ File.'''
880     bl_idname = "import_anim.bvh"
881     bl_label = "Import BVH"
882
883     path = StringProperty(name="File Path", description="File path used for importing the OBJ file", maxlen=1024, default="")
884
885     def execute(self, context):
886         # print("Selected: " + context.active_object.name)
887
888         read_bvh(context, self.properties.path)
889
890         return {'FINISHED'}
891
892     def invoke(self, context, event):
893         wm = context.manager
894         wm.add_fileselect(self)
895         return {'RUNNING_MODAL'}
896
897
898 menu_func = lambda self, context: self.layout.operator(BvhImporter.bl_idname, text="Motion Capture (.bvh)...")
899
900 def register():
901     bpy.types.register(BvhImporter)
902     bpy.types.INFO_MT_file_export.append(menu_func)
903     
904 def unregister():
905     bpy.types.unregister(BvhImporter)
906     bpy.types.INFO_MT_file_export.remove(menu_func)