In which order are Biovision Hierarchy transforms applied?

Question

I'm trying to display Biovision Hierarchy mocap file with pygame. For now, in the orthographic projection, the perspective would be z division. I seem to have bones imported, but I cannot choose the right axes even in still armature (XY and XZ are not working).

The most funny thing happens when I'm trying to apply the animation. It looks like a contortion of the android from Blade Runner.

Show me how to fix it. I'm using the 3dsMax version of CMU mocap.

Here's my code:

import numpy as np

class Cell(list):
    def getname(self):
        if hasattr(self, "name"):
            return self.name
        return ""
    def setname(self, name):
        self.name=name
    def __repr__(self):
        if hasattr(self, "name"):
            if self:
                return self.name+" "+list.__repr__(self)
            return self.name
        return list.__repr__(self)

class Node(object):
    def __init__(self, name, parent, offset, channels):
        self.name=name
        self.parent=parent
        self.children=[]
        self.offset=offset
        self.channels=channels
    def add_child(self, child):
        self.children.append(child)
    def get_bones(self):
        bones=[]
        if self.parent is not None:
            bones.append((self.parent.abspos(),self.abspos()))
        for child in self.children:
            bones.extend(child.get_bones())
        return bones
    def abspos(self):
        if self.parent is None:
            return np.dot(self.get_matrix(),np.hstack((self.offset, 1)))[:3]#also need rotation
        return self.parent.abspos()+np.dot(self.get_matrix(),np.hstack((self.offset, 1)))[:3]#also need rotation
    def __repr__(self):
        return "<"+self.name+" ["+", ".join([repr(child) for child in self.children])+"]>"
    def feed_transform(self, transform):
        obj=transform
        matrix=np.zeros((4,4))
        matrix[0,0]=1
        matrix[1,1]=1
        matrix[2,2]=1
        matrix[3,3]=1
        for channel in self.channels:
            if channel=="Xposition":
                mod=np.zeros((4,4))
                mod[0,0]=1
                mod[1,1]=1
                mod[2,2]=1
                mod[3,3]=1
                mod[3,0]=obj[0]
                obj=obj[1:]
                matrix=np.dot(matrix, mod)
            elif channel=="Yposition":
                mod=np.zeros((4,4))
                mod[0,0]=1
                mod[1,1]=1
                mod[2,2]=1
                mod[3,3]=1
                mod[3,1]=obj[0]
                obj=obj[1:]
                matrix=np.dot(matrix, mod)
            elif channel=="Zposition":
                mod=np.zeros((4,4))
                mod[0,0]=1
                mod[1,1]=1
                mod[2,2]=1
                mod[3,3]=1
                mod[3,2]=obj[0]
                obj=obj[1:]
                matrix=np.dot(matrix, mod)
            elif channel=="Xrotation":
                mod=np.zeros((4,4))
                mod[0,0]=1
                mod[1,1]=np.cos(obj[0])
                mod[2,2]=np.cos(obj[0])
                mod[1,2]=np.sin(obj[0])
                mod[2,1]=-np.sin(obj[0])
                mod[3,3]=1
                obj=obj[1:]
                matrix=np.dot(matrix, mod)
            elif channel=="Yrotation":
                mod=np.zeros((4,4))
                mod[1,1]=1
                mod[0,0]=np.cos(obj[0])
                mod[2,2]=np.cos(obj[0])
                mod[2,0]=np.sin(obj[0])
                mod[0,2]=-np.sin(obj[0])
                mod[3,3]=1
                obj=obj[1:]
                matrix=np.dot(matrix, mod)
            elif channel=="Zrotation":
                mod=np.zeros((4,4))
                mod[2,2]=1
                mod[0,0]=np.cos(obj[0])
                mod[1,1]=np.cos(obj[0])
                mod[0,1]=np.sin(obj[0])
                mod[1,0]=-np.sin(obj[0])
                mod[3,3]=1
                obj=obj[1:]
                matrix=np.dot(matrix, mod)
        self.matrix=matrix
        for child in self.children:
            obj=child.feed_transform(obj)
        return obj
    def get_matrix(self):
        if self.parent is None:
            return self.matrix
        return np.dot(self.matrix,self.parent.get_matrix())
