Esempio n. 1
0
    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
Esempio n. 2
0
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
Esempio n. 3
0
  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()
Esempio n. 4
0
    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))
Esempio n. 5
0
	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
Esempio n. 6
0
 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)
Esempio n. 9
0
	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)
Esempio n. 11
0
	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
Esempio n. 12
0
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
Esempio n. 13
0
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
Esempio n. 14
0
	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
Esempio n. 15
0
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
Esempio n. 16
0
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()
Esempio n. 17
0
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'
Esempio n. 18
0
    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)
Esempio n. 19
0
    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)
Esempio n. 20
0
    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()
Esempio n. 21
0
 def reset_camera(self):
   self.camera = mat4(1.0)
   self.camera.translate(vec3(0, 0, 0.2))
Esempio n. 22
0
 def recompose_camera(self):
   (tr, rot, _) = self.camera.decompose()
   self.camera = mat4(1.0)
   self.camera.translate(tr)
   self.camera = self.camera * rot
Esempio n. 23
0
def buildTransformMatrix( location, yaw, pitch, roll ):
    mloc   = mat4(1).translation( location )
    return mloc * buildRotationMatrix( yaw, pitch, roll )
Esempio n. 24
0
 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)
Esempio n. 25
0
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 ----')
Esempio n. 26
0
def buildTransformMatrix(location, yaw, pitch, roll):
    mloc = mat4(1).translation(location)
    return mloc * buildRotationMatrix(yaw, pitch, roll)
Esempio n. 27
0
    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
Esempio n. 28
0
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
Esempio n. 29
0
def buildTransformMatrixQ(location, q4):
    mloc = mat4(1).translation(location)
    mrot = q4.toMat4()
    return mloc * mrot
Esempio n. 30
0
 def recompose_camera(self):
   (tr, rot, _) = self.camera.decompose()
   self.camera = mat4(1.0)
   self.camera.translate(tr)
   self.camera = self.camera * rot
Esempio n. 31
0
 def matrixChanged(self, body, matrix):
     self._matrix = mat4(matrix).transpose()
     self._readLocationFromPhysics()
     self._adjustBoundingVolume()
Esempio n. 32
0
    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
Esempio n. 33
0
 def matrixChanged(self, body, matrix):
     self._matrix = mat4(matrix).transpose()
     self._readLocationFromPhysics()
     self._adjustBoundingVolume()
Esempio n. 34
0
    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
Esempio n. 35
0
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
Esempio n. 36
0
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)
Esempio n. 38
0
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)
Esempio n. 40
0
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
Esempio n. 41
0
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
Esempio n. 42
0
 def reset_camera(self):
   self.camera = mat4(1.0)
   self.camera.translate(vec3(0, 0, 0.5))