Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Pushed updated code #3

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
103 changes: 99 additions & 4 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -1,5 +1,100 @@
*.pyc
### https://raw.github.com/github/gitignore/5b8eb2c3715a7d5c4fe322abe7f0873b8985d50b/Global/osx.gitignore

.DS_Store
.AppleDouble
.LSOverride

# Icon must end with two \r
Icon

# Thumbnails
._*

# Files that might appear in the root of a volume
.DocumentRevisions-V100
.fseventsd
.Spotlight-V100
.TemporaryItems
.Trashes
.VolumeIcon.icns

# Directories potentially created on remote AFP share
.AppleDB
.AppleDesktop
Network Trash Folder
Temporary Items
.apdisk


### https://raw.github.com/github/gitignore/5b8eb2c3715a7d5c4fe322abe7f0873b8985d50b/python.gitignore

# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]
*$py.class

# C extensions
*.so
imusim/maths/*.c
build
docs/build

# Distribution / packaging
.Python
env/
build/
develop-eggs/
dist/
downloads/
eggs/
.eggs/
lib/
lib64/
parts/
sdist/
var/
*.egg-info/
.installed.cfg
*.egg

# PyInstaller
# Usually these files are written by a python script from a template
# before PyInstaller builds the exe, so as to inject date/other infos into it.
*.manifest
*.spec

# Installer logs
pip-log.txt
pip-delete-this-directory.txt

# Unit test / coverage reports
htmlcov/
.tox/
.coverage
.coverage.*
.cache
nosetests.xml
coverage.xml
*,cover

# Translations
*.mo
*.pot

# Django stuff:
*.log

# Sphinx documentation
docs/_build/

# PyBuilder
target/


### https://raw.github.com/github/gitignore/5b8eb2c3715a7d5c4fe322abe7f0873b8985d50b/Global/vim.gitignore

[._]*.s[a-w][a-z]
[._]s[a-w][a-z]
*.un~
Session.vim
.netrwhist
*~


Expand Down
18 changes: 10 additions & 8 deletions imusim/io/bvh.py
Original file line number Diff line number Diff line change
Expand Up @@ -118,21 +118,22 @@ def _readMotionData(self):
try:
p = np.array([[xPos,yPos,zPos]]).T * \
self.conversionFactor
p = convertCGtoNED(p)
del xPos, yPos, zPos
p = convertCGtoNED(p)
joint.positionKeyFrames.add(time, p)
except UnboundLocalError:
raise SyntaxError("No position data for root joint")

try:
if joint.hasChildren:
q = Quaternion.fromEuler((zRot,xRot,yRot), order='zxy')
q = convertCGtoNED(q)
del xRot, yRot, zRot
# Rotation in BVH is relative to parent joint so
# combine the rotations
if joint.hasParent:
q = lastRotation[joint.parent] * q
else:
q = convertCGtoNED(q)
joint.rotationKeyFrames.add(time, q)
lastRotation[joint] = q
except UnboundLocalError:
Expand Down Expand Up @@ -169,7 +170,7 @@ def _readJoint(self):
y = self._floatToken()
z = self._floatToken()
currentJoint.positionOffset = \
convertCGtoNED(np.array([[x,y,z]]).T*self.conversionFactor)
np.array([[x,y,z]]).T*self.conversionFactor
elif token == "CHANNELS":
n = self._intToken()
channels = []
Expand Down Expand Up @@ -273,7 +274,7 @@ def saveBVHFile(model, filename, samplePeriod, conversionFactor = 1):
exporter = BVHExporter(model,bvhFile, samplePeriod, conversionFactor)
exporter.writeHeader()
for frame in xrange(exporter.frames):
exporter.writeFrame(model.startTime + frame * samplePeriod)
exporter.writeFrame(frame * samplePeriod)

class BVHExporter(object):
"""
Expand All @@ -299,7 +300,7 @@ def __init__(self, model, bvhFile, samplePeriod, conversionFactor):
self.bvhFile = bvhFile
self.model = model
self.samplePeriod = samplePeriod
self.conversionFactor = conversionFactor
self.conversionFactor = 1

@property
def frames(self):
Expand All @@ -326,7 +327,7 @@ def post(j):
self.bvhFile.write("%s{\n" %(pad*d))

self.bvhFile.write("%sOFFSET" %(pad*(d+1)))
for v in convertNEDtoCG(p.positionOffset):
for v in p.positionOffset:
self.bvhFile.write(" %.8f" % (v * self.conversionFactor))
self.bvhFile.write("\n")

Expand All @@ -351,15 +352,16 @@ def writeFrame(self, t):
if hasattr(j,'channels') and len(j.channels) > 0:

if not j.hasParent:
pos = j.position(t) * self.conversionFactor
pos = j.position(t) * M_TO_CM_CONVERSION
pos = convertNEDtoCG(pos)

rot = j.rotation(t)
if j.hasParent:
# BVH rotations are relative to parent joint
# RigidBodyModel rotations are absolute
rot = j.parent.rotation(t).conjugate * rot
rot = convertNEDtoCG(rot)
else:
rot = convertNEDtoCG(rot)
rot = rot.toEuler(order='zxy')

for chan in j.channels:
Expand Down
130 changes: 57 additions & 73 deletions imusim/maths/matrices.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,98 +22,79 @@
import numpy as np
import math
import operator
from itertools import izip

_validEulerSequences = [
'xyz', 'xzy',
'yxz', 'yzx',
'zxy', 'zyx',
'xyx', 'xzx',
'yxy', 'yzy',
'zxz', 'zyz' ]

_rotationMatrices = dict(
x = lambda rx: np.matrix((
(1,0,0),
(0,math.cos(rx),-math.sin(rx)),
(0,math.sin(rx),math.cos(rx))),dtype=float),
(0,math.sin(rx),math.cos(rx)))),
y = lambda ry: np.matrix((
(math.cos(ry),0,math.sin(ry)),
(0,1,0),
(-math.sin(ry),0,math.cos(ry))),dtype=float),
(-math.sin(ry),0,math.cos(ry)))),
z = lambda rz: np.matrix((
(math.cos(rz),-math.sin(rz),0),
(math.sin(rz),math.cos(rz),0),
(0,0,1)),dtype=float))

_EPS = 1e-12

_eulerFuncs = dict(
xyz = lambda m: \
(np.arctan2(-m[1,2], m[2,2]), np.arcsin(m[0,2]), np.arctan2(-m[0,1], m[0,0])) if abs(m[0,2]) < 1 - _EPS \
else (np.arctan2(m[1,0], m[1,1]), np.pi/2, 0) if m[0,2] > 0 \
else (-np.arctan2(m[1,0], m[1,1]), -np.pi/2, 0),
xzy = lambda m: \
(np.arctan2(m[2,1], m[1,1]), np.arcsin(-m[0,1]), np.arctan2(m[0,2], m[0,0])) if abs(m[0,1]) < 1 - _EPS \
else (np.arctan2(-m[2,0], m[2,2]), -np.pi/2, 0) if m[0,1] > 0 \
else (-np.arctan2(-m[2,0], m[2,2]), np.pi/2, 0),
yxz = lambda m: \
(np.arctan2(m[0,2], m[2,2]), np.arcsin(-m[1,2]), np.arctan2(m[1,0], m[1,1])) if abs(m[1,2]) < 1 - _EPS \
else (np.arctan2(-m[0,1], m[0,0]), -np.pi/2, 0) if m[1,2] > 0 \
else (-np.arctan2(-m[0,1], m[0,0]), np.pi/2, 0),
yzx = lambda m: \
(np.arctan2(-m[2,0], m[0,0]), np.arcsin(m[1,0]), np.arctan2(-m[1,2], m[1,1])) if abs(m[1,0]) < 1 - _EPS \
else (np.arctan2(m[2,1], m[2,2]), np.pi/2, 0) if m[1,0] > 0 \
else (-np.arctan2(m[2,1], m[2,2]), -np.pi/2, 0),
zxy = lambda m: \
(np.arctan2(-m[0,1], m[1,1]), np.arcsin(m[2,1]), np.arctan2(-m[2,0], m[2,2])) if abs(m[2,1]) < 1 - _EPS \
else (np.arctan2(m[0,2], m[0,0]), np.pi/2, 0) if m[2,1] > 0 \
else (-np.arctan2(m[0,2], m[0,0]), -np.pi/2, 0),
zyx = lambda m: \
(np.arctan2(m[1,0], m[0,0]), np.arcsin(-m[2,0]), np.arctan2(m[2,1], m[2,2])) if abs(m[2,0]) < 1 - _EPS \
else (np.arctan2(-m[1,2], m[1,1]), -np.pi/2, 0) if m[2,0] > 0 \
else (-np.arctan2(-m[1,2], m[1,1]), np.pi/2, 0),
xyx = lambda m: \
(np.arctan2(m[1,0], -m[2,0]), np.arccos(m[0,0]), np.arctan2(m[0,1], m[0,2])) if abs(m[0,0]) < 1 - _EPS \
else (np.arctan2(-m[1,2], m[1,1]), 0, 0) if m[0,0] > 0 \
else (-np.arctan2(-m[1,2], m[1,1]), np.pi, 0),
xzx = lambda m: \
(np.arctan2(m[2,0], m[1,0]), np.arccos(m[0,0]), np.arctan2(m[0,2], -m[0,1])) if abs(m[0,0]) < 1 - _EPS \
else (np.arctan2(m[2,1], m[2,2]), 0, 0) if m[0,0] > 0 \
else (-np.arctan2(m[2,1], m[2,2]), np.pi, 0),
yxy = lambda m: \
(np.arctan2(m[0,1], m[2,1]), np.arccos(m[1,1]), np.arctan2(m[1,0], -m[1,2])) if abs(m[1,1]) < 1 - _EPS \
else (np.arctan2(m[0,2], m[0,0]), 0, 0) if m[1,1] > 0 \
else (-np.arctan2(m[0,2], m[0,0]), np.pi, 0),
yzy = lambda m: \
(np.arctan2(m[2,1], -m[0,1]), np.arccos(m[1,1]), np.arctan2(m[1,2], m[1,0])) if abs(m[1,1]) < 1 - _EPS \
else (np.arctan2(-m[2,0], m[2,2]), 0, 0) if m[1,1] > 0 \
else (-np.arctan2(-m[2,0], m[2,2]), np.pi, 0),
zxz = lambda m: \
(np.arctan2(m[0,2], -m[1,2]), np.arccos(m[2,2]), np.arctan2(m[2,0], m[2,1])) if abs(m[2,2]) < 1 - _EPS \
else (np.arctan2(-m[0,1], m[0,0]), 0, 0) if m[2,2] > 0 \
else (-np.arctan2(-m[0,1], m[0,0]), np.pi, 0),
zyz = lambda m: \
(np.arctan2(m[1,2], m[0,2]), np.arccos(m[2,2]), np.arctan2(m[2,1], -m[2,0])) if abs(m[2,2]) < 1 - _EPS \
else (np.arctan2(m[1,0], m[1,1]), 0, 0) if m[2,2] > 0 \
else (-np.arctan2(m[1,0], m[1,1]), np.pi, 0),
xy = lambda m: (np.arctan2(m[2,1], m[1,1]), np.arctan2(m[0,2], m[0,0])),
xz = lambda m: (np.arctan2(-m[1,2], m[2,2]), np.arctan2(-m[0,1], m[0,0])),
yx = lambda m: (np.arctan2(-m[2,0], m[0,0]), np.arctan2(-m[1,2], m[1,1])),
yz = lambda m: (np.arctan2(m[0,2], m[2,2]), np.arctan2(m[1,0], m[1,1])),
zx = lambda m: (np.arctan2(m[1,0], m[0,0]), np.arctan2(m[2,1], m[2,2])),
zy = lambda m: (np.arctan2(-m[0,1], m[1,1]), np.arctan2(-m[2,0], m[2,2])),
x = lambda m: (np.arctan2(m[2,1], m[2,2]),),
y = lambda m: (np.arctan2(m[0,2], m[0,0]),),
z = lambda m: (np.arctan2(m[1,0], m[1,1]),))
(0,0,1))))

def matrixToEuler(m,order='zyx',inDegrees=True):
"""
Convert a 3x3 rotation matrix to an Euler angle sequence.

@param m: 3x3 L{np.matrix}, or equivalent, to convert.
@param order: The order of the Euler angle sequence, e.g. 'zyx'
@param order: The order of the Euler angle sequence, 'zyx' or 'zxy'.
@param inDegrees: True to return result in degrees, False for radians.

@return: L{np.ndarray} of Euler angles in specified order.
"""

EPS = 1e-6
order = order.lower()
if order not in _eulerFuncs.keys():
raise NotImplementedError, "Order %s not implemented" % order
result = np.array(_eulerFuncs[order](m))
assert order in _validEulerSequences, "Invalid Euler sequence '%s'" % order

if order in 'zyx':
sp = -m[2,0]
if sp < (1-EPS):
if sp > (-1+EPS):
p = np.arcsin(sp)
r = np.arctan2(m[2,1],m[2,2])
y = np.arctan2(m[1,0],m[0,0])
else:
p = -np.pi/2.
r = 0
y = np.pi-np.arctan2(-m[0,1],m[0,2])
else:
p = np.pi/2.
y = np.arctan2(-m[0,1],m[0,2])
r = 0
result = np.array((y,p,r))

elif order in 'zxy':
sx = m[2,1]
if sx < (1-EPS):
if sx > (-1+EPS):
x = np.arcsin(sx)
z = np.arctan2(-m[0,1],m[1,1])
y = np.arctan2(-m[2,0],m[2,2])
else:
x = -np.pi/2
y = 0
z = -np.arctan2(m[0,2],m[0,0])
else:
x = np.pi/2
y = 0
z = np.arctan2(m[0,2],m[0,0])
result = np.array((z,x,y))

else:
raise NotImplementedError, "Unimplemented rotation order:%r"%order

if inDegrees:
return np.degrees(result)
Expand All @@ -136,8 +117,11 @@ def matrixFromEuler(angles, order, inDegrees=True):
assert len(angles) == len(order)

if inDegrees:
angles = np.radians(angles)
angles = (np.radians(angle) for angle in angles)

return reduce(operator.mul,
(_rotationMatrices[axis](angle) for axis,angle in
izip(order.lower(), angles)))
(_rotationMatrices[axis.lower()](angle) for axis,angle in
zip(order, angles)))



Loading