gl_widget.py
Go to the documentation of this file.
1 # Copyright (c) 2011, Dorian Scholz, TU Darmstadt
2 # All rights reserved.
3 #
4 # Redistribution and use in source and binary forms, with or without
5 # modification, are permitted provided that the following conditions
6 # are met:
7 #
8 # * Redistributions of source code must retain the above copyright
9 # notice, this list of conditions and the following disclaimer.
10 # * Redistributions in binary form must reproduce the above
11 # copyright notice, this list of conditions and the following
12 # disclaimer in the documentation and/or other materials provided
13 # with the distribution.
14 # * Neither the name of the TU Darmstadt nor the names of its
15 # contributors may be used to endorse or promote products derived
16 # from this software without specific prior written permission.
17 #
18 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
21 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
22 # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
23 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
28 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 # POSSIBILITY OF SUCH DAMAGE.
30 
31 from __future__ import division
32 import math
33 import numpy
34 
35 from python_qt_binding.QtCore import QPoint, Qt
36 from python_qt_binding.QtOpenGL import QGLFormat, QGLWidget
37 
38 import OpenGL
39 OpenGL.ERROR_CHECKING = True
40 from OpenGL.GL import glClear, glClearColor, glEnable, glGetDoublev, glLoadIdentity, glLoadMatrixd, glMatrixMode, glMultMatrixd, glRotated, glTranslated, glTranslatef, glViewport, GL_COLOR_BUFFER_BIT, GL_DEPTH_BUFFER_BIT, GL_DEPTH_TEST, GL_MODELVIEW, GL_MODELVIEW_MATRIX, GL_PROJECTION
41 from OpenGL.GLU import gluPerspective
42 
43 
44 class GLWidget(QGLWidget):
45 
46  def __init__(self, parent=None):
47  glformat = QGLFormat()
48  glformat.setSampleBuffers(True)
49  super(GLWidget, self).__init__(glformat, parent)
50 
51  self.setCursor(Qt.OpenHandCursor)
52  self.setMouseTracking(True)
53 
54  self._modelview_matrix = numpy.identity(4)
55  self._near = 0.1
56  self._far = 100.0
57  self._fovy = 45.0
58  self._radius = 5.0
59  self._last_point_2d = QPoint()
60  self._last_point_3d = [0.0, 0.0, 0.0]
61  self._last_point_3d_ok = False
62 
63  def initializeGL(self):
64  glClearColor(0.0, 0.0, 0.0, 0.0)
65  glEnable(GL_DEPTH_TEST)
66 
67  def resizeGL(self, width, height):
68  glViewport(0, 0, width, height)
69  self.set_projection(self._near, self._far, self._fovy)
70  self.updateGL()
71 
72  def paintGL(self):
73  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
74  glMatrixMode(GL_MODELVIEW)
75  glLoadMatrixd(self._modelview_matrix)
76 
77  def get_view_matrix(self):
78  return self._modelview_matrix.tolist()
79 
80  def set_view_matrix(self, matrix):
81  self._modelview_matrix = numpy.array(matrix)
82 
83  def set_projection(self, near, far, fovy):
84  self._near = near
85  self._far = far
86  self._fovy = fovy
87  self.makeCurrent()
88  glMatrixMode(GL_PROJECTION)
89  glLoadIdentity()
90  height = max(self.height(), 1)
91  gluPerspective(self._fovy, float(self.width()) / float(height), self._near, self._far)
92  self.updateGL()
93 
94  def reset_view(self):
95  # scene pos and size
96  glMatrixMode(GL_MODELVIEW)
97  glLoadIdentity()
98  self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
99  self.view_all()
100 
101  def reset_rotation(self):
102  self._modelview_matrix[0] = [1.0, 0.0, 0.0, 0.0]
103  self._modelview_matrix[1] = [0.0, 1.0, 0.0, 0.0]
104  self._modelview_matrix[2] = [0.0, 0.0, 1.0, 0.0]
105  glMatrixMode(GL_MODELVIEW)
106  glLoadMatrixd(self._modelview_matrix)
107 
108  def translate(self, trans):
109  # translate the object
110  self.makeCurrent()
111  glMatrixMode(GL_MODELVIEW)
112  glLoadIdentity()
113  glTranslated(trans[0], trans[1], trans[2])
114  glMultMatrixd(self._modelview_matrix)
115  # update _modelview_matrix
116  self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
117 
118  def rotate(self, axis, angle):
119  # rotate the object
120  self.makeCurrent()
121  glMatrixMode(GL_MODELVIEW)
122  glLoadIdentity()
123  t = [self._modelview_matrix[3][0], self._modelview_matrix[3][1], self._modelview_matrix[3][2]]
124  glTranslatef(t[0], t[1], t[2])
125  glRotated(angle, axis[0], axis[1], axis[2])
126  glTranslatef(-t[0], -t[1], -t[2])
127  glMultMatrixd(self._modelview_matrix)
128  # update _modelview_matrix
129  self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
130 
131  def view_all(self):
132  self.translate([-self._modelview_matrix[0][3], -self._modelview_matrix[1][3], -self._modelview_matrix[2][3] - self._radius / 2.0])
133 
134  def wheelEvent(self, event):
135  # only zoom when no mouse buttons are pressed, to prevent interference with other user interactions
136  if event.buttons() == Qt.NoButton:
137  try:
138  delta = event.angleDelta().y()
139  except AttributeError:
140  delta = event.delta()
141  d = float(delta) / 200.0 * self._radius
142  self.translate([0.0, 0.0, d])
143  self.updateGL()
144  event.accept()
145 
146  def mousePressEvent(self, event):
147  self._last_point_2d = event.pos()
149 
150  def mouseMoveEvent(self, event):
151  new_point_2d = event.pos()
152 
153  if not self.rect().contains(new_point_2d):
154  return
155 
156  new_point_3d_ok, new_point_3d = self._map_to_sphere(new_point_2d)
157 
158  dy = float(new_point_2d.y() - self._last_point_2d.y())
159  h = float(self.height())
160 
161  # left button: rotate around center
162  if event.buttons() == Qt.LeftButton and event.modifiers() == Qt.NoModifier:
163  if self._last_point_3d_ok and new_point_3d_ok:
164  cos_angle = numpy.dot(self._last_point_3d, new_point_3d)
165  if abs(cos_angle) < 1.0:
166  axis = numpy.cross(self._last_point_3d, new_point_3d)
167  angle = 2.0 * math.acos(cos_angle) * 180.0 / math.pi
168  self.rotate(axis, angle)
169 
170  # middle button (or left + shift): move in x-y-direction
171  elif event.buttons() == Qt.MidButton or (event.buttons() == Qt.LeftButton and event.modifiers() == Qt.ShiftModifier):
172  dx = float(new_point_2d.x() - self._last_point_2d.x())
173  w = float(self.width())
174  z = -self._modelview_matrix[3][2] / self._modelview_matrix[3][3]
175  n = 0.01 * self._radius
176  up = math.tan(self._fovy / 2.0 * math.pi / 180.0) * n
177  right = up * w / h
178  self.translate([2.0 * dx / w * right / n * z, -2.0 * dy / h * up / n * z, 0.0])
179 
180  # left and middle button (or left + ctrl): move in z-direction
181  elif event.buttons() == (Qt.LeftButton | Qt.MidButton) or (event.buttons() == Qt.LeftButton and event.modifiers() == Qt.ControlModifier):
182  delta_z = self._radius * dy * 2.0 / h
183  self.translate([0.0, 0.0, delta_z])
184 
185  # remember the new points and flag
186  self._last_point_2d = new_point_2d
187  self._last_point_3d = new_point_3d
188  self._last_point_3d_ok = new_point_3d_ok
189 
190  # trigger redraw
191  self.updateGL()
192 
193  def mouseReleaseEvent(self, _event):
194  self._last_point_3d_ok = False
195 
196  def _map_to_sphere(self, pos):
197  v = [0.0, 0.0, 0.0]
198  # check if inside widget
199  if self.rect().contains(pos):
200  # map widget coordinates to the centered unit square [-0.5..0.5] x [-0.5..0.5]
201  v[0] = float(pos.x() - 0.5 * self.width()) / self.width()
202  v[1] = float(0.5 * self.height() - pos.y()) / self.height()
203  # use Pythagoras to compute z (the sphere has radius sqrt(2.0*0.5*0.5))
204  v[2] = math.sqrt(max(0.5 - v[0] * v[0] - v[1] * v[1], 0.0))
205  # normalize direction to unit sphere
206  v = numpy.array(v) / numpy.linalg.norm(v)
207  return True, v
208  else:
209  return False, v
def mouseReleaseEvent(self, _event)
Definition: gl_widget.py:193
def set_projection(self, near, far, fovy)
Definition: gl_widget.py:83
def __init__(self, parent=None)
Definition: gl_widget.py:46
def mouseMoveEvent(self, event)
Definition: gl_widget.py:150
def resizeGL(self, width, height)
Definition: gl_widget.py:67
def rotate(self, axis, angle)
Definition: gl_widget.py:118
def set_view_matrix(self, matrix)
Definition: gl_widget.py:80
def mousePressEvent(self, event)
Definition: gl_widget.py:146


rqt_pose_view
Author(s): Dorian Scholz
autogenerated on Sat Jun 8 2019 18:05:11