def parse(obj, parent=None):
    if obj is None:
        return None
    tp, nm=obj.getname().split(" ")
    offset=None
    channels=None
    joints=[]
    if tp not in ("ROOT", "JOINT"):
        print("warning", tp)
    for line in obj:
        if line.getname().startswith("OFFSET"):
            offset=np.array([float(x) for x in line.getname().split(" ")[1:4]])
        elif line.getname().startswith("CHANNELS"):
            channels=tuple(line.getname().split(" ")[2:])
        elif line.getname().startswith("JOINT"):
            joints.append(line)
    res=Node(nm, parent, offset, channels)
    for joint in joints:
        res.add_child(parse(joint, parent=res))
    return res

tree=Cell()

depth=0
with open("/media/stuntj/ESD-ISO/cmu/01/01_01.bvh","r") as f:
    for line in f:
        ls=line.strip(" \t\r\n")
        obj=tree
        for i in range(depth):
            obj=obj[-1]
        if not ls:
            pass
        elif ls=="{":
            depth+=1
        elif ls=="}":
            depth-=1
        else:
            obj.append(Cell())
            obj[-1].setname(ls)

#print(tree)
hier=False
moti=False
parsed=[]
frames=[]
fps=30
for line in tree:
    if line.getname() == "HIERARCHY":
        hier=True
        moti=False
    elif line.getname() == "MOTION":
        hier=False
        moti=True
    elif hier:
        tp, nm=line.getname().split(" ")
        if tp == "ROOT":
            parsed.append(parse(line))
    elif moti:
        if line.getname().startswith("Frame Time:"):
            fps=int(1.0/float(line.getname()[11:].strip(" \t\r\n"))+.5)
        elif line.getname()[0:1] in "-.0123456789":
            frames.append([float(x) for x in line.getname().split(" ")])
#print(parsed)
#print(frames)
#print(fps)
#print(parsed[0].get_bones())import pygame, sys
import pygame, sys
from pygame.locals import *
pygame.init()
DISPLAYSURF = pygame.display.set_mode((800, 800))
pygame.display.set_caption('Yet Another BVH Lexer')
fpsClock = pygame.time.Clock()
WHITE = (255, 255, 255)
BLACK = (0, 0, 0)
for frame in frames:
    obj=frame
    for ps in parsed:
        obj=ps.feed_transform(obj)
        DISPLAYSURF.fill(WHITE)
        for bone in ps.get_bones():
            pygame.draw.line(DISPLAYSURF, BLACK, (bone[0][0]+400,bone[0][2]+400), (bone[1][0]*8+400,bone[1][2]*-8+400),1)
        pygame.display.update()
    if obj:
        print("Malformed data")
    fpsClock.tick(fps)
pygame.quit()
sys.exit()

Answer 1

It's fixed. The key problem was rotation in BVH was specified in degrees, not the radians. Then some brute forse on the matrices, and it's done.

import numpy as np

class Node(object):
    def __init__(self, name, parent, offset, channels):
        self.name=name
        self.parent=parent
        self.children=[]
        self.offset=offset
        self.channels=channels
        matrix=np.zeros((4,4))
        matrix[0,0]=1
        matrix[1,1]=1
        matrix[2,2]=1
        matrix[3,3]=1
        self.matrix=matrix
    def add_child(self, child):
        self.children.append(child)
    def get_bones(self):
        bones=[]
        if self.parent is not None and self.parent.parent is not None:
            bones.append((self.parent.abspos(),self.abspos()))
        for child in self.children:
            bones.extend(child.get_bones())
        return bones
    def abspos(self):
        if self.parent is None:
            return np.dot(self.get_matrix(),np.hstack((self.offset, 1)))[:3]
        return self.parent.abspos()+np.dot(self.get_matrix(),np.hstack((self.offset, 1)))[:3]+self.parent.trans
    def __repr__(self):
        return "<"+self.name+" ["+", ".join([repr(child) for child in self.children])+"]>"
    def feed_transform(self, transform):
        obj=transform
        trans=np.zeros(3)
        matrix=np.zeros((4,4))
        matrix[0,0]=1
        matrix[1,1]=1
        matrix[2,2]=1
        matrix[3,3]=1
        for channel in self.channels:
            if channel=="Xposition":
                trans[0]+=obj[0]
                obj=obj[1:]
            elif channel=="Yposition":
                trans[1]+=obj[0]
                obj=obj[1:]
            elif channel=="Zposition":
                trans[2]+=obj[0]
                obj=obj[1:]
            elif channel=="Xrotation":
                rad=obj[0]*(np.pi/180.0)
                mod=np.zeros((4,4))
                mod[0,0]=1
                mod[1,1]=np.cos(rad)
                mod[2,2]=np.cos(rad)
                mod[2,1]=np.sin(rad)
                mod[1,2]=-np.sin(rad)
                mod[3,3]=1
                obj=obj[1:]
                matrix=np.dot(matrix, mod)
            elif channel=="Yrotation":
                rad=obj[0]*(np.pi/180.0)
                mod=np.zeros((4,4))
                mod[1,1]=1
                mod[0,0]=np.cos(rad)
                mod[2,2]=np.cos(rad)
                mod[0,2]=np.sin(rad)
                mod[2,0]=-np.sin(rad)
                mod[3,3]=1
                obj=obj[1:]
                matrix=np.dot(matrix, mod)
            elif channel=="Zrotation":
                rad=obj[0]*(np.pi/180.0)
                mod=np.zeros((4,4))
                mod[2,2]=1
                mod[0,0]=np.cos(rad)
                mod[1,1]=np.cos(rad)
                mod[0,1]=-np.sin(rad)
                mod[1,0]=np.sin(rad)
                mod[3,3]=1
                obj=obj[1:]
                matrix=np.dot(matrix, mod)
        self.trans=trans
        self.matrix=matrix
        for child in self.children:
            obj=child.feed_transform(obj)
        return obj
    def get_matrix(self):
        if self.parent is None:
            matrix=np.zeros((4,4))
            matrix[0,0]=1
            matrix[1,1]=1
            matrix[2,2]=1
            matrix[3,3]=1
            return matrix
        return np.dot(self.parent.get_matrix(), self.parent.matrix)
