Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Newer
Older
100644 882 lines (772 sloc) 31.501 kb
a46ed7a4 »
2012-03-15 ad
1 #!/usr/bin/python
2
3 # This file is part of the Printrun suite.
4 #
5 # Printrun is free software: you can redistribute it and/or modify
6 # it under the terms of the GNU General Public License as published by
7 # the Free Software Foundation, either version 3 of the License, or
8 # (at your option) any later version.
9 #
10 # Printrun is distributed in the hope that it will be useful,
11 # but WITHOUT ANY WARRANTY; without even the implied warranty of
12 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 # GNU General Public License for more details.
14 #
15 # You should have received a copy of the GNU General Public License
16 # along with Printrun. If not, see <http://www.gnu.org/licenses/>.
17
18 import os
19 import math
20 import stltool
21 import wx
22 from wx import glcanvas
23 import time
24 import threading
25
26 import pyglet
27 pyglet.options['shadow_window'] = False
28 pyglet.options['debug_gl'] = False
29 from pyglet.gl import *
30
31
32 class GLPanel(wx.Panel):
33 '''A simple class for using OpenGL with wxPython.'''
34
35 def __init__(self, parent, id, pos=wx.DefaultPosition,
36 size=wx.DefaultSize, style=0):
37 # Forcing a no full repaint to stop flickering
38 style = style | wx.NO_FULL_REPAINT_ON_RESIZE
39 #call super function
40 super(GLPanel, self).__init__(parent, id, pos, size, style)
41
42 #init gl canvas data
43 self.GLinitialized = False
44 attribList = (glcanvas.WX_GL_RGBA, # RGBA
45 glcanvas.WX_GL_DOUBLEBUFFER, # Double Buffered
46 glcanvas.WX_GL_DEPTH_SIZE, 24) # 24 bit
47 # Create the canvas
48 self.sizer = wx.BoxSizer(wx.HORIZONTAL)
49 self.canvas = glcanvas.GLCanvas(self, attribList=attribList)
50 self.sizer.Add(self.canvas, 1, wx.EXPAND)
51 self.SetSizer(self.sizer)
52 #self.sizer.Fit(self)
53 self.Layout()
54
55 # bind events
56 self.canvas.Bind(wx.EVT_ERASE_BACKGROUND, self.processEraseBackgroundEvent)
57 self.canvas.Bind(wx.EVT_SIZE, self.processSizeEvent)
58 self.canvas.Bind(wx.EVT_PAINT, self.processPaintEvent)
59
60 #==========================================================================
61 # Canvas Proxy Methods
62 #==========================================================================
63 def GetGLExtents(self):
64 '''Get the extents of the OpenGL canvas.'''
65 return self.canvas.GetClientSize()
66
67 def SwapBuffers(self):
68 '''Swap the OpenGL buffers.'''
69 self.canvas.SwapBuffers()
70
71 #==========================================================================
72 # wxPython Window Handlers
73 #==========================================================================
74 def processEraseBackgroundEvent(self, event):
75 '''Process the erase background event.'''
76 pass # Do nothing, to avoid flashing on MSWin
77
78 def processSizeEvent(self, event):
79 '''Process the resize event.'''
80 if self.canvas.GetContext():
81 # Make sure the frame is shown before calling SetCurrent.
82 self.Show()
83 self.canvas.SetCurrent()
84 size = self.GetGLExtents()
85 self.winsize = (size.width, size.height)
86 self.width, self.height = size.width, size.height
87 self.OnReshape(size.width, size.height)
88 self.canvas.Refresh(False)
89 event.Skip()
90
91 def processPaintEvent(self, event):
92 '''Process the drawing event.'''
93 self.canvas.SetCurrent()
94
95 # This is a 'perfect' time to initialize OpenGL ... only if we need to
96 if not self.GLinitialized:
97 self.OnInitGL()
98 self.GLinitialized = True
99
100 self.OnDraw()
101 event.Skip()
102
103 def Destroy(self):
104 #clean up the pyglet OpenGL context
105 #self.pygletcontext.destroy()
106 #call the super method
107 super(wx.Panel, self).Destroy()
108
109 #==========================================================================
110 # GLFrame OpenGL Event Handlers
111 #==========================================================================
112 def OnInitGL(self):
113 '''Initialize OpenGL for use in the window.'''
114 #create a pyglet context for this panel
115 self.pmat = (GLdouble * 16)()
116 self.mvmat = (GLdouble * 16)()
117 self.pygletcontext = Context(current_context)
118 self.pygletcontext.set_current()
119 self.dist = 1000
120 self.vpmat = None
121 #normal gl init
122 glClearColor(0, 0, 0, 1)
123 glColor3f(1, 0, 0)
124 glEnable(GL_DEPTH_TEST)
125 glEnable(GL_CULL_FACE)
126 # Uncomment this line for a wireframe view
127 #glPolygonMode(GL_FRONT_AND_BACK, GL_LINE)
128
129 # Simple light setup. On Windows GL_LIGHT0 is enabled by default,
130 # but this is not the case on Linux or Mac, so remember to always
131 # include it.
132 glEnable(GL_LIGHTING)
133 glEnable(GL_LIGHT0)
134 glEnable(GL_LIGHT1)
135
136 # Define a simple function to create ctypes arrays of floats:
137 def vec(*args):
138 return (GLfloat * len(args))(*args)
139
140 glLightfv(GL_LIGHT0, GL_POSITION, vec(.5, .5, 1, 0))
141 glLightfv(GL_LIGHT0, GL_SPECULAR, vec(.5, .5, 1, 1))
142 glLightfv(GL_LIGHT0, GL_DIFFUSE, vec(1, 1, 1, 1))
143 glLightfv(GL_LIGHT1, GL_POSITION, vec(1, 0, .5, 0))
144 glLightfv(GL_LIGHT1, GL_DIFFUSE, vec(.5, .5, .5, 1))
145 glLightfv(GL_LIGHT1, GL_SPECULAR, vec(1, 1, 1, 1))
146
147 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(0.5, 0, 0.3, 1))
148 glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, vec(1, 1, 1, 1))
149 glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 50)
150 glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, vec(0, 0.1, 0, 0.9))
151 #create objects to draw
152 #self.create_objects()
153
154 def OnReshape(self, width, height):
155 '''Reshape the OpenGL viewport based on the dimensions of the window.'''
156
157 if not self.GLinitialized:
158 self.OnInitGL()
159 self.GLinitialized = True
160 self.pmat = (GLdouble * 16)()
161 self.mvmat = (GLdouble * 16)()
162 glViewport(0, 0, width, height)
163 glMatrixMode(GL_PROJECTION)
164 glLoadIdentity()
165 gluPerspective(60., width / float(height), .1, 1000.)
166 glMatrixMode(GL_MODELVIEW)
167 glLoadIdentity()
168 #pyglet stuff
169 self.vpmat = (GLint * 4)(0, 0, *list(self.GetClientSize()))
170 glGetDoublev(GL_PROJECTION_MATRIX, self.pmat)
171 glGetDoublev(GL_MODELVIEW_MATRIX, self.mvmat)
172 #glMatrixMode(GL_PROJECTION)
173
174 # Wrap text to the width of the window
175 if self.GLinitialized:
176 self.pygletcontext.set_current()
177 self.update_object_resize()
178
179 def OnDraw(self, *args, **kwargs):
180 """Draw the window."""
181 #clear the context
182 self.canvas.SetCurrent()
183 self.pygletcontext.set_current()
184 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
185 #draw objects
186 self.draw_objects()
187 #update screen
188 self.SwapBuffers()
189
190 #==========================================================================
191 # To be implemented by a sub class
192 #==========================================================================
193 def create_objects(self):
194 '''create opengl objects when opengl is initialized'''
195 pass
196
197 def update_object_resize(self):
198 '''called when the window recieves only if opengl is initialized'''
199 pass
200
201 def draw_objects(self):
202 '''called in the middle of ondraw after the buffer has been cleared'''
203 pass
204
205
206 class stlview(object):
207 def __init__(self, facets, batch):
208 # Create the vertex and normal arrays.
209 vertices = []
210 normals = []
211
212 for i in facets:
213 for j in i[1]:
214 vertices.extend(j)
215 normals.extend(i[0])
216
217 # Create a list of triangle indices.
218 indices = range(3 * len(facets)) # [[3*i,3*i+1,3*i+2] for i in xrange(len(facets))]
219 #print indices[:10]
220 self.vertex_list = batch.add_indexed(len(vertices) // 3,
221 GL_TRIANGLES,
222 None, # group,
223 indices,
224 ('v3f/static', vertices),
225 ('n3f/static', normals))
226
227 def delete(self):
228 self.vertex_list.delete()
229
230
231 def vdiff(v, o):
232 return [x[0] - x[1] for x in zip(v, o)]
233
234
235 class gcview(object):
236 def __init__(self, lines, batch, w=0.5, h=0.5):
237 # Create the vertex and normal arrays.
238 vertices = []
239 normals = []
240 self.prev = [0.001, 0.001, 0.001, 0.001]
241 self.fline = 1
242 self.vlists = []
243 self.layers = {}
244 t0 = time.time()
245 lines = [self.transform(i) for i in lines]
246 lines = [i for i in lines if i is not None]
247 print "transformed lines in %fs" % (time.time() - t0)
248 t0 = time.time()
249 layertemp = {}
250 lasth = None
251 counter = 0
252 if len(lines) == 0:
253 return
254 for i in lines:
255 counter += 1
256 if i[0][2] not in layertemp:
257 layertemp[i[0][2]] = [[], []]
258 if lasth is not None:
259 self.layers[lasth] = pyglet.graphics.Batch()
260 lt = layertemp[lasth][0]
261 indices = range(len(layertemp[lasth][0]) // 3) # [[3*i,3*i+1,3*i+2] for i in xrange(len(facets))]
262 self.vlists.append(self.layers[lasth].add_indexed(len(layertemp[lasth][0]) // 3,
263 GL_TRIANGLES,
264 None, # group,
265 indices,
266 ('v3f/static', layertemp[lasth][0]),
267 ('n3f/static', layertemp[lasth][1])))
268
269 lasth = i[0][2]
270
271 spoints, epoints, S, E = self.genline(i, h, w)
272
273 verticestoadd = [[
274 spoints[(j + 1) % 8],
275 epoints[(j) % 8],
276 spoints[j],
277 epoints[j],
278 spoints[(j + 1) % 8],
279 epoints[(j + 1) % 8]
280 ] for j in xrange(8)]
281 normalstoadd = [map(vdiff, v, [S, E, S, E, S, E]) for v in verticestoadd]
282 v1 = []
283 map(v1.extend, verticestoadd)
284 v2 = []
285 map(v2.extend, v1)
286 n1 = []
287 map(n1.extend, normalstoadd)
288 n2 = []
289 map(n2.extend, n1)
290
291 layertemp[i[0][2]][0] += v2
292 vertices += v2
293 layertemp[i[0][2]][1] += n2
294 normals += n2
295 print "appended lines in %fs" % (time.time() - t0)
296 t0 = time.time()
297
298 # Create a list of triangle indices.
299 indices = range(3 * 16 * len(lines)) # [[3*i,3*i+1,3*i+2] for i in xrange(len(facets))]
300 self.vlists.append(batch.add_indexed(len(vertices) // 3,
301 GL_TRIANGLES,
302 None, # group,
303 indices,
304 ('v3f/static', vertices),
305 ('n3f/static', normals)))
306 if lasth is not None:
307 self.layers[lasth] = pyglet.graphics.Batch()
308 indices = range(len(layertemp[lasth][0])) # [[3*i,3*i+1,3*i+2] for i in xrange(len(facets))]
309 self.vlists.append(self.layers[lasth].add_indexed(len(layertemp[lasth][0]) // 3,
310 GL_TRIANGLES,
311 None, # group,
312 indices,
313 ('v3f/static', layertemp[lasth][0]),
314 ('n3f/static', layertemp[lasth][1])))
315
316 def genline(self, i, h, w):
317 S = i[0][:3]
318 E = i[1][:3]
319 v = map(lambda x, y: x - y, E, S)
320 vlen = math.sqrt(float(sum(map(lambda a: a * a, v[:3]))))
321
322 if vlen == 0:
323 vlen = 0.01
324 sq2 = math.sqrt(2.0) / 2.0
325 htw = float(h) / w
326 d = w / 2.0
327 if i[1][3] == i[0][3]:
328 d = 0.05
329 points = [[d, 0, 0],
330 [sq2 * d, sq2 * d, 0],
331 [0, d, 0],
332 [-sq2 * d, sq2 * d, 0],
333 [-d, 0, 0],
334 [-sq2 * d, -sq2 * d, 0],
335 [0, -d, 0],
336 [sq2 * d, -sq2 * d, 0]
337 ]
338 axis = stltool.cross([0, 0, 1], v)
339 alen = math.sqrt(float(sum(map(lambda a: a * a, v[:3]))))
340 if alen > 0:
341 axis = map(lambda m: m / alen, axis)
342 angle = math.acos(v[2] / vlen)
343
344 def vrot(v, axis, angle):
345 kxv = stltool.cross(axis, v)
346 kdv = sum(map(lambda x, y: x * y, axis, v))
347 return map(lambda x, y, z: x * math.cos(angle) + y * math.sin(angle) + z * kdv * (1.0 - math.cos(angle)), v, kxv, axis)
348
349 points = map(lambda x: vrot(x, axis, angle), points)
350 points = map(lambda x: [x[0], x[1], htw * x[2]], points)
351
352 def vadd(v, o):
353 return map(sum, zip(v, o))
354 spoints = map(lambda x: vadd(S, x), points)
355 epoints = map(lambda x: vadd(E, x), points)
356 return spoints, epoints, S, E
357
358 def transform(self, line):
359 line = line.split(";")[0]
360 cur = self.prev[:]
361 if len(line) > 0:
362 if "G1" in line or "G0" in line or "G92" in line:
363 if("X" in line):
364 cur[0] = float(line.split("X")[1].split(" ")[0])
365 if("Y" in line):
366 cur[1] = float(line.split("Y")[1].split(" ")[0])
367 if("Z" in line):
368 cur[2] = float(line.split("Z")[1].split(" ")[0])
369 if("E" in line):
370 cur[3] = float(line.split("E")[1].split(" ")[0])
371 if self.prev == cur:
372 return None
373 if self.fline or "G92" in line:
374 self.prev = cur
375 self.fline = 0
376 return None
377 else:
378 r = [self.prev, cur]
379 self.prev = cur
380 return r
381
382 def delete(self):
383 for i in self.vlists:
384 i.delete()
385 self.vlists = []
386
387
388 def trackball(p1x, p1y, p2x, p2y, r):
389 TRACKBALLSIZE = r
390 #float a[3]; /* Axis of rotation */
391 #float phi; /* how much to rotate about axis */
392 #float p1[3], p2[3], d[3];
393 #float t;
394
395 if (p1x == p2x and p1y == p2y):
396 return [0.0, 0.0, 0.0, 1.0]
397
398 p1 = [p1x, p1y, project_to_sphere(TRACKBALLSIZE, p1x, p1y)]
399 p2 = [p2x, p2y, project_to_sphere(TRACKBALLSIZE, p2x, p2y)]
400 a = stltool.cross(p2, p1)
401
402 d = map(lambda x, y: x - y, p1, p2)
403 t = math.sqrt(sum(map(lambda x: x * x, d))) / (2.0 * TRACKBALLSIZE)
404
405 if (t > 1.0):
406 t = 1.0
407 if (t < -1.0):
408 t = -1.0
409 phi = 2.0 * math.asin(t)
410
411 return axis_to_quat(a, phi)
412
413
414 def vec(*args):
415 return (GLfloat * len(args))(*args)
416
417
418 def axis_to_quat(a, phi):
419 #print a, phi
420 lena = math.sqrt(sum(map(lambda x: x * x, a)))
421 q = map(lambda x: x * (1 / lena), a)
422 q = map(lambda x: x * math.sin(phi / 2.0), q)
423 q.append(math.cos(phi / 2.0))
424 return q
425
426
427 def build_rotmatrix(q):
428 m = (GLdouble * 16)()
429 m[0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
430 m[1] = 2.0 * (q[0] * q[1] - q[2] * q[3])
431 m[2] = 2.0 * (q[2] * q[0] + q[1] * q[3])
432 m[3] = 0.0
433
434 m[4] = 2.0 * (q[0] * q[1] + q[2] * q[3])
435 m[5] = 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0])
436 m[6] = 2.0 * (q[1] * q[2] - q[0] * q[3])
437 m[7] = 0.0
438
439 m[8] = 2.0 * (q[2] * q[0] - q[1] * q[3])
440 m[9] = 2.0 * (q[1] * q[2] + q[0] * q[3])
441 m[10] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0])
442 m[11] = 0.0
443
444 m[12] = 0.0
445 m[13] = 0.0
446 m[14] = 0.0
447 m[15] = 1.0
448 return m
449
450
451 def project_to_sphere(r, x, y):
452 d = math.sqrt(x * x + y * y)
453 if (d < r * 0.70710678118654752440):
454 return math.sqrt(r * r - d * d)
455 else:
456 t = r / 1.41421356237309504880
457 return t * t / d
458
459
460 def mulquat(q1, rq):
461 return [q1[3] * rq[0] + q1[0] * rq[3] + q1[1] * rq[2] - q1[2] * rq[1],
462 q1[3] * rq[1] + q1[1] * rq[3] + q1[2] * rq[0] - q1[0] * rq[2],
463 q1[3] * rq[2] + q1[2] * rq[3] + q1[0] * rq[1] - q1[1] * rq[0],
464 q1[3] * rq[3] - q1[0] * rq[0] - q1[1] * rq[1] - q1[2] * rq[2]]
465
466
467 class TestGlPanel(GLPanel):
468
469 def __init__(self, parent, size, id=wx.ID_ANY):
470 super(TestGlPanel, self).__init__(parent, id, wx.DefaultPosition, size, 0)
471 self.batches = []
472 self.rot = 0
473 self.canvas.Bind(wx.EVT_MOUSE_EVENTS, self.move)
474 self.canvas.Bind(wx.EVT_LEFT_DCLICK, self.double)
475 self.initialized = 1
476 self.canvas.Bind(wx.EVT_MOUSEWHEEL, self.wheel)
477 self.parent = parent
478 self.initpos = None
479 self.dist = 200
480 self.bedsize = [200, 200]
481 self.transv = [0, 0, -self.dist]
482 self.basequat = [0, 0, 0, 1]
483 wx.CallAfter(self.forceresize)
484 self.mousepos = [0, 0]
485
486 def double(self, event):
487 p = event.GetPositionTuple()
488 sz = self.GetClientSize()
489 v = map(lambda m, w, b: b * m / w, p, sz, self.bedsize)
490 v[1] = self.bedsize[1] - v[1]
491 v += [300]
492 print v
493 self.add_file("../prusa/metric-prusa/x-end-idler.stl", v)
494
495 def forceresize(self):
496 self.SetClientSize((self.GetClientSize()[0], self.GetClientSize()[1] + 1))
497 self.SetClientSize((self.GetClientSize()[0], self.GetClientSize()[1] - 1))
498 threading.Thread(target=self.update).start()
499 self.initialized = 0
500
501 def move_shape(self, delta):
502 """moves shape (selected in l, which is list ListBox of shapes)
503 by an offset specified in tuple delta.
504 Positive numbers move to (rigt, down)"""
505 name = self.parent.l.GetSelection()
506 if name == wx.NOT_FOUND:
507 return False
508
509 name = self.parent.l.GetString(name)
510
511 model = self.parent.models[name]
512 model.offsets = [
513 model.offsets[0] + delta[0],
514 model.offsets[1] + delta[1],
515 model.offsets[2]
516 ]
517 self.Refresh()
518 return True
519
520 def move(self, event):
521 """react to mouse actions:
522 no mouse: show red mousedrop
523 LMB: move active object,
524 with shift rotate viewport
525 RMB: nothing
526 with shift move viewport
527 """
528 if event.Dragging() and event.LeftIsDown():
529 if self.initpos == None:
530 self.initpos = event.GetPositionTuple()
531 else:
532 if not event.ShiftDown():
533 currentpos = event.GetPositionTuple()
534 delta = (
535 (currentpos[0] - self.initpos[0]),
536 -(currentpos[1] - self.initpos[1])
537 )
538 self.move_shape(delta)
539 self.initpos = None
540 return
541 #print self.initpos
542 p1 = self.initpos
543 self.initpos = None
544 p2 = event.GetPositionTuple()
545 sz = self.GetClientSize()
546 p1x = (float(p1[0]) - sz[0] / 2) / (sz[0] / 2)
547 p1y = -(float(p1[1]) - sz[1] / 2) / (sz[1] / 2)
548 p2x = (float(p2[0]) - sz[0] / 2) / (sz[0] / 2)
549 p2y = -(float(p2[1]) - sz[1] / 2) / (sz[1] / 2)
550 #print p1x,p1y,p2x,p2y
551 quat = trackball(p1x, p1y, p2x, p2y, -self.transv[2] / 250.0)
552 if self.rot:
553 self.basequat = mulquat(self.basequat, quat)
554 #else:
555 glGetDoublev(GL_MODELVIEW_MATRIX, self.mvmat)
556 #self.basequat = quatx
557 mat = build_rotmatrix(self.basequat)
558 glLoadIdentity()
559 glTranslatef(self.transv[0], self.transv[1], 0)
560 glTranslatef(0, 0, self.transv[2])
561 glMultMatrixd(mat)
562 glGetDoublev(GL_MODELVIEW_MATRIX, self.mvmat)
563 self.rot = 1
564
565 elif event.ButtonUp(wx.MOUSE_BTN_LEFT):
566 if self.initpos is not None:
567 self.initpos = None
568 elif event.ButtonUp(wx.MOUSE_BTN_RIGHT):
569 if self.initpos is not None:
570 self.initpos = None
571
572 elif event.Dragging() and event.RightIsDown() and event.ShiftDown():
573 if self.initpos is None:
574 self.initpos = event.GetPositionTuple()
575 else:
576 p1 = self.initpos
577 p2 = event.GetPositionTuple()
578 sz = self.GetClientSize()
579 p1 = list(p1)
580 p2 = list(p2)
581 p1[1] *= -1
582 p2[1] *= -1
583
584 self.transv = map(lambda x, y, z, c: c - self.dist * (x - y) / z, list(p1) + [0], list(p2) + [0], list(sz) + [1], self.transv)
585
586 glLoadIdentity()
587 glTranslatef(self.transv[0], self.transv[1], 0)
588 glTranslatef(0, 0, self.transv[2])
589 if(self.rot):
590 glMultMatrixd(build_rotmatrix(self.basequat))
591 glGetDoublev(GL_MODELVIEW_MATRIX, self.mvmat)
592 self.rot = 1
593 self.initpos = None
594 else:
595 #mouse is moving without a button press
596 p = event.GetPositionTuple()
597 sz = self.GetClientSize()
598 v = map(lambda m, w, b: b * m / w, p, sz, self.bedsize)
599 v[1] = self.bedsize[1] - v[1]
600 self.mousepos = v
601
602 def rotate_shape(self, angle):
603 """rotates acive shape
604 positive angle is clockwise
605 """
606 name = self.parent.l.GetSelection()
607 if name == wx.NOT_FOUND:
608 return False
609 name = self.parent.l.GetString(name)
610 model = self.parent.models[name]
611 model.rot += angle
612
613 def wheel(self, event):
614 """react to mouse wheel actions:
615 rotate object
616 with shift zoom viewport
617 """
618 z = event.GetWheelRotation()
619 angle = 10
620 if not event.ShiftDown():
621 i = self.parent.l.GetSelection()
622
623 if i < 0:
624 try:
625 self.parent.setlayerindex(z)
626 except:
627 pass
628 return
629
630 if z > 0:
631 self.rotate_shape(angle / 2)
632 else:
633 self.rotate_shape(-angle / 2)
634 return
635 if z > 0:
636 self.transv[2] += angle
637 else:
638 self.transv[2] -= angle
639
640 glLoadIdentity()
641 glTranslatef(*self.transv)
642 if(self.rot):
643 glMultMatrixd(build_rotmatrix(self.basequat))
644 glGetDoublev(GL_MODELVIEW_MATRIX, self.mvmat)
645 self.rot = 1
646
647 def keypress(self, event):
648 """gets keypress events and moves/rotates acive shape"""
649 keycode = event.GetKeyCode()
650 print keycode
651 step = 5
652 angle = 18
653 if event.ControlDown():
654 step = 1
655 angle = 1
656 #h
657 if keycode == 72:
658 self.move_shape((-step, 0))
659 #l
660 if keycode == 76:
661 self.move_shape((step, 0))
662 #j
663 if keycode == 75:
664 self.move_shape((0, step))
665 #k
666 if keycode == 74:
667 self.move_shape((0, -step))
668 #[
669 if keycode == 91:
670 self.rotate_shape(-angle)
671 #]
672 if keycode == 93:
673 self.rotate_shape(angle)
674 event.Skip()
675
676 def update(self):
677 while(1):
678 dt = 0.05
679 time.sleep(0.05)
680 try:
681 wx.CallAfter(self.Refresh)
682 except:
683 return
684
685 def anim(self, obj):
686 g = 50 * 9.8
687 v = 20
688 dt = 0.05
689 basepos = obj.offsets[2]
690 obj.offsets[2] += obj.animoffset
691 while obj.offsets[2] > -1:
692 time.sleep(dt)
693 obj.offsets[2] -= v * dt
694 v += g * dt
695 if(obj.offsets[2] < 0):
696 obj.scale[2] *= 1 - 3 * dt
697 #return
698 v = v / 4
699 while obj.offsets[2] < basepos:
700 time.sleep(dt)
701 obj.offsets[2] += v * dt
702 v -= g * dt
703 obj.scale[2] *= 1 + 5 * dt
704 obj.scale[2] = 1.0
705
706 def create_objects(self):
707 '''create opengl objects when opengl is initialized'''
708 self.initialized = 1
709 wx.CallAfter(self.Refresh)
710
711 def drawmodel(self, m, n):
712 batch = pyglet.graphics.Batch()
713 stl = stlview(m.facets, batch=batch)
714 m.batch = batch
715 m.animoffset = 300
716 #print m
717 #threading.Thread(target = self.anim, args = (m, )).start()
718 wx.CallAfter(self.Refresh)
719
720 def update_object_resize(self):
721 '''called when the window recieves only if opengl is initialized'''
722 pass
723
724 def draw_objects(self):
725 '''called in the middle of ondraw after the buffer has been cleared'''
726 if self.vpmat is None:
727 return
728 if not self.initialized:
729 self.create_objects()
730
731 #glLoadIdentity()
732 #print list(self.pmat)
733 if self.rot == 1:
734 glLoadIdentity()
735 glMultMatrixd(self.mvmat)
736 else:
737 glLoadIdentity()
738 glTranslatef(*self.transv)
739 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(0.2, 0.2, 0.2, 1))
740 glBegin(GL_LINES)
741 glNormal3f(0, 0, 1)
742 rows = 10
743 cols = 10
744 zheight = 50
745 for i in xrange(-rows, rows + 1):
746 if i % 5 == 0:
747 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(0.6, 0.6, 0.6, 1))
748 else:
749 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(0.2, 0.2, 0.2, 1))
750 glVertex3f(10 * -cols, 10 * i, 0)
751 glVertex3f(10 * cols, 10 * i, 0)
752 for i in xrange(-cols, cols + 1):
753 if i % 5 == 0:
754 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(0.6, 0.6, 0.6, 1))
755 else:
756 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(0.2, 0.2, 0.2, 1))
757 glVertex3f(10 * i, 10 * -rows, 0)
758 glVertex3f(10 * i, 10 * rows, 0)
759 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(0.6, 0.6, 0.6, 1))
760 glVertex3f(10 * -cols, 10 * -rows, 0)
761 glVertex3f(10 * -cols, 10 * -rows, zheight)
762 glVertex3f(10 * cols, 10 * rows, 0)
763 glVertex3f(10 * cols, 10 * rows, zheight)
764 glVertex3f(10 * cols, 10 * -rows, 0)
765 glVertex3f(10 * cols, 10 * -rows, zheight)
766 glVertex3f(10 * -cols, 10 * rows, 0)
767 glVertex3f(10 * -cols, 10 * rows, zheight)
768
769 glVertex3f(10 * -cols, 10 * rows, zheight)
770 glVertex3f(10 * cols, 10 * rows, zheight)
771 glVertex3f(10 * cols, 10 * rows, zheight)
772 glVertex3f(10 * cols, 10 * -rows, zheight)
773 glVertex3f(10 * cols, 10 * -rows, zheight)
774 glVertex3f(10 * -cols, 10 * -rows, zheight)
775 glVertex3f(10 * -cols, 10 * -rows, zheight)
776 glVertex3f(10 * -cols, 10 * rows, zheight)
777
778 glEnd()
779 glPushMatrix()
780 glTranslatef(self.mousepos[0] - self.bedsize[0] / 2, self.mousepos[1] - self.bedsize[1] / 2, 0)
781 glBegin(GL_TRIANGLES)
782 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(1, 0, 0, 1))
783 glNormal3f(0, 0, 1)
784 glVertex3f(2, 2, 0)
785 glVertex3f(-2, 2, 0)
786 glVertex3f(-2, -2, 0)
787 glVertex3f(2, -2, 0)
788 glVertex3f(2, 2, 0)
789 glVertex3f(-2, -2, 0)
790 glEnd()
791 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(0.3, 0.7, 0.5, 1))
792 #glTranslatef(0, 40, 0)
793 glPopMatrix()
794 glPushMatrix()
795 glTranslatef(-100, -100, 0)
796
797 for i in self.parent.models.values():
798 glPushMatrix()
799 glTranslatef(*(i.offsets))
800 glRotatef(i.rot, 0.0, 0.0, 1.0)
801 glScalef(*i.scale)
802
803 try:
804 if i.curlayer in i.gc.layers:
805 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(0.13, 0.37, 0.25, 1))
806 [i.gc.layers[j].draw() for j in i.gc.layers.keys() if j < i.curlayer]
807 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, vec(0.5, 0.6, 0.9, 1))
808 b = i.gc.layers[i.curlayer]
809 b.draw()
810 else:
811 i.batch.draw()
812 except:
813 i.batch.draw()
814 glPopMatrix()
815 glPopMatrix()
816 #print "drawn batch"
817
818
819 class GCFrame(wx.Frame):
820 '''A simple class for using OpenGL with wxPython.'''
821
822 def __init__(self, parent, ID, title, pos=wx.DefaultPosition,
823 size=wx.DefaultSize, style=wx.DEFAULT_FRAME_STYLE):
824 super(GCFrame, self).__init__(parent, ID, title, pos, (size[0] + 150, size[1]), style)
825
826 class d:
827 def GetSelection(self):
828 return wx.NOT_FOUND
829 self.p = self
830 m = d()
831 m.offsets = [0, 0, 0]
832 m.rot = 0
833 m.curlayer = 0.0
834 m.scale = [1.0, 1.0, 1.0]
835 m.batch = pyglet.graphics.Batch()
836 m.gc = gcview([], batch=m.batch)
837 self.models = {"": m}
838 self.l = d()
839 self.modelindex = 0
840 self.GLPanel1 = TestGlPanel(self, size)
841
842 def addfile(self, gcode=[]):
843 self.models[""].gc.delete()
844 self.models[""].gc = gcview(gcode, batch=self.models[""].batch)
845
846 def clear(self):
847 self.models[""].gc.delete()
848 self.models[""].gc = gcview([], batch=self.models[""].batch)
849
850 def Show(self, arg=True):
851 wx.Frame.Show(self, arg)
852 self.SetClientSize((self.GetClientSize()[0], self.GetClientSize()[1] + 1))
853 self.SetClientSize((self.GetClientSize()[0], self.GetClientSize()[1] - 1))
854 self.Refresh()
855 wx.FutureCall(500, self.GLPanel1.forceresize)
856 #threading.Thread(target = self.update).start()
857 #self.initialized = 0
858
859 def setlayerindex(self, z):
860 m = self.models[""]
861 mlk = sorted(m.gc.layers.keys())
862 if z > 0 and self.modelindex < len(mlk) - 1:
863 self.modelindex += 1
864 if z < 0 and self.modelindex > 0:
865 self.modelindex -= 1
866 m.curlayer = mlk[self.modelindex]
867 wx.CallAfter(self.SetTitle, "Gcode view, shift to move. Layer %d, Z = %f" % (self.modelindex, m.curlayer))
868
869
870 def main():
871 app = wx.App(redirect=False)
872 frame = GCFrame(None, wx.ID_ANY, 'Gcode view, shift to move view, mousewheel to set layer', size=(400, 400))
873 frame.addfile(list(open("carriage dump_export.gcode")))
874 #frame = wx.Frame(None, -1, "GL Window", size=(400, 400))
875 #panel = TestGlPanel(frame)
876 #frame.Show(True)
877 #app.MainLoop()
878 app.Destroy()
879
880 if __name__ == "__main__":
881 import cProfile
882 print cProfile.run("main()")
Something went wrong with that request. Please try again.