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