1from __future__ import print_function
2
3__author__ = 'Frank Sehnke, sehnke@in.tum.de'
4
5#@PydevCodeAnalysisIgnore
6#########################################################################
7# OpenGL viewer for the FlexCube Environment
8#
9# The FlexCube Environment is a Mass-Spring-System composed of 8 mass points.
10# These resemble a cube with flexible edges.
11#
12# This viewer uses an UDP connection found in tools/networking/udpconnection.py
13#
14# The viewer recieves the position matrix of the 8 masspoints and the center of gravity.
15# With this information it renders a Glut based 3d visualization of teh FlexCube
16#
17# Options:
18# - serverIP: The ip of the server to which the viewer should connect
19# - ownIP: The IP of the computer running the viewer
20# - port: The starting port (2 adjacent ports will be used)
21#
22# Saving the images is possible by setting self.savePics=True.
23# Changing the point and angle of view is possible by using the mouse
24# while button 1 or 2 pressed.
25#
26# Requirements: OpenGL
27#
28#########################################################################
29
30from OpenGL.GLUT import *
31from OpenGL.GL import *
32from OpenGL.GLE import *
33from OpenGL.GLU import *
34from time import sleep
35from scipy import ones, array, cos, sin
36from pybrain.tools.networking.udpconnection import UDPClient
37
38class FlexCubeRenderer(object):
39    #Options: ServerIP(default:localhost), OwnIP(default:localhost), Port(default:21560)
40    def __init__(self, servIP="127.0.0.1", ownIP="127.0.0.1", port="21580"):
41        self.oldScreenValues = None
42        self.view = 0
43        self.worldRadius = 400
44
45        # Start of mousepointer
46        self.lastx = 0
47        self.lasty = 15
48        self.lastz = 300
49        self.zDis = 1
50
51        # Start of cube
52        self.cube = [0.0, 0.0, 0.0]
53        self.bmpCount = 0
54        self.actCount = 0
55        self.calcPhysics = 0
56        self.newPic = 1
57        self.picCount = 0
58        self.sensors = [0.0, 0.0, 0.0]
59        self.centerOfGrav = array([0.0, 5.0, 0.0])
60
61        self.savePics = False
62        self.drawCounter = 0
63        self.fps = 50
64        self.dt = 1.0 / float(self.fps)
65        self.step = 0
66
67        self.client = UDPClient(servIP, ownIP, port)
68
69    # If self.savePics=True this method saves the produced images
70    def saveTo(self, filename, format="JPEG"):
71        import Image # get PIL's functionality...
72        width, height = 800, 600
73        glPixelStorei(GL_PACK_ALIGNMENT, 1)
74        data = glReadPixels(0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE)
75        image = Image.fromstring("RGB", (width, height), data)
76        image = image.transpose(Image.FLIP_TOP_BOTTOM)
77        image.save(filename, format)
78        print(('Saved image to ', filename))
79        return image
80
81    # the render method containing the Glut mainloop
82    def _render(self):
83        # Call init: Parameter(Window Position -> x, y, height, width)
84        self.init_GL(self, 300, 300, 800, 600)
85        self.quad = gluNewQuadric()
86        glutMainLoop()
87
88    # The Glut idle function
89    def drawIdleScene(self):
90        #recive data from server and update the points of the cube
91        try: self.sensors = self.client.listen(self.sensors)
92        except: pass
93        if self.sensors == ["r", "r", "r"]: self.centerOfGrav = array([0.0, 5.0, 0.0])
94        else:
95            self.step += 1
96            a = self.sensors[0] / 360.0 * 3.1428
97            dir = array([cos(a), 0.0, -sin(a)])
98            self.centerOfGrav += self.sensors[2] * dir * 0.02
99            self.drawScene()
100        if self.savePics:
101            self.saveTo("./screenshots/image_jump" + repr(10000 + self.picCount) + ".jpg")
102            self.picCount += 1
103        else: sleep(self.dt)
104
105    def drawScene(self):
106        ''' This methode describes the complete scene.'''
107        # clear the buffer
108        if self.zDis < 10: self.zDis += 0.25
109        if self.lastz > 100: self.lastz -= self.zDis
110        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
111        glLoadIdentity()
112
113        # Point of view
114        glRotatef(self.lastx, 0.0, 1.0, 0.0)
115        glRotatef(self.lasty, 1.0, 0.0, 0.0)
116        #glRotatef(15, 0.0, 0.0, 1.0)
117        # direction of view is aimed to the center of gravity of the cube
118        glTranslatef(-self.centerOfGrav[0], -self.centerOfGrav[1] - 50.0, -self.centerOfGrav[2] - self.lastz)
119
120        #Objects
121
122        #Massstab
123        for lk in range(41):
124            if float(lk - 20) / 10.0 == (lk - 20) / 10:
125                glColor3f(0.75, 0.75, 0.75)
126                glPushMatrix()
127                glRotatef(90, 1, 0, 0)
128                glTranslate(self.worldRadius / 40.0 * float(lk) - self.worldRadius / 2.0, -40.0, -30)
129                quad = gluNewQuadric()
130                gluCylinder(quad, 2, 2, 60, 4, 1)
131                glPopMatrix()
132            else:
133                if float(lk - 20) / 5.0 == (lk - 20) / 5:
134                    glColor3f(0.75, 0.75, 0.75)
135                    glPushMatrix()
136                    glRotatef(90, 1, 0, 0)
137                    glTranslate(self.worldRadius / 40.0 * float(lk) - self.worldRadius / 2.0, -40.0, -15.0)
138                    quad = gluNewQuadric()
139                    gluCylinder(quad, 1, 1, 30, 4, 1)
140                    glPopMatrix()
141                else:
142                    glColor3f(0.75, 0.75, 0.75)
143                    glPushMatrix()
144                    glRotatef(90, 1, 0, 0)
145                    glTranslate(self.worldRadius / 40.0 * float(lk) - self.worldRadius / 2.0, -40.0, -7.5)
146                    quad = gluNewQuadric()
147                    gluCylinder(quad, 0.5, 0.5, 15, 4, 1)
148                    glPopMatrix()
149
150        # Floor
151        tile = self.worldRadius / 40.0
152        glEnable (GL_BLEND)
153        glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
154
155        glColor3f(0.8, 0.8, 0.5)
156        glPushMatrix()
157        glTranslatef(0.0, -3.0, 0.0)
158        glBegin(GL_QUADS)
159        glNormal(0.0, 1.0, 0.0)
160        glVertex3f(-self.worldRadius, 0.0, -self.worldRadius)
161        glVertex3f(-self.worldRadius, 0.0, self.worldRadius)
162        glVertex3f(self.worldRadius, 0.0, self.worldRadius)
163        glVertex3f(self.worldRadius, 0.0, -self.worldRadius)
164        glEnd()
165        glPopMatrix()
166
167        #Water
168        for xF in range(40):
169            for yF in range(40):
170                if float(xF + yF) / 2.0 == (xF + yF) / 2: glColor4f(0.7, 0.7, 1.0, 0.5)
171                else: glColor4f(0.9, 0.9, 1.0, 0.5)
172                glPushMatrix()
173                glTranslatef(0.0, -0.03, 0.0)
174                glBegin(GL_QUADS)
175                glNormal(0.5 + sin(float(xF) + float(self.step) / 4.0) * 0.5, 0.5 + cos(float(xF) + float(self.step) / 4.0) * 0.5, 0.0)
176                for i in range(2):
177                    for k in range(2):
178                        glVertex3f((i + xF - 20) * tile, sin(float(xF + i) + float(self.step) / 4.0) * 3.0, ((k ^ i) + yF - 20) * tile)
179                glEnd()
180                glPopMatrix()
181
182        self.ship()
183
184        # swap the buffer
185        glutSwapBuffers()
186
187    def ship(self):
188        glColor3f(0.4, 0.1, 0.2)
189        glPushMatrix()
190        glTranslate(self.centerOfGrav[0] + 14, self.centerOfGrav[1], self.centerOfGrav[2])
191        glRotatef(180 - self.sensors[0], 0.0, 1.0, 0.0)
192
193        self.cuboid(0, 0, 0, 20, 5, 5)
194        #bow of ship
195        glBegin(GL_TRIANGLES)
196        glNormal3fv(self.calcNormal(self.points2Vector([-5, 6, 2.5], [0, 5, 5]), self.points2Vector([-5, 6, 2.5], [0, 5, 0])))
197        glVertex3f(-5, 6, 2.5), glVertex3f(0, 5, 0), glVertex3f(0, 5, 5)
198        glNormal3fv(self.calcNormal(self.points2Vector([-5, 6, 2.5], [0, 0, 5]), self.points2Vector([-5, 6, 2.5], [0, 5, 5])))
199        glVertex3f(-5, 6, 2.5), glVertex3f(0, 0, 5), glVertex3f(0, 5, 5)
200        glNormal3fv(self.calcNormal(self.points2Vector([-5, 6, 2.5], [0, 0, 0]), self.points2Vector([-5, 6, 2.5], [0, 0, 5])))
201        glVertex3f(-5, 6, 2.5), glVertex3f(0, 0, 5), glVertex3f(0, 0, 0)
202        glNormal3fv(self.calcNormal(self.points2Vector([-5, 6, 2.5], [0, 5, 0]), self.points2Vector([-5, 6, 2.5], [0, 0, 0])))
203        glVertex3f(-5, 6, 2.5), glVertex3f(0, 0, 0), glVertex3f(0, 5, 0)
204        glEnd()
205        # stern
206        glPushMatrix()
207        glRotatef(-90, 1.0, 0.0, 0.0)
208        glTranslatef(15, -2.5, 0)
209        gluCylinder(self.quad, 2.5, 2.5, 5, 10, 1)
210        glTranslatef(0, 0, 5)
211        gluDisk(self.quad, 0, 2.5, 10, 1)
212        glPopMatrix()
213        # deck
214        if abs(self.sensors[0]) < 5.0: reward = (self.sensors[2] + 10.0) / 50.0
215        else: reward = 0.2
216        glColor3f(1.0 - reward, reward, 0)
217        self.cuboid(5, 5, 1, 10, 8, 4)
218        glPushMatrix()
219        glRotatef(-90, 1.0, 0.0, 0.0)
220        glTranslatef(13, -2.5, 5)
221        glColor3f(1, 1, 1)
222        gluCylinder(self.quad, 1, 0.8, 5, 20, 1)
223        glPopMatrix()
224        glPopMatrix()
225
226    def cuboid(self, x0, y0, z0, x1, y1, z1):
227        glBegin(GL_QUADS)
228        glNormal(0, 0, 1)
229        glVertex3f(x0, y0, z1); glVertex3f(x0, y1, z1); glVertex3f(x1, y1, z1); glVertex3f(x1, y0, z1)  #front
230        glNormal(-1, 0, 0)
231        glVertex3f(x0, y0, z0); glVertex3f(x0, y0, z1); glVertex3f(x0, y1, z1); glVertex3f(x0, y1, z0) # left
232        glNormal(0, -1, 0)
233        glVertex3f(x0, y0, z0); glVertex3f(x0, y0, z1); glVertex3f(x1, y0, z1); glVertex3f(x1, y0, z0) # bottom
234        glNormal(0, 0, -1)
235        glVertex3f(x0, y0, z0); glVertex3f(x1, y0, z0); glVertex3f(x1, y1, z0); glVertex3f(x0, y1, z0) # back
236        glNormal(0, 1, 0)
237        glVertex3f(x0, y1, z0); glVertex3f(x1, y1, z0); glVertex3f(x1, y1, z1); glVertex3f(x0, y1, z1) # top
238        glNormal(1, 0, 0)
239        glVertex3f(x1, y0, z0); glVertex3f(x1, y0, z1); glVertex3f(x1, y1, z1); glVertex3f(x1, y1, z0) # right
240        glEnd()
241
242    def calcNormal(self, xVector, yVector):
243        result = [0, 0, 0]
244        result[0] = xVector[1] * yVector[2] - yVector[1] * xVector[2]
245        result[1] = -xVector[0] * yVector[2] + yVector[0] * xVector[2]
246        result[2] = xVector[0] * yVector[1] - yVector[0] * xVector[1]
247        return [result[0], result[1], result[2]]
248
249    def points2Vector(self, startPoint, endPoint):
250        result = [0, 0, 0]
251        result[0] = endPoint[0] - startPoint[0]
252        result[1] = endPoint[1] - startPoint[1]
253        result[2] = endPoint[2] - startPoint[2]
254        return [result[0], result[1], result[2]]
255
256    def resizeScene(self, width, height):
257        '''Needed if window size changes.'''
258        if height == 0: # Prevent A Divide By Zero If The Window Is Too Small
259            height = 1
260
261        glViewport(0, 0, width, height) # Reset The Current Viewport And Perspective Transformation
262        glMatrixMode(GL_PROJECTION)
263        glLoadIdentity()
264        gluPerspective(45.0, float(width) / float(height), 0.1, 700.0)
265        glMatrixMode(GL_MODELVIEW)
266
267    def activeMouse(self, x, y):
268        #Returns mouse coordinates while any mouse button is pressed.
269        # store the mouse coordinate
270        if self.mouseButton == GLUT_LEFT_BUTTON:
271            self.lastx = x - self.xOffset
272            self.lasty = y - self.yOffset
273        if self.mouseButton == GLUT_RIGHT_BUTTON:
274            self.lastz = y - self.zOffset
275        # redisplay
276        glutPostRedisplay()
277
278    def passiveMouse(self, x, y):
279        '''Returns mouse coordinates while no mouse button is pressed.'''
280        pass
281
282    def completeMouse(self, button, state, x, y):
283        #Returns mouse coordinates and which button was pressed resp. released.
284        self.mouseButton = button
285        if state == GLUT_DOWN:
286            self.xOffset = x - self.lastx
287            self.yOffset = y - self.lasty
288            self.zOffset = y - self.lastz
289        # redisplay
290        glutPostRedisplay()
291
292    #Initialise an OpenGL windows with the origin at x, y and size of height, width.
293    def init_GL(self, pyWorld, x, y, height, width):
294        # initialize GLUT
295        glutInit([])
296
297        glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH)
298        glutInitWindowSize(height, width)
299        glutInitWindowPosition(x, y)
300        glutCreateWindow("The Curious Cube")
301        glClearDepth(1.0)
302        glEnable(GL_DEPTH_TEST)
303        glClearColor(0.0, 0.0, 0.0, 0.0)
304        glShadeModel(GL_SMOOTH)
305        glMatrixMode(GL_MODELVIEW)
306        # initialize lighting */
307        glLightfv(GL_LIGHT0, GL_DIFFUSE, [1, 1, 1, 1.0])
308        glLightModelfv(GL_LIGHT_MODEL_AMBIENT, [1.0, 1.0, 1.0, 1.0])
309        glEnable(GL_LIGHTING)
310        glEnable(GL_LIGHT0)
311        #
312        glColorMaterial(GL_FRONT, GL_DIFFUSE)
313        glEnable(GL_COLOR_MATERIAL)
314        # Automatic vector normalise
315        glEnable(GL_NORMALIZE)
316
317        ### Instantiate the virtual world ###
318        glutDisplayFunc(pyWorld.drawScene)
319        glutMotionFunc(pyWorld.activeMouse)
320        glutMouseFunc(pyWorld.completeMouse)
321        glutReshapeFunc(pyWorld.resizeScene)
322        glutIdleFunc(pyWorld.drawIdleScene)
323
324if __name__ == '__main__':
325    s = sys.argv[1:]
326    r = FlexCubeRenderer(*s)
327    r._render()
328
329