Beispiel #1
0
    def draw_body_geom(self, body, geom):
        """Draw an ODE body. Also draws a (bodyless) plane. """
        # check if external texture with "addTexture(...)" is set
        # if body and body.name in self.textures.keys():
        #     # load texture (with function from the "MyOpenGLTools" module)
        #     tex = self._loadTexture(self.textures[body.name])
        #     glEnable(GL_TEXTURE_2D)
        #     glBindTexture(GL_TEXTURE_2D, tex[2])
        # elif geom.name in self.textures.keys():
        #     # in case of plane, no body is present. Load texture anyway.
        #     tex = self._loadTexture(self.textures[geom.name])
        #     glEnable(GL_TEXTURE_2D)
        #     glBindTexture(GL_TEXTURE_2D, tex[2])
        # else:
        # no texture request found for this object, disable textures
        glDisable(GL_TEXTURE_2D)

        glPushMatrix()
        # draw real bodies (boxes, spheres, ...)
        if body != None:
            # set color of object (currently dark gray)
            glColor3f(0.3, 0.3, 0.3)

            # transform (rotate, translate) body accordingly
            (x, y, z) = body.getPosition()
            R = body.getRotation()

            rot = [
                R[0], R[3], R[6], 0.0, R[1], R[4], R[7], 0.0, R[2], R[5], R[8],
                0.0, x, y, z, 1.0
            ]
            glMultMatrixd(rot)

            # switch different geom objects
            if type(geom) == ode.GeomBox:
                # cube
                (sx, sy, sz) = geom.getLengths()
                glScaled(sx, sy, sz)
                glutSolidCube(1)
            elif type(geom) == ode.GeomSphere:
                # sphere
                radius = geom.getRadius()
                glutSolidSphere(radius, 20, 20)

            elif type(geom) == ode.GeomCCylinder:
                # capped cylinder
                radius = geom.getParams()[0]
                length = geom.getParams()[1] - 2 * radius
                quad = gluNewQuadric()
                # draw cylinder and two spheres, one at each end
                glTranslate(0.0, 0.0, -length / 2)
                gluCylinder(quad, radius, radius, length, 32, 32)
                glutSolidSphere(radius, 20, 20)
                glTranslate(0.0, 0.0, length)
                glutSolidSphere(radius, 20, 20)

            # FIXME: sometimes cylinder attribute is undefined; this has something to
            # to with the pyODE hack, but we don't know anymore where it came from...  X-(
            elif hasattr(ode, 'GeomCylinder'):
                if type(geom) == ode.GeomCylinder:
                    # solid cylinder
                    radius = geom.getParams()[0]
                    length = geom.getParams()[1]
                    glTranslate(0.0, 0.0, -length / 2)
                    quad = gluNewQuadric()
                    gluDisk(quad, 0, radius, 32, 1)
                    quad = gluNewQuadric()
                    gluCylinder(quad, radius, radius, length, 32, 32)
                    glTranslate(0.0, 0.0, length)
                    quad = gluNewQuadric()
                    gluDisk(quad, 0, radius, 32, 1)
            else:
                # TODO: add other geoms here
                pass

        else:
            # no body found, then it must be a plane (we only draw planes)
            if type(geom) == ode.GeomPlane:
                # set color of plane (currently green)
                if self.isFloorGreen:
                    glColor3f(0.2, 0.6, 0.3)
                else:
                    glColor3f(0.2, 0.3, 0.8)

                # for planes, we need a Quadric object
                quad = gluNewQuadric()
                gluQuadricTexture(quad, GL_TRUE)

                p = geom.getParams()[0]  # the normal vector to the plane
                d = geom.getParams()[1]  # the distance to the origin
                q = (0.0, 0.0, 1.0
                     )  # the normal vector of default gluDisks (z=0 plane)

                # calculate the cross product to get the rotation axis
                c = crossproduct(p, q)
                # calculate the angle between default normal q and plane normal p
                theta = acos(dotproduct(p, q) / (norm(p) * norm(q))) / pi * 180

                # rotate the plane
                glPushMatrix()
                glTranslate(d * p[0], d * p[1], d * p[2])
                glRotate(-theta, c[0], c[1], c[2])
                gluDisk(quad, 0, 20, 20, 1)
                glPopMatrix()

        glPopMatrix()
