""" An example program to show a possible simplification of a laser scan mesh. """ import sys from collections import deque from math import pi, sin, cos, sqrt from random import random from OpenGL.GL import * from OpenGL.GLU import * from OpenGL.GLUT import * #----------------------------[ basic data structures ]-------------------------# class Vertex(object): def __init__(self, u, v): self.u, self.v = u, v def __str__(self): return "<%d,%d>"%(self.u, self.v) class Point3D(object): def __init__(self, x, y, z): self.x, self.y, self.z = x, y, z def __str__(self): return "<%f,%f,%f>" % (self.x, self.y, self.z) def __add__(self, p): return Point3D(self.x+p.x, self.y+p.y, self.z+p.z) def __sub__(self, p): return Point3D(self.x-p.x, self.y-p.y, self.z-p.z) def __div__(self, d): return Point3D(self.x/d, self.y/d, self.z/d) def cross(self, p): return Point3D( self.y * p.z - self.z * p.y, self.z * p.x - self.x * p.z, self.x * p.y - self.y * p.x ) def distanceFrom(self, p): dx, dy, dz = self.x - p.x, self.y - p.y, self.z - p.z return sqrt(dx*dx + dy*dy + dz*dz) class Triangle(object): def __init__(self, p1, p2, p3): self.p1, self.p2, self.p3 = p1, p2, p3 class SphereGridMesh(object): """Represents a spherical mesh based on what I assume to be something similar to the format in which the laser scans are taken.""" def __init__(self, points): """The given points should be a 2D array of Point3D objects so that the rows correspond to u (angle of rotation about the vertical axis) and the columns correspond to v (angle of deviation from the horizontal plane).""" self.points = points self.usteps = len(points) self.vsteps = len(points[0]) # change this if you have an actual matrix format instead of an array of arrays def getVertex(self, v): uind, vind = v.u % self.usteps, v.v return self.points[uind][vind] def iterTriangles(self): """Iterates over all of the triangles in this mesh.""" for uind1 in xrange(self.usteps): uind2 = (uind1+1) % self.usteps # modulo is so we wrap-around correctly for vind1 in xrange(self.vsteps-1): vind2 = vind1+1 # this dimension does not wrap around yield Triangle( self.points[uind1][vind1], self.points[uind2][vind1], self.points[uind2][vind2] ) yield Triangle( self.points[uind1][vind1], self.points[uind2][vind2], self.points[uind1][vind2] ) #-----------------------------[ mesh simplification ]--------------------------# class SphereTreeNode(object): def __init__(self, triangle, level, approximationError, *children): """ triangle.p3 is always opposite the hypotenuse. """ self.triangle = triangle self.approximationError = approximationError self.level = level if len(children) == 0: self.isLeaf = True children = [None]*2 else: self.isLeaf = False assert len(children) == 2 self.child1, self.child2 = children[0], children[1] def setChildren(self, child1, child2): assert child1 is not None assert child2 is not None self.child1, self.child2 = child1, child2 self.isLeaf = False class SphereTreeMesh(object): def __init__(self, *roots): self.roots = roots def iterTriangles(self): """Iterates over all of the triangles in this mesh. Unlike this method in the SphereGridMesh class, the method to do this is a bit more complicated and we have to traverse to the leaves in the tree rooted at self.root to do this.""" for root in self.roots: for tri in self.iterTriangles_helper(root): yield tri def iterTriangles_helper(self, node): if node.isLeaf: yield node.triangle else: for tri in self.iterTriangles_helper(node.child1): yield tri for tri in self.iterTriangles_helper(node.child2): yield tri def simplifyMesh(data, maxError=0.05, useConservativeError=True): """This is the function which actually does the work of simplifying the input points to generate a mesh. The algorithm used here is based on a binary triangle tree and is similar to that used in the ROAM terrain triangulation algorithm. Other algorithms are possible, and might well be more efficient -- I just haven't looked into it too deeply. If the parameter `useConservativeError' is True, then the approximation will be guaranteed to stay within `maxError' of the initial mesh, but will sometimes use more triangles than necessary. If `useConservativeError' is False then the approximation will always be worse then `maxError', but will often not be too bad and will use fewer triangles.""" # Note that in these methods p1, p2, p3 will be the positions of the # vertices of the triangle in anticlockwise order with p3 being opposite # the hypotenuse. v1, v2, v3 are in the same order, but correspond to # the 2D points in the underlying mesh data from which p1,p2,p3 are # generated. sibling1, sibling2, sibling3 are other mesh triangles, # numbered so that they are adjacent to the edge opposite v1,v2,v3 # respectively. # In Python you can define functions inside of other functions (which I # use to create a helper function here. Everything would still work if # you defined the functions in the `standard' way. def constructNode(v1, v2, v3, level=0): """Recursively builds a triangulation to approximate the `data' mesh with as few triangles as possible while ensuring that the deviation of this triangulation from the initial mesh is bounded by `maxError'""" triangle = Triangle(data.getVertex(v1), data.getVertex(v2), data.getVertex(v3)) if (abs(v1.u - v2.u) == 1) and (abs(v1.v - v2.v) == 1): # if we're at the fine mesh resolution, construct the nodes directly node = SphereTreeNode(triangle, level, 0.0) # triangle fits points exactly, so approximationError is 0.0 else: # bisect the hypotenuse vmid = Vertex(int(v1.u+v2.u)/2, int(v1.v+v2.v)/2) # integer division intentional # construct the child nodes c1 = constructNode(v3, v1, vmid, level+1) c2 = constructNode(v2, v3, vmid, level+1) # calculate the error of merging the two children pmid = (triangle.p1 + triangle.p2) / 2.0 # calculate the approximation error estimate of the parent from # the estimates for the children if useConservativeError: approximationError = pmid.distanceFrom(data.getVertex(vmid)) + max( c1.approximationError, c2.approximationError ) else: approximationError = max( pmid.distanceFrom(data.getVertex(vmid)), c1.approximationError, c2.approximationError ) # merge the child nodes if the error in doing so is low enough if approximationError < maxError: node = SphereTreeNode(triangle, level, approximationError) else: node = SphereTreeNode(triangle, level, approximationError, c1, c2) c1.parent, c2.parent = node, node node.v1, node.v2, node.v3 = v1, v2, v3 return node def balanceTree(*roots): """Ensures that the binary triangle tree tree rooted at the given list of root nodes (which should be all of the same level) is balanced and thus free of any T-junctions""" Q = deque() Q.extend(roots) while len(Q) > 0: node = Q.popleft() assert hasattr(node, 'sibling1') assert hasattr(node, 'sibling2') assert hasattr(node, 'sibling3') # set the sibling pointers for this node's children if not node.isLeaf: # get the siblings of this node so we can work with them more easily sibling1, sibling2, sibling3 = node.sibling1, node.sibling2, node.sibling3 if not leafTest(sibling1): assert sibling1.level == node.level if not leafTest(sibling2): assert sibling2.level == node.level if not leafTest(sibling3): assert sibling3.level == node.level #splitNode(node.sibling3) splitNode(node) if node.sibling3 is not None: assert not node.sibling3.isLeaf setChildSiblings(node) # insert the children into the queue for later processing Q.append(node.child1) Q.append(node.child2) def setChildSiblings(node): """Sets the sibling pointers for both of the children of this node. This is straightforward, but takes a bit of care to avoid making a mistake somewhere.""" sibling1, sibling2, sibling3 = node.sibling1, node.sibling2, node.sibling3 # determine which edge these siblings attach to this node through if sibling1 is not None: if sibling1.sibling1 == node: isSibling1of1 = True elif sibling1.sibling2 == node: isSibling1of1 = False else: raise RuntimeError, "this is a bug" # just in case else: isSibling1of1 = True # value doesn't actually matter in this case if sibling2 is not None: if sibling2.sibling1 == node: isSibling1of2 = True elif sibling2.sibling2 == node: isSibling1of2 = False else: raise RuntimeError, "this is a bug" # just in case else: isSibling1of2 = True # value doesn't actually matter in this case # determine what the childeren's siblings are if not leafTest(sibling1): if isSibling1of1: node.child2.sibling3 = sibling1.child2 sibling1.child2.sibling3 = node.child2 else: node.child2.sibling3 = sibling1.child1 sibling1.child1.sibling3 = node.child2 else: node.child2.sibling3 = sibling1 if not leafTest(sibling2): if isSibling1of2: node.child1.sibling3 = sibling2.child2 sibling2.child2.sibling3 = node.child1 else: node.child1.sibling3 = sibling2.child1 sibling2.child1.sibling3 = node.child1 else: node.child1.sibling3 = sibling2 if sibling3 is not None: assert not sibling3.isLeaf node.child1.sibling1 = sibling3.child2 sibling3.child2.sibling2 = node.child1 node.child2.sibling2 = sibling3.child1 sibling3.child1.sibling1 = node.child2 else: node.child1.sibling1 = None node.child2.sibling2 = None node.child1.sibling2 = node.child2 node.child2.sibling1 = node.child1 def splitNode(node): """Ensures that the given node is not a leaf node by adding two new children to it if necessary. This method also ensures that any new T-junctions created by this split are recursively removed.""" if node is not None: if node.isLeaf: v1, v2, v3 = node.v1, node.v2, node.v3 # bisect the hypotenuse vmid = Vertex(int(v1.u+v2.u)/2, int(v1.v+v2.v)/2) # integer division intentional # construct the child nodes triangle1 = Triangle(data.getVertex(v3), data.getVertex(v1), data.getVertex(vmid)) triangle2 = Triangle(data.getVertex(v2), data.getVertex(v3), data.getVertex(vmid)) node.child1 = SphereTreeNode(triangle1, node.level+1, node.approximationError) node.child2 = SphereTreeNode(triangle2, node.level+1, node.approximationError) node.isLeaf = False node.child1.v1, node.child1.v2, node.child1.v3 = v3, v1, vmid node.child2.v1, node.child2.v2, node.child2.v3 = v2, v3, vmid node.child1.parent, node.child2.parent = node, node # we now may need to recursively split this node's sibling3 sibling3 = node.sibling3 if (sibling3 is not None) and sibling3.isLeaf: if sibling3.level != node.level: assert node.level == sibling3.level+1 splitNode(sibling3) if sibling3.sibling1 == node.parent: splitNode(sibling3.child2) elif sibling3.sibling2 == node.parent: splitNode(sibling3.child1) else: raise RuntimeError, "this is a bug" else: splitNode(sibling3) setChildSiblings(node) def leafTest(node): return (node is None) or (node.isLeaf) v1 = Vertex(0, 0) v2 = Vertex(data.usteps, data.vsteps-1) # usteps doesn't have a -1 because the mesh wraps around in that dimenison v3 = Vertex(0, data.vsteps-1) v4 = Vertex(data.usteps, 0) root1 = constructNode(v1, v2, v3) root2 = constructNode(v2, v1, v4) root1.sibling1, root1.sibling2, root1.sibling3 = None, root2, root2 root2.sibling1, root2.sibling2, root2.sibling3 = None, root1, root1 balanceTree(root1, root2) return SphereTreeMesh(root1,root2) #---------------------------[ graphics and interaction ]-----------------------# class Interface(object): """A simple object-oriented wrapper for the standard OpenGL callbacks.""" def __init__(self, usteps, vsteps): self.usteps, self.vsteps = usteps, vsteps self.data = self.createData1() self.simplifiedData = None # None is used as a marker value here self.drawFilled = False self.drawSimplified = False self.useConservativeError = True # stuff for enabling mouse rotation self.__isLeftDragging = False self.__theta = 0.0 self.__phi = 0.0 def displayFunc(self): glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) self.setViewMatrix() if self.drawFilled: glEnable(GL_LIGHTING) glDisable(GL_CULL_FACE) glPolygonMode(GL_FRONT_AND_BACK, GL_FILL) glDepthRange(0.01, 1.0) glColor3d(1,1,1) self.drawMesh(self.data) glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) glDepthRange(0.0, 0.99) glColor4d(0,0,0,0.1) self.drawMesh(self.data) else: glDisable(GL_LIGHTING) glEnable(GL_CULL_FACE) glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) glCullFace(GL_BACK) glColor3d(0,0,0) self.drawMesh(self.data) glCullFace(GL_FRONT) glColor3d(0.7, 0.7, 0.7) self.drawMesh(self.data) glFlush() glutSwapBuffers() def setViewMatrix(self): glLoadIdentity() glRotated(self.__phi, 1, 0, 0) glRotated(self.__theta, 0, 1, 0) def drawMesh(self, data): if self.drawSimplified: if self.simplifiedData is None: # call the function to compute the mesh simplification self.simplifiedData = simplifyMesh(self.data, useConservativeError=self.useConservativeError) iter = self.simplifiedData.iterTriangles() else: iter = data.iterTriangles() glBegin(GL_TRIANGLES) for tri in iter: normal = (tri.p2-tri.p1).cross(tri.p3-tri.p1) glNormal3f(normal.x, normal.y, normal.z) glVertex3d(tri.p1.x, tri.p1.y, tri.p1.z) glVertex3d(tri.p2.x, tri.p2.y, tri.p2.z) glVertex3d(tri.p3.x, tri.p3.y, tri.p3.z) glEnd() def createData1(self): return makeScanData(f_sphere, self.usteps, self.vsteps) def createData2(self): return makeScanData(f_bumpySphere1, self.usteps, self.vsteps) def createData3(self): return makeScanData(f_bumpySphere2, self.usteps, self.vsteps) def createData4(self): return makeScanData(f_tube, self.usteps, self.vsteps) def addNoise(self, amount): for uind in xrange(self.data.usteps): for vind in xrange(self.data.vsteps): self.data.points[uind][vind].x += amount*(2*random()-1) self.data.points[uind][vind].y += amount*(2*random()-1) self.data.points[uind][vind].z += amount*(2*random()-1) def keyFunc(self, key, x, y): if key == '1': self.data = self.createData1() self.simplifiedData = None self.redraw() elif key == '2': self.data = self.createData2() self.simplifiedData = None self.redraw() elif key == '3': self.data = self.createData3() self.simplifiedData = None self.redraw() elif key == '4': self.data = self.createData4() self.simplifiedData = None self.redraw() elif key == 'n': self.addNoise(0.005) self.simplifiedData = None self.redraw() elif key == 'e': self.useConservativeError = not self.useConservativeError self.simplifiedData = None self.redraw() elif key == 'w': self.drawFilled = not self.drawFilled self.redraw(); elif key == 's': self.drawSimplified = not self.drawSimplified self.redraw() elif key in ['?', 'h', 'H']: print "1 - select data set #1 (sphere)" print "2 - select data set #2 (bumpy sphere)" print "3 - select data set #3 (sphere with non-unifom bumps)" print "4 - select data set #4 (cylinder)" print "n - add a bit of noise to the data set" print "e - toggle between conservative and liberal error estimates" print "w - toggle filled/wireframe drawing" print "s - toggle drawing of simplified mesh" print "?/h/H - display this help" print "q/Q - quit" elif key in ['q', 'Q']: print "exiting" sys.exit(0) def mouseFunc(self, button, state, x, y): if button == GLUT_LEFT_BUTTON: if(state == GLUT_DOWN): # begin interactive dragging self.__mousex = x self.__mousey = y self.__isLeftDragging = True elif(state == GLUT_UP): # finalize new viewpoint self.__isLeftDragging = False def mouseMoveFunc(self, x, y): update = False if self.__isLeftDragging: self.__theta += x - self.__mousex self.__phi += y - self.__mousey self.__theta = self.__theta % 360.0 self.__phi = self.__phi % 360.0 update = True self.__mousex = x self.__mousey = y if update: self.redraw() def resizeFunc(self, w, h): pass def idleFunc(self): pass def redraw(self): glutPostRedisplay() def startInterface(interface): """A utility function to do some OpenGL-related setup and then launch a window to show the given interface. This function calls glutMainLoop, so it never returns.""" glutInit([]) glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH) glutInitWindowSize(500, 500) glutCreateWindow("openGL interface") glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glEnable(GL_DEPTH_TEST) glDepthFunc(GL_LEQUAL) glShadeModel(GL_SMOOTH) # set background to white glClearColor(1.0, 1.0, 1.0, 0.0) # set lights glEnable(GL_NORMALIZE) glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE) glEnable(GL_COLOR_MATERIAL) l0_pos = [ 30.0, 30.0, 50.0, 0.0] diffuse = [ 0.7, 0.7, 0.7, 1.0] ambient = [ 0.1, 0.1, 0.1, 1.0] specular = [ 0.0, 0.0, 0.0, 1.0] glEnable(GL_LIGHT0); glLightfv(GL_LIGHT0, GL_POSITION, l0_pos) glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse) glLightfv(GL_LIGHT0, GL_AMBIENT, ambient) glLightfv(GL_LIGHT0, GL_SPECULAR, specular) # set viewing area glMatrixMode(GL_PROJECTION) glLoadIdentity() glOrtho(-1.5, 1.5, -1.5, 1.5, -1.5, 1.5) glMatrixMode(GL_MODELVIEW) # set the callback functions glutDisplayFunc(interface.displayFunc) glutKeyboardFunc(interface.keyFunc) glutMouseFunc(interface.mouseFunc) glutMotionFunc(interface.mouseMoveFunc) glutIdleFunc(interface.idleFunc) # Start the GLUT main loop -- this function should never return glutMainLoop() #-------------------------------[ example data ]-------------------------------# def f_sphere(u,v): return Point3D( cos(u)*sin(v), cos(v), # y is the vertical axis in this code sin(u)*sin(v) ) def f_bumpySphere1(u,v): r = sin(6*u)*sin(6*v) r = 0.1*r + 1 return Point3D( r*cos(u)*sin(v), r*cos(v), r*sin(u)*sin(v) ) def f_bumpySphere2(u,v): r = sin(8*sin(u)+8*u) * sin(8*sin(v-pi/2)+8*v) r = 0.1*r + 1 return Point3D( r*cos(u)*sin(v), r*cos(v), r*sin(u)*sin(v) ) def f_tube(u,v): r = 0.5 / sin(v) return Point3D( r*cos(u)*sin(v), r*cos(v), r*sin(u)*sin(v) ) def makeScanData(fcn, usteps, vsteps): """Takes a function (u,v) -> Point3D and generates some fake scan data for it. I represent this scan data as an array-of-arrays, which is easy but you probably have you own (more efficient) representation.""" assert usteps > 1 assert vsteps > 1 points = [None]*usteps for uind in xrange(usteps): points[uind] = [None]*vsteps u = (2*pi*uind)/usteps for vind in xrange(vsteps): v = pi*(0.8*vind/float(vsteps-1) + 0.1) points[uind][vind] = fcn(u,v) return SphereGridMesh(points) if __name__ == "__main__": """If this Python file is called with "python spheremesh.py" then this code will be executed and a window showing some test data will pop up.""" interface = Interface(128, 1+128) # must be 2^n by 1+2^n in this code startInterface(interface)