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()
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
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()
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()