Beispiel #2
0
    def draw_item(self, item):
        """Draw Item

        Draws an object from the ODE specifications.

        Arguments:
            item: A dictionary containing keys relating to an ODE geometry.
        """
        glDisable(GL_TEXTURE_2D)

        # Create a matrix for the new geometry calculations
        glPushMatrix()

        # Determine which geometric object we are dealing with. Draw it
        if item['type'] in ['box', 'sphere', 'cylinder', 'ccylinder']:
            # set color of object (currently dark gray)
            if item.has_key('color'):
                glEnable(GL_BLEND)
                glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
                glColor4f(*(item['color']))
            else: glColor3f(0.1, 0.1, 0.1)

            # transform (rotate, translate) body accordingly
            (x, y, z) = item['position']
            R = item['rotation']

            # Create rotation matrix
            rot = [R[0], R[3], R[6], 0.0,
                   R[1], R[4], R[7], 0.0,
                   R[2], R[5], R[8], 0.0,
                      x,    y,    z, 1.0]

            # Apply rotation matrix to any shape created
            glMultMatrixd(rot)

            if item['type'] == 'box':
                # Draw a cube scaled to size
                (sx, sy, sz) = item['scale']
                glScaled(float(sx), float(sy), float(sz))
                glutSolidCube(1.0)
            elif item['type'] == 'sphere':
                # sphere
                glutSolidSphere(item['radius'], 20, 20)
            elif item['type'] == 'ccylinder':
                quad = gluNewQuadric()
                # draw cylinder and two spheres, one at each end
                glTranslate(0.0, 0.0, -item['length']/2.0)

                # Draw the cylinder object
                gluCylinder(quad, item['radius'], item['radius'], item['length'], 32, 32)

                # Draw the first cylinder end sphere
                glutSolidSphere(item['radius'], 20, 20)

                # Draw the second cylinder end sphere
                glTranslate(0.0, 0.0, item['length'])
                glutSolidSphere(item['radius'], 20, 20)
            elif item['type'] == 'cylinder':
                glTranslate(0.0, 0.0, -item['length']/2.0)

                # Draw the first cylinder end disk
                quad = gluNewQuadric()
                # Modify the orientation. This quad normal is inward
                gluQuadricOrientation(quad, GLU_INSIDE)
                gluDisk(quad, 0, item['radius'], 32, 1)

                # Draw the cylinder
                quad = gluNewQuadric()
                gluCylinder(quad, item['radius'], item['radius'], item['length'], 32, 32)

                # Draw the second cylinder end disk
                glTranslate(0.0, 0.0, item['length'])
                quad = gluNewQuadric()
                gluDisk(quad, 0.0, item['radius'], 32, 1)
        elif item['type'] == 'plane':
            # set color of plane (currently green)
            glColor3f(0.0, 0.2, 0.0)

            # for planes, we need a Quadric object
            quad = gluNewQuadric()
            gluQuadricTexture(quad, GL_TRUE)

            p = item['normal']      # the normal vector to the plane
            d = item['distance']    # the distance to the origin
            q = (0.0, 0.0, 1.0)     # the normal vector of default gluDisks (z=0 plane)

            # calculate the cross product to get the rotation axis
            c = crossproduct(p, q)
            # calculate the angle between default normal q and plane normal p
            theta = acos(dotproduct(p, q) / (norm(p) * norm(q))) / pi * 180

            # rotate the plane
            glTranslate(d * p[0], d * p[1], d * p[2])
            glRotate(-theta, c[0], c[1], c[2])

            # Create the disk with "infinite" (very large) radius
            gluDisk(quad, 0, 200, 20, 1)

        glPopMatrix()
        return
