#!/usr/bin/python
# coding: utf-8
import sys
import os
import math
import numpy
def to_radian(deglee):
return math.pi * deglee / 180.0
###############################################################################
# Channel
###############################################################################
class Channel(object):
__slots__=['index', 'motion', 'matrix']
def __init__(self, index=0):
self.index=index
self.motion=[]
self.matrix=numpy.identity(4)
def getValue(self, frame):
assert(false)
def getMatrix(self, frame):
assert(false)
class ZeroChannel(Channel):
__slots__=[]
def getValue(self, frame):
return 0
def getMatrix(self):
return numpy.identity(4)
class ChannelPositionX(Channel):
__slots__=[]
def __str__(self):
return "px"
def getValue(self, frame):
return self.motion[frame]
def getMatrix(self, frame):
"""
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[x, 0, 0, 1],
"""
self.matrix[3, 0]=self.getValue(frame)
return self.matrix
class ChannelPositionY(Channel):
__slots__=[]
def __str__(self):
return "py"
def getValue(self, frame):
return self.motion[frame]
def getMatrix(self, frame):
"""
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, z, 1],
"""
self.matrix[3, 1]=self.getValue(frame)
return self.matrix
class ChannelPositionZ(Channel):
__slots__=[]
def __str__(self):
return "pz"
def getValue(self, frame):
return self.motion[frame]
def getMatrix(self, frame):
"""
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, z, 1],
"""
self.matrix[3, 2]=self.getValue(frame)
return self.matrix
class ChannelRotationX(Channel):
__slots__=[]
def __str__(self):
return "rx"
def getValue(self, frame):
return self.motion[frame]
def getMatrix(self, frame):
"""
[1, 0, 0, 0],
[0, c, s, 0],
[0,-s, c, 0],
[0, 0, 0, 1],
"""
radian=to_radian(self.getValue(frame))
c=math.cos(radian)
s=math.sin(radian)
self.matrix[1, 1]=c
self.matrix[1, 2]=s
self.matrix[2, 1]=-s
self.matrix[2, 2]=c
return self.matrix
class ChannelRotationY(Channel):
__slots__=[]
def __str__(self):
return "ry"
def getValue(self, frame):
return self.motion[frame]
def getMatrix(self, frame):
"""
[c, 0,-s, 0],
[0, 1, 0, 0],
[s, 0, c, 0],
[0, 0, 0, 1],
"""
radian=to_radian(self.getValue(frame))
c=math.cos(radian)
s=math.sin(radian)
self.matrix[2, 2]=c
self.matrix[2, 0]=s
self.matrix[0, 2]=-s
self.matrix[0, 0]=c
return self.matrix
class ChannelRotationZ(Channel):
__slots__=[]
def __str__(self):
return "rz"
def getValue(self, frame):
return self.motion[frame]
def getMatrix(self, frame):
"""
[ c, s, 0, 0],
[-s, c, 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1],
"""
radian=to_radian(self.getValue(frame))
r=numpy.identity(4)
c=math.cos(radian)
s=math.sin(radian)
self.matrix[0, 0]=c
self.matrix[0, 1]=s
self.matrix[1, 0]=-s
self.matrix[1, 1]=c
return self.matrix
channel_map={
'Xposition': ChannelPositionX,
'Yposition': ChannelPositionY,
'Zposition': ChannelPositionZ,
'Xrotation': ChannelRotationX,
'Yrotation': ChannelRotationY,
'Zrotation': ChannelRotationZ,
}
def createChannel(key, index):
if key in channel_map:
return channel_map[key](index)
raise ValueError("unkown key: %s" % key)
###############################################################################
# Joint
###############################################################################
class Joint(object):
__slots__=['name', 'parent', 'children',
'offset', 'tail', 'channels', 'channelMap']
def __init__(self, name):
self.name=name
self.parent=None
self.children=[]
self.offset=None
self.tail=None
self.channels=[]
self.channelMap={
ChannelPositionX: ZeroChannel(),
ChannelPositionY: ZeroChannel(),
ChannelPositionZ: ZeroChannel(),
ChannelRotationX: ZeroChannel(),
ChannelRotationY: ZeroChannel(),
ChannelRotationZ: ZeroChannel(),
}
def addChild(self, child):
self.children.append(child)
child.parent=self
return child
def addChannel(self, channel):
self.channels.append(channel)
self.channelMap[channel.__class__]=channel
def show(self, indent=''):
channels=' '.join(str(c) for c in self.channels)
if self.tail:
print "%s%s(%s) %f %f %f > %f %f %f" % (indent, self.name, channels,
self.offset[0], self.offset[1], self.offset[2],
self.tail[0], self.tail[1], self.tail[2]
)
else:
print "%s%s(%s) %f %f %f" % (indent, self.name, channels,
self.offset[0], self.offset[1], self.offset[2])
for child in self.children:
child.show(indent+' ')
def getOffsetMatrix(self):
t=numpy.identity(4)
t[3][0]=self.offset[0]
t[3][1]=self.offset[1]
t[3][2]=self.offset[2]
return t
def getMatrix(self, frame):
"""
各チャンネルの行列を左にかけていった結果を返す
"""
m=reduce(lambda acum, ch: numpy.dot(ch.getMatrix(frame), acum),
self.channels, numpy.identity(4))
return m
###############################################################################
# bvh Loader
#
# Jointの木構造を読み込む
#
# Root以外のチャンネルは、
# Joint
# Channel0(rotz)
# Channel1(rotx)
# Channel2(roty)
# という構造になっている。
#
# Rootチャンネルは
# Channel0(posx)
# Channel1(posy)
# Channel2(posz)
# Channel3(rotz)
# Channel4(rotx)
# Channel5(roty)
# というように移動チャンネルを余分にもつ
# 回転チャンネルは順不同(オイラー角の分解方法が違う場合がある)
#
# 各チャンネルにはモーションデータ
# Channel
# motion[m0, m1, m2, m3, m4,....]
#
# とデータを読み込んでおき
#
# Joint.getMatrix(frame)とすることで
# return Channel0.getMatrix(frame)
# * Channel1.getMatrix(frame)
# * Channel2.getMatrix(frame)
#
# 該当するフレームの行列を返すようにする
###############################################################################
class Loader(object):
"""
BVHローダ
"""
__slots__=[
'root', 'channels',
'frame_count', 'frame_interval',
'joint_list',
]
def __init__(self):
self.joint_list=[]
def load(self, path):
io=open(path, "rb")
if not io:
raise "fail to open %s" % path
return self.process(io)
def process(self, io):
self.channels=[]
if io.readline().strip()!="HIERARCHY":
raise "invalid signature"
# root
type, name=io.readline().strip().split()
self.root=Joint(name)
self.joint_list.append(self.root)
# joints
self.parseJoint(io, self.root)
# motion
self.parseMotion(io)
def parseJoint(self, io, joint):
line=io.readline().strip()
if line!="{":
raise "no {"
# position
type, x, y, z=io.readline().strip().split()
if type!="OFFSET":
raise "no OFFSET"
joint.offset=[float(x), float(y), float(z)]
# create channels
tokens=io.readline().strip().split()
if tokens.pop(0)!="CHANNELS":
raise "no CHANNELS"
channelCount=int(tokens.pop(0))
joint.channels=[]
for channel in tokens:
channel=createChannel(channel, len(self.channels))
joint.addChannel(channel)
self.channels.append(channel)
assert(len(joint.channels)==channelCount)
# チャンネルの出現順を確認する(ものによって違った)
if joint.parent:
# joint
assert(channelCount==3)
#assert(joint.channels[0].__class__ is ChannelRotationZ)
#assert(joint.channels[1].__class__ is ChannelRotationX)
#assert(joint.channels[2].__class__ is ChannelRotationY)
else:
# root
assert(channelCount==6)
#assert(joint.channels[0].__class__ is ChannelPositionX)
#assert(joint.channels[1].__class__ is ChannelPositionY)
#assert(joint.channels[2].__class__ is ChannelPositionZ)
#assert(joint.channels[3].__class__ is ChannelRotationZ)
#assert(joint.channels[4].__class__ is ChannelRotationX)
#assert(joint.channels[5].__class__ is ChannelRotationY)
# 入れ子のjointを読み取る
while True:
line=io.readline()
if line=="":
raise "invalid eof"
tokens=line.strip().split()
if tokens[0]=="JOINT":
# 子ジョイント
child=joint.addChild(Joint(tokens[1]))
self.joint_list.append(child)
self.parseJoint(io, child)
elif tokens[0]=="End":
# 末端
line=io.readline().strip()
if line!="{":
raise "no {"
type, x, y, z=io.readline().strip().split()
if type!="OFFSET":
raise "no OFFSET"
joint.tail=[float(x), float(y), float(z)]
if io.readline().strip()!="}":
raise "no }"
elif tokens[0]=="}":
return
else:
raise "unknown type"
def parseMotion(self, io):
line=io.readline().strip()
if line!="MOTION":
print line
raise "no MOTION"
type, frame_count=io.readline().strip().split()
if type!="Frames:":
raise "no Frames:"
tokens=io.readline().strip().split()
if tokens[0]!="Frame":
raise "no Frame"
if tokens[1]!="Time:":
raise "no Time:"
self.frame_count=int(frame_count)
self.frame_interval=float(tokens[2])
print "%d frames, %f sec/frame" % (
self.frame_count, self.frame_interval)
# モーション各行を読み込む
while True:
line=io.readline()
if line=="":
# eof
break
# モーション行を空白で分解
tokens=line.strip().split()
assert(len(tokens)==len(self.channels))
# 各チャンネルに分配する
for i, t in enumerate(tokens):
self.channels[i].motion.append(float(t))
def load(path):
l=Loader()
try:
l.load(path)
except Exception as e:
print 'Exception'
print e
return
return l
###############################################################################
if __name__=='__main__':
"""
テスト
"""
if len(sys.argv)==1:
print 'usage: %s {bvh file}' % sys.argv[0]
sys.exit()
print('load "%s"' % sys.argv[1])
import time
start=time.clock()
l=load(sys.argv[1])
if not l:
sys.exit()
print 'elapsed %f sec' % (time.clock()-start)
l.root.show()