def getLocalTransform(animation,
jointlock,
frame,
pointpos,
handthick=3.5):
"""
Return the local transform of the surface point in respect to its parent joint jointlock
:type animation: pyanimation.Animation
:param animation: Calibration animation
:type jointlock: pyanimation.Joint
:param jointlock: Joint to be used as parent of the surface point
:type frame: int
:param frame: The frame number of the pose to evaluate
:type pointpos: numpy.ndarray
:param pointpos: Position of the surface point
"""
jointTransform = jointlock.getGlobalTransform(frame)
if handthick:
jointPosition = np.dot(jointTransform, [0, 0, 0, 1])[:-1]
#Find parametric equation to remove hands surface (hand thickness)
vec = jointPosition - pointpos
distance = np.linalg.norm(vec)
t = handthick / distance
position = pointpos + t * vec
else:
position = pointpos
jointInverse = mathutils.inverseMatrix(jointTransform)
globalTransform = mathutils.matrixTranslation(position[0], position[1],
position[2])
localTransform = np.dot(jointInverse, globalTransform)
return localTransform
def getLocalTransformBaseRotation(self, frame):
#OBSOLETE
print('Do not use this function!')
localRotMatrix = mathutils.expandShape3ToShape4(
self.getRecalcRotationMatrix(frame))
translation = mathutils.matrixTranslation(0, self.getLength(), 0)
localTransform = mathutils.matrixMultiply(translation, localRotMatrix)
#return localTransform
return None
def getEndSitePosition(self, frame=0):
if len(self.endsite) == 0:
print(
'Unable to get joint\'s EndSite at readbvh.getEndSitePosition(joint, frame) because the joint %s does not have an EndSite.'
% self.name)
return None
transform = mathutils.matrixTranslation(self.endsite[0],
self.endsite[1],
self.endsite[2])
transform = np.dot(self.getGlobalTransform(frame), transform)
# return np.dot(transform,[0,0,0,1])[:-1]
return np.asarray(transform[:-1, -1])
def getLocalTransform(self, frame=0):
"""
Get joint local transform
"""
trans = self.getLocalTranslation(frame)
rot = self.getLocalRotation(frame)
rotx = mathutils.matrixRotation(rot[0], 1, 0, 0)
roty = mathutils.matrixRotation(rot[1], 0, 1, 0)
rotz = mathutils.matrixRotation(rot[2], 0, 0, 1)
transform = mathutils.matrixTranslation(trans[0], trans[1], trans[2])
if self.order == "ZXY":
transform = np.dot(transform, rotz)
transform = np.dot(transform, rotx)
transform = np.dot(transform, roty)
elif self.order == "XYZ":
transform = np.dot(transform, rotx)
transform = np.dot(transform, roty)
transform = np.dot(transform, rotz)
return transform
def GetAvatarSurfaceLocalTransform(avatar, avatarSurface):
"""
Pega a local baseado na animação com um frame da TPose
"""
for point in avatarSurface.points:
if point.pointtype == 'mesh':
joint = skeletonmap.getmatchingjoint(point.jointlock, avatar)
if not joint:
print(
'Something went wrong in retarget.GetAvatarSurfaceLocalTransform()'
)
print('Matching joint could not be found')
else:
globalTransform = mathutils.matrixTranslation(
point.baseposition[0], point.baseposition[1],
point.baseposition[2])
parentInverse = mathutils.inverseMatrix(
joint.getGlobalTransform(frame=0))
point.calibrationLocalTransform = np.dot(
parentInverse, globalTransform)
joint = None
def GetPositions(joint, frame=0, parentTransform=[], positions=[]):
# Recebe o frame e a junta root, recursivamente calcula a posição de todos
# os filhos para esse frame
# Retorna vetor com posição dos ossos (linhas)
# Salva a posição em cada frame dentro de cada instância junta
#if not parentTransform and not joint.parent:
if len(parentTransform) == 0:
positions = []
posroot = joint.translation[frame]
rotroot = joint.rotation[frame]
rotx = mathutils.matrixRotation(rotroot[0], 1, 0, 0)
roty = mathutils.matrixRotation(rotroot[1], 0, 1, 0)
rotz = mathutils.matrixRotation(rotroot[2], 0, 0, 1)
parentTransform = mathutils.matrixTranslation(posroot[0], posroot[1],
posroot[2])
if joint.order == "ZXY":
parentTransform = mathutils.matrixMultiply(parentTransform, rotz)
parentTransform = mathutils.matrixMultiply(parentTransform, rotx)
parentTransform = mathutils.matrixMultiply(parentTransform, roty)
elif joint.order == "XYZ":
parentTransform = mathutils.matrixMultiply(parentTransform, rotx)
parentTransform = mathutils.matrixMultiply(parentTransform, roty)
parentTransform = mathutils.matrixMultiply(parentTransform, rotz)
position = mathutils.matrixMultiply(parentTransform, [0, 0, 0, 1])
#Salva a posição da junta no frame atual
joint.addPosition(np.asarray(position[:-1]), frame)
else:
pos = joint.translation[frame]
rot = joint.rotation[frame]
rotx = mathutils.matrixRotation(rot[0], 1, 0, 0)
roty = mathutils.matrixRotation(rot[1], 0, 1, 0)
rotz = mathutils.matrixRotation(rot[2], 0, 0, 1)
transform = mathutils.matrixTranslation(pos[0], pos[1], pos[2])
if joint.order == "ZXY":
transform = mathutils.matrixMultiply(transform, rotz)
transform = mathutils.matrixMultiply(transform, rotx)
transform = mathutils.matrixMultiply(transform, roty)
elif joint.order == "XYZ":
transform = mathutils.matrixMultiply(transform, rotx)
transform = mathutils.matrixMultiply(transform, roty)
transform = mathutils.matrixMultiply(transform, rotz)
transform = mathutils.matrixMultiply(parentTransform, transform)
positionfinal = mathutils.matrixMultiply(transform, [0, 0, 0, 1])
positioninit = mathutils.matrixMultiply(parentTransform, [0, 0, 0, 1])
positions.append(np.concatenate((positioninit, positionfinal)))
parentTransform = np.copy(transform)
#Salva a posição da junta no frame atual
joint.addPosition(np.asarray(positionfinal[:-1]), frame)
#Caso a junta tenha um endsite (é um end effector)
if len(joint.endsite) > 0:
transform = mathutils.matrixTranslation(joint.endsite[0],
joint.endsite[1],
joint.endsite[2])
transform = mathutils.matrixMultiply(parentTransform, transform)
endsitepos = mathutils.matrixMultiply(transform, [0, 0, 0, 1])
positions.append(np.concatenate((positionfinal, endsitepos)))
#Salva a posição do endsite da junta no frame atual
joint.addPosition(np.asarray(endsitepos[:-1]), frame)
for child in joint.children:
GetPositions(child, frame, parentTransform, positions)
parentTransform = []
return positions
def GetPositions(joint,
frame=0,
parentTransform=[],
surfaceinfo=None,
calibrating=None):
# Recebe o frame e a junta root, recursivamente calcula a posição de todos
# os filhos para esse frame
# Salva a posição em cada frame dentro de cada instância junta
# Caso precise recalcular as posições das juntas, os dados antigos precisam
# ser apagados
#TODO: Orientation não significa nada, arrumar
rot = joint.rotation[frame]
transform = joint.getLocalTransform(frame)
if len(parentTransform) == 0:
#Se for root apenas calcula a posição
positionfinal = np.dot(transform, [0, 0, 0, 1])
orientation = np.asarray([rot[0], rot[1], rot[2]])
else:
#Nos outros casos, multiplica pela transformada da junta pai
transform = np.dot(parentTransform, transform)
positionfinal = np.dot(transform, [0, 0, 0, 1])
orientation = joint.parent.orientation[frame, :] + np.asarray(
[rot[0], rot[1], rot[2]])
if not calibrating:
joint.addPosition(np.asarray(positionfinal[:-1]), frame)
joint.addOrientation(orientation, frame)
#Caso a junta tenha um endsite (é um end effector)
if len(joint.endsite) > 0:
ee_transform = mathutils.matrixTranslation(joint.endsite[0],
joint.endsite[1],
joint.endsite[2])
ee_transform = np.dot(transform, ee_transform)
endsitepos = np.dot(ee_transform, [0, 0, 0, 1])
if not calibrating:
#Salva a posição do endsite da junta no frame atual
joint.addEndSitePosition(np.asarray(endsitepos[:-1]), frame)
#if there is surface information and a surface point or limb is attached
#to this joint, calculate the surface point or limb position
if surfaceinfo:
for point in surfaceinfo.points:
if point.jointlock == joint.name:
if len(point.calibrationLocalTransform) > 0:
local_transform = np.dot(transform,
point.calibrationLocalTransform)
point.position.append(np.dot(local_transform,
[0, 0, 0, 1]))
# Get calibration local transform of the actor surface points for the mocap animation
if calibrating:
point = calibrating
if skeletonmap.isamatch(point.jointlock, joint.name):
if point.pointtype == 'mesh':
globalTransform = mathutils.matrixTranslation(
point.calibrationPosition[0], point.calibrationPosition[1],
point.calibrationPosition[2])
parentInverse = mathutils.inverseMatrix(transform)
point.calibrationLocalTransform = np.dot(
parentInverse, globalTransform)
if point.pointtype == 'limb':
# TODO: testar esse caso!!!
p1 = np.dot(transform, [0, 0, 0, 1])[:-1]
# aux_transform = np.dot(parentTransform,joint.getChildren(0).getLocalTransform(frame))
aux_transform = np.dot(
transform,
joint.getChildren(0).getLocalTransform(frame))
p2 = np.dot(aux_transform, [0, 0, 0, 1])[:-1]
#calibration point = p0
p0 = np.asarray(point.calibrationPosition)
#distance between point p0 and line passing through p1 and p2:
# d = |(p0 - p1) x (p0 - p2)|/|p2-p1| = |(p0p1) x (p0p2)|/|p2p1|
p0p1 = p0 - p1
p0p2 = p0 - p2
p2p1 = p2 - p1
d = np.linalg.norm(np.cross(p0p1, p0p2)) / np.linalg.norm(p2p1)
point.radius = d
parentTransform = np.copy(transform)
for child in joint.children:
GetPositions(child, frame, parentTransform, surfaceinfo, calibrating)
parentTransform = []
def GetPositions(joint, frame=0, parentTransform=[], surfaceinfo=None, calibrating=None):
# Recebe o frame e a junta root, recursivamente calcula a posição de todos
# os filhos para esse frame
# Salva a posição em cada frame dentro de cada instância junta
pos = joint.translation[frame]
rot = joint.rotation[frame]
rotx = mathutils.matrixRotation(rot[0],1,0,0)
roty = mathutils.matrixRotation(rot[1],0,1,0)
rotz = mathutils.matrixRotation(rot[2],0,0,1)
transform = mathutils.matrixTranslation(pos[0], pos[1], pos[2])
if joint.order == "ZXY":
transform = mathutils.matrixMultiply(transform, rotz)
transform = mathutils.matrixMultiply(transform, rotx)
transform = mathutils.matrixMultiply(transform, roty)
elif joint.order == "XYZ":
transform = mathutils.matrixMultiply(transform, rotx)
transform = mathutils.matrixMultiply(transform, roty)
transform = mathutils.matrixMultiply(transform, rotz)
if len(parentTransform) == 0:
positionfinal = mathutils.matrixMultiply(transform, [0,0,0,1])
else:
transform = mathutils.matrixMultiply(parentTransform,transform)
positionfinal = mathutils.matrixMultiply(transform,[0,0,0,1])
parentTransform = np.copy(transform)
joint.addPosition(np.asarray(positionfinal[:-1]), frame)
#if there is surface information and a surface point or limb is attached
#to this joint, calculate the surface point or limb position
if surfaceinfo:
for point in surfaceinfo:
if point.jointlock == joint.name:
if len(point.position) == 0:
globalTransform = mathutils.matrixTranslation(point.calibrationPosition[0], point.calibrationPosition[1], point.calibrationPosition[2])
parentInverse = mathutils.invertMatrix(parentTransform)
point.calibrationLocalTransform = mathutils.matrixMultiply(parentInverse,globalTransform)
transform = mathutils.matrixMultiply(parentTransform,point.calibrationLocalTransform)
point.position.append(mathutils.matrixMultiply(transform,[0,0,0,1]))
#Caso a junta tenha um endsite (é um end effector)
if len(joint.endsite)>0:
transform = mathutils.matrixTranslation(joint.endsite[0], joint.endsite[1], joint.endsite[2])
transform = mathutils.matrixMultiply(parentTransform,transform)
endsitepos = mathutils.matrixMultiply(transform,[0,0,0,1])
#Salva a posição do endsite da junta no frame atual
joint.addEndSitePosition(np.asarray(endsitepos[:-1]), frame)
# #if not parentTransform and not joint.parent:
# if len(parentTransform) == 0:
#
# #creates root (or first joint) transform
# posroot = joint.translation[frame]
# rotroot = joint.rotation[frame]
# rotx = mathutils.matrixRotation(rotroot[0],1,0,0)
# roty = mathutils.matrixRotation(rotroot[1],0,1,0)
# rotz = mathutils.matrixRotation(rotroot[2],0,0,1)
# parentTransform = mathutils.matrixTranslation(posroot[0], posroot[1], posroot[2])
# if joint.order == "ZXY":
# parentTransform = mathutils.matrixMultiply(parentTransform, rotz)
# parentTransform = mathutils.matrixMultiply(parentTransform, rotx)
# parentTransform = mathutils.matrixMultiply(parentTransform, roty)
# elif joint.order == "XYZ":
# parentTransform = mathutils.matrixMultiply(parentTransform, rotx)
# parentTransform = mathutils.matrixMultiply(parentTransform, roty)
# parentTransform = mathutils.matrixMultiply(parentTransform, rotz)
# position = mathutils.matrixMultiply(parentTransform, [0,0,0,1])
# #Salva a posição da junta no frame atual
# joint.addPosition(np.asarray(position[:-1]), frame)
# #if there is surface information and a surface point or limb is attached
# #to this joint, calculate the surface point or limb position
# if surfaceinfo:
# for point in surfaceinfo:
# if point.jointlock == joint.name:
# if len(point.position) == 0:
# globalTransform = mathutils.matrixTranslation(point.calibrationPosition[0], point.calibrationPosition[1], point.calibrationPosition[2])
# parentInverse = mathutils.invertMatrix(parentTransform)
# point.calibrationLocalTransform = mathutils.matrixMultiply(parentInverse,globalTransform)
# transform = mathutils.matrixMultiply(parentTransform,point.calibrationLocalTransform)
# point.position.append(mathutils.matrixMultiply(transform,[0,0,0,1]))
#
# else:
# pos = joint.translation[frame]
# rot = joint.rotation[frame]
# rotx = mathutils.matrixRotation(rot[0],1,0,0)
# roty = mathutils.matrixRotation(rot[1],0,1,0)
# rotz = mathutils.matrixRotation(rot[2],0,0,1)
# transform = mathutils.matrixTranslation(pos[0], pos[1], pos[2])
# if joint.order == "ZXY":
# transform = mathutils.matrixMultiply(transform, rotz)
# transform = mathutils.matrixMultiply(transform, rotx)
# transform = mathutils.matrixMultiply(transform, roty)
# elif joint.order == "XYZ":
# transform = mathutils.matrixMultiply(transform, rotx)
# transform = mathutils.matrixMultiply(transform, roty)
# transform = mathutils.matrixMultiply(transform, rotz)
#
# transform = mathutils.matrixMultiply(parentTransform,transform)
# positionfinal = mathutils.matrixMultiply(transform,[0,0,0,1])
# parentTransform = np.copy(transform)
#
# #Salva a posição da junta no frame atual
# joint.addPosition(np.asarray(positionfinal[:-1]), frame)
#
# #if there is surface information and a surface point or limb is attached
# #to this joint, calculate the surface point or limb position
# if surfaceinfo:
# for point in surfaceinfo:
# if point.jointlock == joint.name:
# if len(point.position) == 0:
# globalTransform = mathutils.matrixTranslation(point.calibrationPosition[0], point.calibrationPosition[1], point.calibrationPosition[2])
# parentInverse = mathutils.invertMatrix(parentTransform)
# point.calibrationLocalTransform = mathutils.matrixMultiply(parentInverse,globalTransform)
# transform = mathutils.matrixMultiply(parentTransform,point.calibrationLocalTransform)
# point.position.append(mathutils.matrixMultiply(transform,[0,0,0,1]))
#
#
#
# #Caso a junta tenha um endsite (é um end effector)
# if len(joint.endsite)>0:
# transform = mathutils.matrixTranslation(joint.endsite[0], joint.endsite[1], joint.endsite[2])
# transform = mathutils.matrixMultiply(parentTransform,transform)
# endsitepos = mathutils.matrixMultiply(transform,[0,0,0,1])
#
# #Salva a posição do endsite da junta no frame atual
# joint.addEndSitePosition(np.asarray(endsitepos[:-1]), frame)
for child in joint.children:
GetPositions(child, frame, parentTransform, surfaceinfo)
parentTransform=[]
return None
