def build_transform_matrix(self, translation, rotation):
theta_x = rotation['x']
rotation_matrix_x = mat4(
1, 0, 0, 0,
0, cos(theta_x), -sin(theta_x), 0,
0, sin(theta_x), cos(theta_x), 0,
0, 0, 0, 1
)
theta_y = rotation['y']
rotation_matrix_y = mat4(
cos(theta_y), 0, -sin(theta_y), 0,
0, 1, 0, 0,
sin(theta_y), 0, cos(theta_y), 0,
0, 0, 0, 1
)
theta_z = rotation['z']
rotation_matrix_z = mat4(
cos(theta_z), -sin(theta_z), 0, 0,
sin(theta_z), cos(theta_z), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
)
rotation_matrix = rotation_matrix_x * rotation_matrix_y * rotation_matrix_z
translation_matrix = mat4().identity()
translation_matrix[0, 3] = translation['x']
translation_matrix[1, 3] = translation['y']
translation_matrix[2, 3] = translation['z']
self.transform = self.transform_parent * translation_matrix * rotation_matrix
def getPivotedRotation(pivot, rot):
m = mat4(1.0)
# translate to origin
m.translate(pivot)
# rotate at origin
rot4 = mat4(rot[0][0], rot[0][1], rot[0][2], 0,
rot[1][0], rot[1][1], rot[1][2], 0,
rot[2][0], rot[2][1], rot[2][2], 0,
0, 0, 0, 1)
m *= rot4
# translate back
m.translate(-pivot)
return m
def render_frame(self):
self.frame += 1
# Fetch the head pose
poses = self.hmd.get_eye_poses(self.frame, self.eyeOffsets)
self.hmd.begin_frame(self.frame)
for i in range(0, 2):
eye = self.hmdDesc.EyeRenderOrder[i]
glMatrixMode(GL_PROJECTION)
glLoadMatrixf(self.projections[eye])
self.eyeview = mat4(1.0)
# Apply the head orientation
rot = poses[eye].Orientation
# Convert the OVR orientation (a quaternion
# structure) to a cgkit quaternion class, and
# from there to a mat4 Coordinates are camera
# coordinates
rot = quat(rot.toList())
rot = rot.toMat4()
# Apply the head position
pos = poses[eye].Position
# Convert the OVR position (a vector3 structure)
# to a cgcit vector3 class. Position is in camera /
# Rift coordinates
pos = vec3(pos.toList())
pos = mat4(1.0).translate(pos)
pose = pos * rot
# apply it to the eyeview matrix
self.eyeview = pose;
# The subclass is responsible for taking eyeview
# and applying it to whatever camera or modelview
# coordinate system it uses before rendering the
# scene
# Active the offscreen framebuffer and render the scene
glBindFramebuffer(GL_FRAMEBUFFER, self.fbo[eye])
size = self.eyeTextures[eye].Texture.Header.RenderViewport.Size
glViewport(0, 0, size.w, size.h)
self.render_scene()
glBindFramebuffer(GL_FRAMEBUFFER, 0)
self.hmd.end_frame(poses, self.eyeTextures)
glGetError()
def __init__(self, other=''):
self.children = []
if isinstance(other, basestring):
self.name = other
self.transform_parent = mat4()
self.transform = mat4()
elif isinstance(other, Joint):
self.name = other.name
self.transform_parent = mat4(other.transform_parent)
self.transform = mat4(other.transform)
for child in other.children:
self.children.append(Joint(child))
def update_mvp(self, modl_ = None): if modl_: modl = modl_ else: modl = cgt.mat4(1) proj = self.update_projection() view = self.update_view() mvp = proj*view*modl self._mvp_matrix_[:] = np.array(mvp).flatten() return mvp
def setUp(self):
a = 0, 1.6, 2, 3, 4.977, 5, 6, .007, 8, 0, 1.6, 2, 3, 4.977, 5, 6
b = 0.000921, 10, 20.5, -34.5
self.cym = cycg.mat4(*a)
self.cgm = cg.mat4(*a)
self.cyvec4 = cycg.vec4(*b)
self.cgvec4 = cg.vec4(*b)
self.cyvec3 = cycg.vec3(2, -4, .8)
self.cgvec3 = cg.vec3(2, -4, .8)
def test_root_transform(self):
transform = mat4(
0, 1, 0, 0,
0, 0, -1, 0,
-1, 0, 0, 0,
0, 0, 0, 1
)
TestUtility().assertAlmostEqual_mat4(transform, self._skeleton._root.transform, 12)
def test_add_children_transform_nested(self):
expected_transform = mat4(
0, 0, 1, 0,
1, 0, 0, 0,
0, 1, 0, -3,
0, 0, 0, 1
)
actual_transform = self._skeleton._root.children[1].children[0].transform
TestUtility().assertAlmostEqual_mat4(actual_transform, expected_transform)
def view(self, regime): r_hat, u_hat, f_hat = self.get_basis(regime) eye = self.location x, y, z = eye view = cgt.mat4([ r_hat[0], r_hat[1], r_hat[2], -r_hat*eye, u_hat[0], u_hat[1], u_hat[2], -u_hat*eye, f_hat[0], f_hat[1], f_hat[2], -f_hat*eye, 0, 0, 0, 1]) return view
def test_copy_is_equal(self):
parent = Joint('parent name')
parent.transform = mat4(
1, 0, 0, 1,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1
)
child = Joint('child name')
child.transform_parent = mat4(parent.transform)
child.transform = mat4(
1, 0, 0, 2,
0, 1, 0, 0,
0, 0, 1, -2,
0, 0, 0, 1
)
parent.children.append(child)
parent_copy = Joint(parent)
self.assertEqual(parent_copy, parent)
def projection_persp(self): near, far = self.znear, self.zfar #fFrustumScale = 1.0*self.zoom_ aspectRatio = 0.75 DEG2RAD = math.pi / 180.0 fov = 60*DEG2RAD h = 10*math.cos(0.5*fov)/(math.sin(0.5*fov)*self.zoom_) w = h * aspectRatio a = (far + near)/(near - far) b = (2*near*far)/(near - far) proj = cgt.mat4([ w, 0, 0, 0, 0, h, 0, 0, 0, 0, a, b, 0, 0,-1, 0]) return proj
def ovrPoseToMat4(pose): # Apply the head orientation rot = pose.Orientation # Convert the OVR orientation (a quaternion structure) to a cgkit quaternion # class, and from there to a mat4 Coordinates are camera coordinates rot = quat(rot[-1:]+rot[:-1]) # reorder xyzw -> wxyz rot = rot.toMat4() # Apply the head position pos = pose.Position # Convert the OVR position (a vector3 structure) to a cgcit vector3 class. # Position is in camera / Rift coordinates pos = vec3(pos) pos = mat4(1.0).translate(pos) pose = pos * rot return pose
def ovrPoseToMat4(pose): # Apply the head orientation rot = pose.Orientation # Convert the OVR orientation (a quaternion structure) to a cgkit quaternion # class, and from there to a mat4 Coordinates are camera coordinates rot = quat(rot[-1:]+rot[:-1]) # reorder xyzw -> wxyz rot = rot.toMat4() # Apply the head position pos = pose.Position # Convert the OVR position (a vector3 structure) to a cgcit vector3 class. # Position is in camera / Rift coordinates pos = vec3(pos) pos = mat4(1.0).translate(pos) pose = pos * rot return pose
def projection_ortho(self): near, far = self.znear, self.zfar corner = 0.1*self.zoom_ left = -corner right = corner bottom = -corner top = corner s1 = 2.0 / (right - left) s2 = 2.0 / (top - bottom) s3 = -2.0 / (far - near) t1 = -(right+left)/(right-left) t2 = -(top+bottom)/(top-bottom) t3 = -(far+near)/(far-near) proj = cgt.mat4([ s1, 0, 0,t1, 0,s2, 0,t2, 0, 0,s3,t3, 0, 0, 0, 1]) return proj
def mouse_drag(x, y):
'''
Process mouse events
'''
global prev_x # Location where mouse was pressed
global prev_y
global curr_x
global curr_y
global rotation_matrix # Current rotation matrix
if arcball_on == True:
curr_x = x
curr_y = y
# Arcball implementation:
if (curr_x != prev_x or curr_y != prev_y) and arcball_on == True:
# Calculating two vectors to both mouse positions on the screen
vec_to_first_click = get_arcball_vector(prev_x, prev_y)
vec_to_second_click = get_arcball_vector(curr_x, curr_y)
# Angle of the turn is calculated by taking a dot product between those two vectors
angle = math.acos(min(1.0, vec_to_first_click*vec_to_second_click))
# Axis of a turn is calculated by taking a cross product
axis_in_camera_coord = vec_to_first_click.cross(vec_to_second_click)
# Magic happens here, to be able to make a turn very intuitive shift y with z axis
x = axis_in_camera_coord.y
axis_in_camera_coord.y = axis_in_camera_coord.x
axis_in_camera_coord.x = x
z = axis_in_camera_coord.x
axis_in_camera_coord.x = axis_in_camera_coord.z
axis_in_camera_coord.z = z
# Multiply current rotation with a new angle from the left
rotation_matrix = mat4(1.0).rotate(math.degrees(angle)/30.0, axis_in_camera_coord)*rotation_matrix
# Save new coordinates as old
prev_x = curr_x
prev_y = curr_y
def processMainMenu(option):
global arcball_on
global rotation_matrix
global localizer_thread_alive
global epoc
global pause_mode
arcball_on = False
if option == 1:
change_transparency_mode()
elif option == 2:
pause_mode = (pause_mode + 1) % 2
if pause_mode:
print 'Pause mode enabled'
else:
print 'Pause mode disabled'
elif option == 3:
rotation_matrix = mat4(1.0)
glLoadIdentity()
elif option == 4:
quit()
def keyboard(key, x, y):
'''
Process keyboard events
'''
global rotation_matrix
global localizer_thread_alive
global epoc
global scene_id
global pause_mode
if key == GLUT_KEY_LEFT:
# Compute an 'object vector' which is a corresponding axis in object's coordinates
object_axis_vector = rotation_matrix.inverse()*vec3([0, 0, 1])
rotation_matrix = rotation_matrix.rotate(-3.14/90, object_axis_vector)
if key == GLUT_KEY_RIGHT:
object_axis_vector = rotation_matrix.inverse()*vec3([0, 0, 1])
rotation_matrix = rotation_matrix.rotate(3.14/90, object_axis_vector)
if key == GLUT_KEY_UP:
object_axis_vector = rotation_matrix.inverse()*vec3([0, 1, 0])
rotation_matrix = rotation_matrix.rotate(3.14/90, object_axis_vector)
if key == GLUT_KEY_DOWN:
object_axis_vector = rotation_matrix.inverse()*vec3([0, 1, 0])
rotation_matrix = rotation_matrix.rotate(-3.14/90, object_axis_vector)
elif key == chr(27):
quit()
elif key == 't' or key == 'T':
change_transparency_mode()
elif key == 'i' or key == 'I':
rotation_matrix = mat4(1.0)
glLoadIdentity()
elif key == 'h' or key == 'H':
scene_id = 0
elif key == 'b' or key == 'B':
scene_id = 1
elif key == 'p' or key == 'P':
pause_mode = (pause_mode + 1) % 2
if pause_mode:
print 'Pause mode enabled'
else:
print 'Pause mode disabled'
def add_children(self, parent, children):
for child in children:
new_child = Joint(child.name)
new_child.transform_parent = mat4(parent.transform)
translation = {
'x': child.offset[0],
'y': child.offset[1],
'z': child.offset[2],
}
if child.name == 'End Site':
rotation = {'x': 0, 'y': 0, 'z': 0}
else:
rotation = {
'x': child.vtx.values[self._frame_number].v * pi / 180,
'y': child.vty.values[self._frame_number].v * pi / 180,
'z': child.vtz.values[self._frame_number].v * pi / 180,
}
new_child.build_transform_matrix(translation, rotation)
parent.children.append(new_child)
self.add_children(new_child, child.children)
def __init__(self, other=None, frame_number=None):
if isinstance(other, BVHReader) and isinstance(frame_number, int):
self._frame_number = frame_number
self._root = Joint(other.root.name)
self._root.transform_parent = mat4().identity()
translation = {
'x': other.root.offset[0] + other.root.vtpos.values[frame_number].v.x,
'y': other.root.offset[1] + other.root.vtpos.values[frame_number].v.y,
'z': other.root.offset[2] + other.root.vtpos.values[frame_number].v.z,
}
rotation = {
'x': other.root.vtx.values[frame_number].v * pi / 180,
'y': other.root.vty.values[frame_number].v * pi / 180,
'z': other.root.vtz.values[frame_number].v * pi / 180,
}
self._root.build_transform_matrix(translation, rotation)
self.add_children(self._root, other.root.children)
elif isinstance(other, Skeleton):
self._frame_number = other._frame_number
self._root = Joint(other._root)
def renderGeoms(self):
log.debug('rendering %d geoms', self.sim.space.getNumGeoms())
if self.wireframe:
gluQuadricDrawStyle(self.quadratic, GLU_SILHOUETTE)
else:
gluQuadricDrawStyle(self.quadratic, GLU_FILL)
for geom in self.sim.space:
glPushMatrix()
if type(geom) is ode.GeomSphere:
log.debug('draw sphere')
glColor(0, 0, 1)
x, y, z = geom.getPosition()
glTranslate(x, y, z)
glutSolidSphere(geom.getRadius(), 20, 20)
elif type(geom) is ode.GeomPlane:
pass
elif type(geom) is ode.GeomBox:
log.debug('draw box(%s) @(%s)', str(geom.getLengths()),
str(geom.getPosition()))
glColor(0.8, 0, 0)
x, y, z = geom.getPosition()
# create openGL 4x4 transform matrix from ODE 3x3 rotation matrix
R = geom.getRotation()
log.debug('ROTATE = %s', str(R)) # R is a 3x3 matrix
T = mat4()
T.setMat3(mat3(R))
T.setColumn(3, vec4(x, y, z, 1.0))
glMultMatrixd(T.toList())
(sx, sy, sz) = geom.getLengths()
log.debug('size (%f,%f,%f)', sx, sy, sz)
glScale(sx, sy, sz)
if self.wireframe:
glutWireCube(1)
else:
glutSolidCube(1)
elif type(geom) is ode.GeomCCylinder:
log.debug('draw ccylinder')
def red():
glColor(1, 0, 0)
def green():
glColor(0, 1, 0)
def blue():
glColor(0, 0, 1)
def plot_axes(ax, ay, az):
for axis, colour in (ax, red), (ay, blue), (az, green):
if axis:
x, y, z = axis
colour()
glBegin(GL_LINES)
glVertex(0, 0, 0)
glVertex(x * 5, y * 5, z * 5)
glEnd()
if self.render_axes and hasattr(geom, 'motor'):
glPushMatrix()
glDisable(GL_LIGHTING)
m = geom.motor
x, y, z = m.joint.getAnchor()
glTranslate(x, y, z)
ax = None
ay = None
az = None
if isinstance(m.joint, ode.HingeJoint):
ax = m.joint.getAxis()
elif isinstance(m.joint, ode.UniversalJoint):
ax = m.joint.getAxis1()
ay = m.joint.getAxis2()
plot_axes(ax, ay, az)
# plot motor axes
# these should be aligned with the joint axes above
ax, ay, az = m.getAxis(0), m.getAxis(1), m.getAxis(2)
plot_axes(ax, ay, az)
glEnable(GL_LIGHTING)
glPopMatrix()
# construct transformation matrix from position and rotation
x, y, z = geom.getPosition()
rotmat = mat3(geom.getRotation())
log.debug('r=%s', geom.getRotation())
# ode ccylinders are aligned along z axis by default
T = mat4()
T.setMat3(rotmat)
T.setColumn(3, vec4(x, y, z, 1.0))
log.debug('geom matrix T is %s', str(T))
glMultMatrixd(T.toList())
(radius, length) = geom.getParams()
log.debug('geom len=%f xyz=%f,%f,%f', length, x, y, z)
# plot the geom
self.plotAxes()
if self.render_bps:
glTranslate(0, 0, -length / 2)
b = 0.8
if not geom.parent:
b = 0.0
glColor(0, 0, b)
gluCylinder(self.quadratic, radius, radius, length, 16, 16)
if geom.left == 'internal':
glColor(0, 1, 0)
else:
glColor(1, 0, 0)
gluSphere(self.quadratic, radius, 10, 10)
glTranslate(0, 0, length)
if geom.right == 'internal':
glColor(0, 1, 0)
else:
glColor(1, 0, 0)
gluSphere(self.quadratic, radius, 10, 10)
else:
log.critical('dont know how to render geom %s', str(geom))
glPopMatrix()
def reset_camera(self): self.camera = mat4(1.0) self.camera.translate(vec3(0, 0, 0.2))
def recompose_camera(self): (tr, rot, _) = self.camera.decompose() self.camera = mat4(1.0) self.camera.translate(tr) self.camera = self.camera * rot
def buildTransformMatrix( location, yaw, pitch, roll ):
mloc = mat4(1).translation( location )
return mloc * buildRotationMatrix( yaw, pitch, roll )
def test_from4lists(self):
m1 = cg.mat4([0, 1.6, 2, 3], [4.977, 5, 6, .007], [8, 0, 1.6, 2], [3, 4.977, 5, 6])
m2 = cycg.mat4([0, 1.6, 2, 3], [4.977, 5, 6, .007], [8, 0, 1.6, 2], [3, 4.977, 5, 6])
self.assertEqual(m1, m2)
print('bor_mat4_t mat4s[] = {')
for m in mat4s:
print(' BOR_MAT4_STATIC({0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, {9}, {10}, {11}, {12}, {13}, {14}, {15}),'.format(*m))
print('};')
print('size_t mat4s_len = sizeof(mat4s) / sizeof(bor_mat4_t);')
print('')
print('')
def pr(m, p = ''):
a = m.toList(True)
print('# {0} {1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} {12} {13} {14} {15} {16}'.format(p, *a))
print('# ---- add sub ----')
for i in range(0, LEN - 1):
a = mat4(*mat4s[i])
b = mat4(*mat4s[i + 1])
pr(a + b, 'add')
pr(a - b, 'sub')
print('# ---- add sub end ----')
print('')
print('# ---- const ----')
for i in range(0, LEN):
a = mat4(*mat4s[i])
c = mat4s[i][0]
pr(a * c, 'scale')
d = mat4(c, c, c, c, c, c, c, c, c, c, c, c, c, c, c, c)
pr(a + d, 'add')
print('# ---- const end ----')
def buildTransformMatrix(location, yaw, pitch, roll):
mloc = mat4(1).translation(location)
return mloc * buildRotationMatrix(yaw, pitch, roll)
def getTransform(self, name, space, useSensorValues, joints=None):
# Utilize cache if available
if name in self.__cache[space]:
return self.__cache[space][name]
if not joints:
joints = dict(zip(self.joints, self.nao.angles))
mat = mat4.identity()
larm = [
'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll',
'LWristYaw', 'LArm'
]
rarm = [
'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll',
'RWristYaw', 'RArm'
]
lleg = [
'LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch',
'LAnklePitch', 'LAnkleRoll', 'LLeg'
]
rleg = [
'RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch',
'RAnklePitch', 'RAnkleRoll', 'RLeg'
]
if name in larm:
path = larm[:larm.index(name) + 1]
for joint in reversed(path):
if joint == "LShoulderPitch":
offset = vec3(0, self.geometry['ShoulderOffsetY'],
self.geometry['ShoulderOffsetZ'])
mat.rotate(-joints['LShoulderPitch'], AXIS_Y)
mat.translate(-offset)
elif joint == "LShoulderRoll":
mat.rotate(-joints['LShoulderRoll'], AXIS_Z)
elif joint == "LElbowYaw":
mat.rotate(-joints['LElbowYaw'], AXIS_X)
offset = vec3(self.geometry['UpperArmLength'],
self.geometry['ElbowOffsetY'], 0)
mat.translate(-offset)
elif joint == "LElbowRoll":
mat.rotate(-joints['LElbowRoll'], AXIS_Z)
elif joint == "LWristYaw":
mat.rotate(-joints['LWristYaw'], AXIS_X)
offset = vec3(self.geometry['LowerArmLength'], 0, 0)
mat.translate(-offset)
elif joint == "LHand" or name == "LArm":
offset = vec3(self.geometry['HandOffsetX'], 0,
-self.geometry['HandOffsetZ'])
mat.translate(-offset)
elif name in rarm:
path = rarm[:rarm.index(name) + 1]
for joint in reversed(path):
if joint == "RShoulderPitch":
offset = vec3(0, -self.geometry['ShoulderOffsetY'],
self.geometry['ShoulderOffsetZ'])
mat.rotate(-joints['RShoulderPitch'], AXIS_Y)
mat.translate(-offset)
elif joint == "RShoulderRoll":
mat.rotate(-joints['RShoulderRoll'], AXIS_Z)
elif joint == "RElbowYaw":
mat.rotate(-joints['RElbowYaw'], AXIS_X)
offset = vec3(self.geometry['UpperArmLength'],
-self.geometry['ElbowOffsetY'], 0)
mat.translate(-offset)
elif joint == "RElbowRoll":
mat.rotate(-joints['RElbowRoll'], AXIS_Z)
elif joint == "RWristYaw":
mat.rotate(-joints['RWristYaw'], AXIS_X)
offset = vec3(self.geometry['LowerArmLength'], 0, 0)
mat.translate(-offset)
elif joint == "RHand" or name == "RArm":
offset = vec3(self.geometry['HandOffsetX'], 0,
-self.geometry['HandOffsetZ'])
mat.translate(-offset)
elif name == "Torso":
pass # No translation
elif name == "Head":
offset = vec3(0, 0, self.geometry['NeckOffsetZ'])
mat.rotate(-joints['HeadPitch'], AXIS_Y)
mat.rotate(-joints['HeadYaw'], AXIS_Z)
mat.translate(-offset)
elif name == "CameraTop":
offset = vec3(self.geometry['CameraTopOffsetX'],
self.geometry['CameraTopOffsetY'],
self.geometry['CameraTopOffsetZ'])
mat.translate(-offset)
mat *= self.getTransform("Head", SPACE_TORSO, useSensorValues,
joints)
elif name == "CameraBottom":
mat.rotate(-self.geometry['CameraBottomPitch'], AXIS_Y)
offset = vec3(self.geometry['CameraBottomOffsetX'],
self.geometry['CameraBottomOffsetY'],
self.geometry['CameraBottomOffsetZ'])
mat.translate(-offset)
mat *= self.getTransform("Head", SPACE_TORSO, useSensorValues,
joints)
elif name in lleg:
path = lleg[:lleg.index(name) + 1]
for joint in reversed(path):
if joint == "LHipYawPitch":
hipYP = vec3(0, 1, -1).normalize()
mat.rotate(-joints['LHipYawPitch'], hipYP)
offset = vec3(0, self.geometry['HipOffsetY'],
-self.geometry['HipOffsetZ'])
mat.translate(-offset)
elif joint == "LHipRoll":
mat.rotate(-joints['LHipRoll'], AXIS_X)
elif joint == "LHipPitch":
mat.rotate(-joints['LHipPitch'], AXIS_Y)
elif joint == "LKneePitch":
mat.rotate(-joints['LKneePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['ThighLength'])
mat.translate(-offset)
elif joint == "LAnklePitch":
mat.rotate(-joints['LAnklePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['TibiaLength'])
mat.translate(-offset)
elif joint == "LAnkleRoll":
mat.rotate(-joints['LAnkleRoll'], AXIS_X)
elif joint == "LLeg":
offset = vec3(0, 0, -self.geometry['FootHeight'])
mat.translate(-offset)
elif name in rleg:
path = rleg[:rleg.index(name) + 1]
for joint in reversed(path):
if joint == "RHipYawPitch":
hipYP = vec3(0, -1, -1).normalize()
mat.rotate(-joints['RHipYawPitch'], hipYP)
offset = vec3(0, -self.geometry['HipOffsetY'],
-self.geometry['HipOffsetZ'])
mat.translate(-offset)
elif joint == "RHipRoll":
mat.rotate(-joints['RHipRoll'], AXIS_X)
elif joint == "RHipPitch":
mat.rotate(-joints['RHipPitch'], AXIS_Y)
elif joint == "RKneePitch":
mat.rotate(-joints['RKneePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['ThighLength'])
mat.translate(-offset)
elif joint == "RAnklePitch":
mat.rotate(-joints['RAnklePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['TibiaLength'])
mat.translate(-offset)
elif joint == "RAnkleRoll":
mat.rotate(-joints['RAnkleRoll'], AXIS_X)
elif joint == "RLeg":
offset = vec3(0, 0, -self.geometry['FootHeight'])
mat.translate(-offset)
else:
print "Unknown effector : `%s'" % name
self.__cache[SPACE_TORSO][name] = mat4(mat)
if space == SPACE_TORSO:
return mat
# SPACE_NAO: Need the position of the torso in Nao Space. The origin is
# located at the average of the positions of the two feet.
#
# CAUTION: this doesn't fully work. Origin might be calculated correctly
# calculated but the orientation is defined such that the X-axis must
# always look forward so with complicated leg configurations, the
# coordinate system might not be the exact average between the two
# orientations. Or something like that. I don't know, I give up ;-)
lleg = self.getPosition("LLeg", SPACE_TORSO, True)
rleg = self.getPosition("RLeg", SPACE_TORSO, True)
lpos = vec3(lleg[:3])
avg = list(numpy.average([lleg, rleg], 0))
offset = vec3(avg[:3])
# Setup transformation matrix
change = mat4.identity()
change.setMat3(mat3.fromEulerZYX(avg[5], avg[4], avg[3]))
mat.translate(offset)
mat *= change
self.__cache[SPACE_NAO][name] = mat4(mat)
if space == SPACE_NAO:
return mat
# SPACE_WORLD: The last option. This is the same as SPACE_NAO when
# naoqi starts, but when the NAO moves about, the origin is left behind.
#
# CAUTION: behavior not identical to naoqi (yet). Don't know what the
# problem is, but the rotation and position reported by a (simulated)
# naoqi are different from the values calculated here, although they do
# seem to make sense... Don't known, don't care, give up (for now).
# Probably naoqi has an estimate of the error when performing walks and
# incorporates that in the resulting odometry.
offset = vec3(self.nao.position)
mat.rotate(-self.nao.orientation, AXIS_Z)
mat.translate(-offset)
self.__cache[SPACE_WORLD][name] = mat4(mat)
return mat
warnings.filterwarnings("ignore", category=DeprecationWarning)
# Global variables
brain = None
program = None
epoc = None
sample_sec = 2.0
localizer = None
source_locations = []
localizer_thread_alive = True
influential_per_source = 3
most_influential_electrodes = dict()
connecting_line_width = 2.0
# Rotation variables:
rotation_matrix = mat4(1.0)
prev_x = 0
prev_y = 0
curr_x = 0
curr_y = 0
arcball_on = False
screen_w = 800
screen_h = 600
zoom_factor = 1.0
# Drawing mode for fragment shader:
# 0 - simple color
# 1 - blinn model
# 2 - xray
# 3 - xray with half of the intensity
def buildTransformMatrixQ(location, q4):
mloc = mat4(1).translation(location)
mrot = q4.toMat4()
return mloc * mrot
def recompose_camera(self): (tr, rot, _) = self.camera.decompose() self.camera = mat4(1.0) self.camera.translate(tr) self.camera = self.camera * rot
def matrixChanged(self, body, matrix):
self._matrix = mat4(matrix).transpose()
self._readLocationFromPhysics()
self._adjustBoundingVolume()
data_scale = 1
xgain *= data_scale
ygain *= data_scale
X = (X - xoffset) * xgain
Y = (Y - yoffset) * ygain
if 0:
import pylab
pylab.plot(X[:, 0], Y[:, 0], '.')
pylab.show()
#sys.exit(0)
if 0:
pos_vec3, ori_quat = cgtypes.vec3(nums[0:3]), cgtypes.quat(nums[3:7])
M = cgtypes.mat4().identity().translate(pos_vec3)
M = M * ori_quat.toMat4()
# grr, I hate cgtypes to numpy conversions!
M = np.array((M[0, 0], M[1, 0], M[2, 0], M[3, 0], M[0, 1], M[1, 1],
M[2, 1], M[3, 1], M[0, 2], M[1, 2], M[2, 2], M[3, 2],
M[0, 3], M[1, 3], M[2, 3], M[3, 3]))
M.shape = (4, 4)
fly_model_node_filename = os.path.join(fsee.data_dir,
'models/fly/body.osg')
model_path = os.path.join(fsee.data_dir,
"models/alice_cylinder/alice_cylinder.osg")
model_path = os.path.join(fsee.data_dir,
"models/alice_cylinder/white_cylinder.osg")
#model_path = None
z = 2 # 2 mm
def matrixChanged(self, body, matrix):
self._matrix = mat4(matrix).transpose()
self._readLocationFromPhysics()
self._adjustBoundingVolume()
def getTransform(self, name, space, useSensorValues, joints=None):
# Utilize cache if available
if name in self.__cache[space]:
return self.__cache[space][name]
if not joints:
joints = dict(zip(self.joints, self.nao.angles))
mat = mat4.identity()
larm = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'LWristYaw', 'LArm']
rarm = ['RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw', 'RArm']
lleg = ['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'LLeg']
rleg = ['RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll', 'RLeg']
if name in larm:
path = larm[:larm.index(name) + 1]
for joint in reversed(path):
if joint == "LShoulderPitch":
offset = vec3(0, self.geometry['ShoulderOffsetY'], self.geometry['ShoulderOffsetZ'])
mat.rotate(-joints['LShoulderPitch'], AXIS_Y)
mat.translate(-offset)
elif joint == "LShoulderRoll":
mat.rotate(-joints['LShoulderRoll'], AXIS_Z)
elif joint == "LElbowYaw":
mat.rotate(-joints['LElbowYaw'], AXIS_X)
offset = vec3(self.geometry['UpperArmLength'], self.geometry['ElbowOffsetY'], 0)
mat.translate(-offset)
elif joint == "LElbowRoll":
mat.rotate(-joints['LElbowRoll'], AXIS_Z)
elif joint == "LWristYaw":
mat.rotate(-joints['LWristYaw'], AXIS_X)
offset = vec3(self.geometry['LowerArmLength'], 0, 0)
mat.translate(-offset)
elif joint == "LHand" or name == "LArm":
offset = vec3(self.geometry['HandOffsetX'], 0, -self.geometry['HandOffsetZ'])
mat.translate(-offset)
elif name in rarm:
path = rarm[:rarm.index(name) + 1]
for joint in reversed(path):
if joint == "RShoulderPitch":
offset = vec3(0, -self.geometry['ShoulderOffsetY'], self.geometry['ShoulderOffsetZ'])
mat.rotate(-joints['RShoulderPitch'], AXIS_Y)
mat.translate(-offset)
elif joint == "RShoulderRoll":
mat.rotate(-joints['RShoulderRoll'], AXIS_Z)
elif joint == "RElbowYaw":
mat.rotate(-joints['RElbowYaw'], AXIS_X)
offset = vec3(self.geometry['UpperArmLength'], -self.geometry['ElbowOffsetY'], 0)
mat.translate(-offset)
elif joint == "RElbowRoll":
mat.rotate(-joints['RElbowRoll'], AXIS_Z)
elif joint == "RWristYaw":
mat.rotate(-joints['RWristYaw'], AXIS_X)
offset = vec3(self.geometry['LowerArmLength'], 0, 0)
mat.translate(-offset)
elif joint == "RHand" or name == "RArm":
offset = vec3(self.geometry['HandOffsetX'], 0, -self.geometry['HandOffsetZ'])
mat.translate(-offset)
elif name == "Torso":
pass # No translation
elif name == "Head":
offset = vec3(0, 0, self.geometry['NeckOffsetZ'])
mat.rotate(-joints['HeadPitch'], AXIS_Y)
mat.rotate(-joints['HeadYaw'], AXIS_Z)
mat.translate(-offset)
elif name == "CameraTop":
offset = vec3(self.geometry['CameraTopOffsetX'],
self.geometry['CameraTopOffsetY'],
self.geometry['CameraTopOffsetZ'])
mat.translate(-offset)
mat *= self.getTransform("Head", SPACE_TORSO, useSensorValues, joints)
elif name == "CameraBottom":
mat.rotate(-self.geometry['CameraBottomPitch'], AXIS_Y)
offset = vec3(self.geometry['CameraBottomOffsetX'],
self.geometry['CameraBottomOffsetY'],
self.geometry['CameraBottomOffsetZ'])
mat.translate(-offset)
mat *= self.getTransform("Head", SPACE_TORSO, useSensorValues, joints)
elif name in lleg:
path = lleg[:lleg.index(name) + 1]
for joint in reversed(path):
if joint == "LHipYawPitch":
hipYP = vec3(0, 1, -1).normalize()
mat.rotate(-joints['LHipYawPitch'], hipYP)
offset = vec3(0, self.geometry['HipOffsetY'], -self.geometry['HipOffsetZ'])
mat.translate(-offset)
elif joint == "LHipRoll":
mat.rotate(-joints['LHipRoll'], AXIS_X)
elif joint == "LHipPitch":
mat.rotate(-joints['LHipPitch'], AXIS_Y)
elif joint == "LKneePitch":
mat.rotate(-joints['LKneePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['ThighLength'])
mat.translate(-offset)
elif joint == "LAnklePitch":
mat.rotate(-joints['LAnklePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['TibiaLength'])
mat.translate(-offset)
elif joint == "LAnkleRoll":
mat.rotate(-joints['LAnkleRoll'], AXIS_X)
elif joint == "LLeg":
offset = vec3(0, 0, -self.geometry['FootHeight'])
mat.translate(-offset)
elif name in rleg:
path = rleg[:rleg.index(name) + 1]
for joint in reversed(path):
if joint == "RHipYawPitch":
hipYP = vec3(0, -1, -1).normalize()
mat.rotate(-joints['RHipYawPitch'], hipYP)
offset = vec3(0, -self.geometry['HipOffsetY'], -self.geometry['HipOffsetZ'])
mat.translate(-offset)
elif joint == "RHipRoll":
mat.rotate(-joints['RHipRoll'], AXIS_X)
elif joint == "RHipPitch":
mat.rotate(-joints['RHipPitch'], AXIS_Y)
elif joint == "RKneePitch":
mat.rotate(-joints['RKneePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['ThighLength'])
mat.translate(-offset)
elif joint == "RAnklePitch":
mat.rotate(-joints['RAnklePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['TibiaLength'])
mat.translate(-offset)
elif joint == "RAnkleRoll":
mat.rotate(-joints['RAnkleRoll'], AXIS_X)
elif joint == "RLeg":
offset = vec3(0, 0, -self.geometry['FootHeight'])
mat.translate(-offset)
else:
print "Unknown effector : `%s'" % name
self.__cache[SPACE_TORSO][name] = mat4(mat)
if space == SPACE_TORSO:
return mat
# SPACE_NAO: Need the position of the torso in Nao Space. The origin is
# located at the average of the positions of the two feet.
#
# CAUTION: this doesn't fully work. Origin might be calculated correctly
# calculated but the orientation is defined such that the X-axis must
# always look forward so with complicated leg configurations, the
# coordinate system might not be the exact average between the two
# orientations. Or something like that. I don't know, I give up ;-)
lleg = self.getPosition("LLeg", SPACE_TORSO, True)
rleg = self.getPosition("RLeg", SPACE_TORSO, True)
lpos = vec3(lleg[:3])
avg = list(numpy.average([lleg, rleg], 0))
offset = vec3(avg[:3])
# Setup transformation matrix
change = mat4.identity()
change.setMat3(mat3.fromEulerZYX(avg[5], avg[4], avg[3]))
mat.translate(offset)
mat *= change
self.__cache[SPACE_NAO][name] = mat4(mat)
if space == SPACE_NAO:
return mat
# SPACE_WORLD: The last option. This is the same as SPACE_NAO when
# naoqi starts, but when the NAO moves about, the origin is left behind.
#
# CAUTION: behavior not identical to naoqi (yet). Don't know what the
# problem is, but the rotation and position reported by a (simulated)
# naoqi are different from the values calculated here, although they do
# seem to make sense... Don't known, don't care, give up (for now).
# Probably naoqi has an estimate of the error when performing walks and
# incorporates that in the resulting odometry.
offset = vec3(self.nao.position)
mat.rotate(-self.nao.orientation, AXIS_Z)
mat.translate(-offset)
self.__cache[SPACE_WORLD][name] = mat4(mat)
return mat
def buildRotationMatrix(yaw, pitch, roll):
m = mat4(1)
m.rotate(yaw % (math.pi * 2), vec3(0, 1, 0))
m.rotate(pitch % (math.pi * 2), vec3(1, 0, 0))
m.rotate(roll % (math.pi * 2), vec3(0, 0, 1))
return m
def MakeRotationMatrix( yaw, pitch, roll ): base = mat4(1) myaw = base.rotation( yaw, vec3( 0, 1, 0 ) ) mpitch = base.rotation( pitch, vec3( 1, 0, 0 ) ) mroll = base.rotation( roll, vec3( 0, 0, 1 ) ) return myaw * mpitch * mroll
def test_constructor_transform(self):
joint = Joint()
mat = mat4()
self.assertEqual(joint.transform, mat)
def buildTransformMatrixQ(location, q4):
mloc = mat4(1).translation( location )
mrot = q4.toMat4()
return mloc * mrot
def test_root_transform_parent(self):
transform_parent = mat4().identity()
self.assertEqual(self._skeleton._root.transform_parent, transform_parent)
def buildRotationMatrix( yaw, pitch, roll ):
m = mat4(1)
m.rotate(yaw % (math.pi*2), vec3( 0, 1, 0 ) )
m.rotate(pitch % (math.pi*2), vec3( 1, 0, 0 ) )
m.rotate(roll % (math.pi*2), vec3( 0, 0, 1 ) )
return m
def MakeRotationMatrix(yaw, pitch, roll):
base = mat4(1)
myaw = base.rotation(yaw, vec3(0, 1, 0))
mpitch = base.rotation(pitch, vec3(1, 0, 0))
mroll = base.rotation(roll, vec3(0, 0, 1))
return myaw * mpitch * mroll
def reset_camera(self): self.camera = mat4(1.0) self.camera.translate(vec3(0, 0, 0.5))
