def transform_direction(obj, v):
'''
Get local position of vector transformed from world space of Transform
Inputs: string, Vector3
Returns: Vector3
'''
current_matrix = get_world_matrix(obj)
current_matrix.m = 0
current_matrix.n = 0
current_matrix.o = 0
s = Vector3.Create(mc.getAttr('%s.scale' % obj)[0])
transform_matrix = EUCLID.Matrix4()
transform_matrix.m = v.x
transform_matrix.n = v.y
transform_matrix.o = v.z
scale_matrix = EUCLID.Matrix4()
scale_matrix.a = s.x
scale_matrix.f = s.y
scale_matrix.k = s.z
scale_matrix.p = 1
result_matrix = transform_matrix * current_matrix * scale_matrix
result_vector = Vector3(
result_matrix.m, result_matrix.n, result_matrix.o) - Vector3(
current_matrix.m, current_matrix.n, current_matrix.o)
return result_vector
def computeBoneTransformRecursive(self, bone, transforms, stack):
parent, parentMatrix, parentDamping, parentTransparency = stack[-1]
transform = euclid.Matrix4() * parentMatrix
pivot = bone.pivot
xMove = pivot[0]
yMove = pivot[1]
transform.translate(xMove, yMove, 0)
transform.translate(bone.translation.x, bone.translation.y, 0)
rotation = bone.rotation
if rotation.y != 0.0:
transform.rotatey(rotation.y)
if rotation.x != 0.0:
transform.rotatex(rotation.x)
if rotation.z != 0.0:
transform.rotatez(rotation.z)
transform.scale(bone.scale.x, bone.scale.y, bone.scale.z)
transform.translate(-xMove, -yMove, 0)
damping = max(bone.damping, parentDamping)
transparency = 1 - ((1 - bone.transparency) * (1 - parentTransparency))
transforms.append(BoneTransform(bone, transform, damping, transparency))
stack.append((bone, transform, damping, transparency))
for childName in bone.children:
self.computeBoneTransformRecursive(self.bones[childName], transforms, stack)
stack.pop()
def getProjection(self):
flipY = -1
projection = utils.createPerspectiveOrtho(-1.0, 1.0, 1.0 * flipY, -1.0 * flipY, -1.0, 1.0)
result = euclid.Matrix4()
for i, attr in enumerate("abcdefghijklmnop"):
setattr(result, attr, projection[i])
return result
def intersects(self, selectRay):
# This is the slow way
m = euclid.Matrix4()
m.translate(*tuple(self.pos))
m *= self.orientation.get_matrix()
size = self.size
m.scale(size, size, 1)
vertlist = [m * v for v in self.vertlist]
for i in range(1, len(vertlist) - 1):
poly = euclid.Triangle(vertlist[0], vertlist[i], vertlist[i + 1])
result = poly.intersect(selectRay)
if result: return result
else: return False
def computeBoneTransforms(self):
transforms = []
stack = []
stack.append((None, euclid.Matrix4(), 0.0, 0.0))
self.computeBoneTransformRecursive(self.root, transforms, stack)
stack.pop()
transforms.sort(key=lambda t: t.bone.zOrder)
if len(stack) != 0:
raise RuntimeError("Unbalanced stack size: %i" % len(stack))
if len(transforms) > skin.MAX_BONES:
raise RuntimeError("Too many bones, maximum is %i" % skin.MAX_BONES)
return transforms
def get_card_from_hit(self, select_ray):
hits = []
m = euclid.Matrix4()
m.translate(*tuple(-self.pos)) # 0, 0, -self.z)
select_ray = m * select_ray
for card in self.cards:
result = card.intersects(select_ray)
if result:
hits.append((card, result))
if not hits: return
hits.sort(cmp=lambda x, y: cmp(abs(x[1] - select_ray.p),
abs(y[1] - select_ray.p)))
selected = hits[0][0]
spot = hits[0][1]
return selected
def loadModel(self, tag, path, textures, matrix=None):
if not matrix:
matrix = euclid.Matrix4()
m = mesh.MeshObj(path)
m.load()
entry = ModelEntry(m, matrix)
for sampler, image in textures.items():
entry.textureMap.setTexture(sampler, image)
old = self.models.get(tag)
if old:
old.free()
self.models[tag] = entry
return entry
def get_world_matrix(obj):
matrix_a = mc.xform(obj, q=True, m=True, ws=True)
current_matrix = EUCLID.Matrix4()
current_matrix.a = matrix_a[0]
current_matrix.b = matrix_a[1]
current_matrix.c = matrix_a[2]
current_matrix.d = matrix_a[3]
current_matrix.e = matrix_a[4]
current_matrix.f = matrix_a[5]
current_matrix.g = matrix_a[6]
current_matrix.h = matrix_a[7]
current_matrix.i = matrix_a[8]
current_matrix.j = matrix_a[9]
current_matrix.k = matrix_a[10]
current_matrix.l = matrix_a[11]
current_matrix.m = matrix_a[12]
current_matrix.n = matrix_a[13]
current_matrix.o = matrix_a[14]
current_matrix.p = matrix_a[15]
return current_matrix
def makeGripperMarker(angle=0.541, color=None, scale=1.0):
"""
Creates an InteractiveMarkerControl with the PR2 gripper shape.
Parameters:
angle: the aperture angle of the gripper (default=0.541)
color: (r,g,b,a) tuple or None (default) if using the material colors
scale: the scale of the gripper, default is 1.0
Returns:
The new gripper InteractiveMarkerControl
"""
T1 = euclid.Matrix4()
T2 = euclid.Matrix4()
T1.translate(0.07691, 0.01, 0.)
T1.rotate_axis(angle, euclid.Vector3(0,0,1))
T2.translate(0.09137, 0.00495, 0.)
T1.rotate_axis(-angle, euclid.Vector3(0,0,1))
T_proximal = T1.copy()
T_distal = T1 * T2
control = InteractiveMarkerControl()
mesh = Marker()
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = scale
mesh.scale.y = scale
mesh.scale.z = scale
if color is not None:
mesh.color.r = color[0]
mesh.color.g = color[1]
mesh.color.b = color[2]
mesh.color.a = color[3]
mesh.mesh_use_embedded_materials = False
else:
mesh.mesh_use_embedded_materials = True
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
mesh.pose.orientation.w = 1
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.pose = matrix4ToPose(T_proximal)
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.pose = matrix4ToPose(T_distal)
control.markers.append(copy.deepcopy(mesh))
T1 = euclid.Matrix4()
T2 = euclid.Matrix4()
T1.translate(0.07691, -0.01, 0.)
T1.rotate_axis(numpy.pi, euclid.Vector3(1,0,0))
T1.rotate_axis(angle, euclid.Vector3(0,0,1))
T2.translate(0.09137, 0.00495, 0.)
T1.rotate_axis(-angle, euclid.Vector3(0,0,1))
T_proximal = T1.copy()
T_distal = T1 * T2
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.pose = matrix4ToPose(T_proximal)
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.pose = matrix4ToPose(T_distal)
control.markers.append(copy.deepcopy(mesh))
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