def parse(obj, parent=None):
    if obj[0]=="End Site":
        tp="End Site"
        nm=""
    else:
        tp, nm=obj[0].split(" ")
    offset=None
    channels=tuple()
    joints=[]
    if tp not in ("ROOT", "JOINT", "End Site"):
        print("warning", tp)
    for line in obj[1:]:
        if line[0].startswith("OFFSET"):
            offset=np.array([float(x) for x in line[0].split(" ")[1:4]])
        elif line[0].startswith("CHANNELS"):
            channels=tuple(line[0].split(" ")[2:])
        elif line[0].startswith("JOINT"):
            joints.append(line)
        elif line[0].startswith("End Site"):
            joints.append(line)
    res=Node(nm, parent, offset, channels)
    for joint in joints:
        res.add_child(parse(joint, parent=res))
    return res

tree=[""]

depth=0
with open("/media/stuntj/ESD-ISO/cmu/01/01_01.bvh","r") as f:
    for line in f:
        ls=line.strip(" \t\r\n")
        obj=tree
        for i in range(depth):
            obj=obj[-1]
        if not ls:
            pass
        elif ls=="{":
            depth+=1
        elif ls=="}":
            depth-=1
        else:
            obj.append([ls])

hier=False
moti=False
parsed=[]
frames=[]
fps=30
for line in tree[1:]:
    if line[0] == "HIERARCHY":
        hier=True
        moti=False
    elif line[0] == "MOTION":
        hier=False
        moti=True
    elif hier:
        tp, nm=line[0].split(" ")
        if tp == "ROOT":
            parsed.append(parse(line))
    elif moti:
        if line[0].startswith("Frame Time:"):
            fps=int(1.0/float(line[0][11:].strip(" \t\r\n"))+.5)
        elif line[0][0:1] in "-.0123456789":
            frames.append([float(x) for x in line[0].split(" ")])

import pygame, sys
from pygame.locals import *
pygame.init()
DISPLAYSURF = pygame.display.set_mode((800, 800))
pygame.display.set_caption('Yet Another BVH Lexer')
fpsClock = pygame.time.Clock()
WHITE = (255, 255, 255)
BLACK = (0, 0, 0)
for frame in frames:
    obj=frame
    for ps in parsed:
        obj=ps.feed_transform(obj)
        DISPLAYSURF.fill(WHITE)
        for bone in ps.get_bones():
            if bone[0][2]<-.1 and bone[1][2]<-.1:
                pygame.draw.line(DISPLAYSURF, BLACK, (bone[0][0]/bone[0][2]*-64+400,bone[0][1]/bone[0][2]*64+400), (bone[1][0]/bone[1][2]*-64+400,bone[1][1]/bone[1][2]*64+400),1)
        pygame.display.update()
    if obj:
        print("Malformed data")
    fpsClock.tick(fps)
pygame.quit()
sys.exit()