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