Beispiel #3
0
    def draw_item(self, item):
        """ draws an object (spere, cube, plane, ...) """
        glDisable(GL_TEXTURE_2D)

        glPushMatrix()

        if item['type'] in ['GeomBox', 'GeomSphere', 'GeomCylinder', 'GeomCCylinder']:
            # set color of object (currently dark gray)
            if item.has_key('color'):
                glEnable (GL_BLEND)
                glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
                glColor4f(*(item['color']))
            else: glColor3f(0.1, 0.1, 0.1)

            # transform (rotate, translate) body accordingly
            (x, y, z) = item['position']
            R = item['rotation']

            rot = [R[0], R[3], R[6], 0.0,
                   R[1], R[4], R[7], 0.0,
                   R[2], R[5], R[8], 0.0,
                      x, y, z, 1.0]

            glMultMatrixd(rot)

            # switch different geom objects
            if item['type'] == 'GeomBox':
                # cube
                (sx, sy, sz) = item['scale']
                glScaled(sx, sy, sz)
                glutSolidCube(1)
            elif item['type'] == 'GeomSphere':
                # sphere
                glutSolidSphere(item['radius'], 20, 20)

            elif item['type'] == 'GeomCCylinder':
                quad = gluNewQuadric()
                # draw cylinder and two spheres, one at each end
                glTranslate(0.0, 0.0, -item['length'] / 2)
                gluCylinder(quad, item['radius'], item['radius'], item['length'], 32, 32)
                glutSolidSphere(item['radius'], 20, 20)
                glTranslate(0.0, 0.0, item['length'])
                glutSolidSphere(item['radius'], 20, 20)

            elif item['type'] == 'GeomCylinder':
                glTranslate(0.0, 0.0, -item['length'] / 2)
                quad = gluNewQuadric()
                gluDisk(quad, 0, item['radius'], 32, 1)
                quad = gluNewQuadric()
                gluCylinder(quad, item['radius'], item['radius'], item['length'], 32, 32)
                glTranslate(0.0, 0.0, item['length'])
                quad = gluNewQuadric()
                gluDisk(quad, 0, item['radius'], 32, 1)
            else:
                # TODO: add other geoms here
                pass

        elif item['type'] == 'GeomPlane':
            # set color of plane (currently green)
            if self.isFloorGreen:
                glColor3f(0.2, 0.6, 0.3)
            else:
                glColor3f(0.2, 0.3, 0.8)

            # for planes, we need a Quadric object
            quad = gluNewQuadric()
            gluQuadricTexture(quad, GL_TRUE)

            p = item['normal']      # the normal vector to the plane
            d = item['distance']    # the distance to the origin
            q = (0.0, 0.0, 1.0)     # the normal vector of default gluDisks (z=0 plane)

            # calculate the cross product to get the rotation axis
            c = crossproduct(p, q)
            # calculate the angle between default normal q and plane normal p
            theta = acos(dotproduct(p, q) / (norm(p) * norm(q))) / pi * 180

            # rotate the plane
            glPushMatrix()
            glTranslate(d * p[0], d * p[1], d * p[2])
            glRotate(-theta, c[0], c[1], c[2])
            gluDisk(quad, 0, 20, 20, 1)
            glPopMatrix()

        glPopMatrix()
    def draw_item(self, item):
        """ draws an object (spere, cube, plane, ...) """
        glDisable(GL_TEXTURE_2D)

        glPushMatrix()

        if item['type'] in [
                'GeomBox', 'GeomSphere', 'GeomCylinder', 'GeomCCylinder'
        ]:
            # set color of object (currently dark gray)
            if item.has_key('color'):
                glEnable(GL_BLEND)
                glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
                glColor4f(*(item['color']))
            else:
                glColor3f(0.1, 0.1, 0.1)

            # transform (rotate, translate) body accordingly
            (x, y, z) = item['position']
            R = item['rotation']

            rot = [
                R[0], R[3], R[6], 0.0, R[1], R[4], R[7], 0.0, R[2], R[5], R[8],
                0.0, x, y, z, 1.0
            ]

            glMultMatrixd(rot)

            # switch different geom objects
            if item['type'] == 'GeomBox':
                # cube
                (sx, sy, sz) = item['scale']
                glScaled(sx, sy, sz)
                glutSolidCube(1)
            elif item['type'] == 'GeomSphere':
                # sphere
                glutSolidSphere(item['radius'], 20, 20)

            elif item['type'] == 'GeomCCylinder':
                quad = gluNewQuadric()
                # draw cylinder and two spheres, one at each end
                glTranslate(0.0, 0.0, -item['length'] / 2)
                gluCylinder(quad, item['radius'], item['radius'],
                            item['length'], 32, 32)
                glutSolidSphere(item['radius'], 20, 20)
                glTranslate(0.0, 0.0, item['length'])
                glutSolidSphere(item['radius'], 20, 20)

            elif item['type'] == 'GeomCylinder':
                glTranslate(0.0, 0.0, -item['length'] / 2)
                quad = gluNewQuadric()
                gluDisk(quad, 0, item['radius'], 32, 1)
                quad = gluNewQuadric()
                gluCylinder(quad, item['radius'], item['radius'],
                            item['length'], 32, 32)
                glTranslate(0.0, 0.0, item['length'])
                quad = gluNewQuadric()
                gluDisk(quad, 0, item['radius'], 32, 1)
            else:
                # TODO: add other geoms here
                pass

        elif item['type'] == 'GeomPlane':
            # set color of plane (currently green)
            if self.isFloorGreen:
                glColor3f(0.2, 0.6, 0.3)
            else:
                glColor3f(0.2, 0.3, 0.8)

            # for planes, we need a Quadric object
            quad = gluNewQuadric()
            gluQuadricTexture(quad, GL_TRUE)

            p = item['normal']  # the normal vector to the plane
            d = item['distance']  # the distance to the origin
            q = (0.0, 0.0, 1.0
                 )  # the normal vector of default gluDisks (z=0 plane)

            # calculate the cross product to get the rotation axis
            c = crossproduct(p, q)
            # calculate the angle between default normal q and plane normal p
            theta = acos(dotproduct(p, q) / (norm(p) * norm(q))) / pi * 180

            # rotate the plane
            glPushMatrix()
            glTranslate(d * p[0], d * p[1], d * p[2])
            glRotate(-theta, c[0], c[1], c[2])
            gluDisk(quad, 0, 20, 20, 1)
            glPopMatrix()

        glPopMatrix()
