def draw_xformed(xform,localDrawFunc):
"""Draws something given a se3 transformation and a drawing function
that draws the object in its local frame.
E.g., draw_xformed(xform,lambda:gldraw.box([ax,ay,az],[bx,by,bz])) draws
a box oriented and translated by xform."""
mat = zip(*se3.homogeneous(xform))
mat = sum([list(coli) for coli in mat],[])
glPushMatrix()
glMultMatrixf(mat)
localDrawFunc()
glPopMatrix()
def display_screen(self):
glDisable(GL_LIGHTING)
glColor3f(1, 1, 1)
# Projection
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective (self.fov,float(self.width)/float(self.height),self.clippingplanes[0],self.clippingplanes[1])
# Initialize ModelView matrix
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
# View transformation
mat = se3.homogeneous(self.camera.matrix())
cols = zip(*mat)
pack = sum((list(c) for c in cols),[])
glMultMatrixf(pack)
labels = dict([ (k, T[1]) for (k, T) in vantage_point_xforms.items() ])
for (k, v) in labels.items():
try:
labels[k] = gluProject(*v)
except:
labels[k] = None
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glOrtho(0,self.width,self.height,0,-1,1);
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
for (s, p) in labels.items():
if p:
glRasterPos3f(p[0], self.height - p[1], p[2])
gldraw.glutBitmapString(GLUT_BITMAP_HELVETICA_12, s)