Beispiel #5
0
    def draw_body_geom(self, body, geom):
        """Draw an ODE body. Also draws a (bodyless) plane. """
        # check if external texture with "addTexture(...)" is set
        # if body and body.name in self.textures.keys():
        #     # load texture (with function from the "MyOpenGLTools" module)
        #     tex = self._loadTexture(self.textures[body.name])
        #     glEnable(GL_TEXTURE_2D)
        #     glBindTexture(GL_TEXTURE_2D, tex[2])
        # elif geom.name in self.textures.keys():
        #     # in case of plane, no body is present. Load texture anyway.
        #     tex = self._loadTexture(self.textures[geom.name])
        #     glEnable(GL_TEXTURE_2D)
        #     glBindTexture(GL_TEXTURE_2D, tex[2])
        # else:                                    
            # no texture request found for this object, disable textures
        glDisable(GL_TEXTURE_2D)
        
        glPushMatrix()
        # draw real bodies (boxes, spheres, ...)
        if body != None:
            # set color of object (currently dark gray)
            glColor3f(0.3, 0.3, 0.3)

            # transform (rotate, translate) body accordingly
            (x,y,z) = body.getPosition()
            R = body.getRotation()

            rot = [R[0], R[3], R[6], 0.0,
                   R[1], R[4], R[7], 0.0,
                   R[2], R[5], R[8], 0.0,
                      x,    y,    z, 1.0]
            glMultMatrixd(rot)

            # switch different geom objects
            if type(geom) == ode.GeomBox:
                # cube
                (sx, sy, sz) = geom.getLengths()
                glScaled(sx, sy, sz)
                glutSolidCube(1)
            elif type(geom) == ode.GeomSphere:
                # sphere
                radius = geom.getRadius()
                glutSolidSphere(radius, 20, 20)
                
            elif type(geom) == ode.GeomCCylinder:
                # capped cylinder
                radius = geom.getParams()[0]
                length = geom.getParams()[1] - 2*radius
                quad = gluNewQuadric()
                # draw cylinder and two spheres, one at each end
                glTranslate(0.0, 0.0, -length/2)
                gluCylinder(quad, radius, radius, length, 32, 32)
                glutSolidSphere(radius, 20, 20)
                glTranslate(0.0, 0.0, length)
                glutSolidSphere(radius, 20, 20)

            # FIXME: sometimes cylinder attribute is undefined; this has something to
            # to with the pyODE hack, but we don't know anymore where it came from...  X-(
            elif hasattr(ode, 'GeomCylinder'):
                if type(geom) == ode.GeomCylinder:
                    # solid cylinder
                    radius = geom.getParams()[0]
                    length = geom.getParams()[1]
                    glTranslate(0.0, 0.0, -length/2)
                    quad = gluNewQuadric()
                    gluDisk(quad, 0, radius, 32, 1)
                    quad = gluNewQuadric()
                    gluCylinder(quad, radius, radius, length, 32, 32)
                    glTranslate(0.0, 0.0, length)
                    quad = gluNewQuadric()
                    gluDisk(quad, 0, radius, 32, 1)                
            else:
                # TODO: add other geoms here
                pass

        else:
            # no body found, then it must be a plane (we only draw planes)
            if type(geom) == ode.GeomPlane:
                # set color of plane (currently green)
                if self.isFloorGreen:
                    glColor3f(0.2, 0.6, 0.3)
                else:
                    glColor3f(0.2, 0.3, 0.8)

                # for planes, we need a Quadric object
                quad = gluNewQuadric()
                gluQuadricTexture(quad, GL_TRUE)

                p = geom.getParams()[0] # the normal vector to the plane
                d = geom.getParams()[1] # the distance to the origin
                q = (0.0, 0.0, 1.0)     # the normal vector of default gluDisks (z=0 plane)

                # calculate the cross product to get the rotation axis
                c = crossproduct(p,q)
                # calculate the angle between default normal q and plane normal p
                theta = acos(dotproduct(p,q) / (norm(p)*norm(q))) / pi * 180

                # rotate the plane
                glPushMatrix()
                glTranslate(d*p[0], d*p[1], d*p[2])
                glRotate(-theta, c[0], c[1], c[2])
                gluDisk(quad, 0, 20, 20, 1)
                glPopMatrix()

        glPopMatrix()