gl_widget.py
Go to the documentation of this file.
00001 # Copyright (c) 2011, Dorian Scholz, TU Darmstadt
00002 # All rights reserved.
00003 #
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions
00006 # are met:
00007 #
00008 #   * Redistributions of source code must retain the above copyright
00009 #     notice, this list of conditions and the following disclaimer.
00010 #   * Redistributions in binary form must reproduce the above
00011 #     copyright notice, this list of conditions and the following
00012 #     disclaimer in the documentation and/or other materials provided
00013 #     with the distribution.
00014 #   * Neither the name of the TU Darmstadt nor the names of its
00015 #     contributors may be used to endorse or promote products derived
00016 #     from this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00019 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00020 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00021 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00022 # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00023 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00024 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00026 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00027 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00028 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 # POSSIBILITY OF SUCH DAMAGE.
00030 
00031 from __future__ import division
00032 import math
00033 import numpy
00034 
00035 from python_qt_binding.QtCore import QPoint, Qt
00036 from python_qt_binding.QtOpenGL import QGLFormat, QGLWidget
00037 
00038 import OpenGL
00039 OpenGL.ERROR_CHECKING = True
00040 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
00041 from OpenGL.GLU import gluPerspective
00042 
00043 
00044 class GLWidget(QGLWidget):
00045 
00046     def __init__(self, parent=None):
00047         glformat = QGLFormat()
00048         glformat.setSampleBuffers(True)
00049         super(GLWidget, self).__init__(glformat, parent)
00050 
00051         self.setCursor(Qt.OpenHandCursor)
00052         self.setMouseTracking(True)
00053 
00054         self._modelview_matrix = numpy.identity(4)
00055         self._near = 0.1
00056         self._far = 100.0
00057         self._fovy = 45.0
00058         self._radius = 5.0
00059         self._last_point_2d = QPoint()
00060         self._last_point_3d = [0.0, 0.0, 0.0]
00061         self._last_point_3d_ok = False
00062 
00063     def initializeGL(self):
00064         glClearColor(0.0, 0.0, 0.0, 0.0)
00065         glEnable(GL_DEPTH_TEST)
00066 
00067     def resizeGL(self, width, height):
00068         glViewport(0, 0, width, height)
00069         self.set_projection(self._near, self._far, self._fovy)
00070         self.updateGL()
00071 
00072     def paintGL(self):
00073         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
00074         glMatrixMode(GL_MODELVIEW)
00075         glLoadMatrixd(self._modelview_matrix)
00076 
00077     def get_view_matrix(self):
00078         return self._modelview_matrix.tolist()
00079 
00080     def set_view_matrix(self, matrix):
00081         self._modelview_matrix = numpy.array(matrix)
00082 
00083     def set_projection(self, near, far, fovy):
00084         self._near = near
00085         self._far = far
00086         self._fovy = fovy
00087         self.makeCurrent()
00088         glMatrixMode(GL_PROJECTION)
00089         glLoadIdentity()
00090         height = max(self.height(), 1)
00091         gluPerspective(self._fovy, float(self.width()) / float(height), self._near, self._far)
00092         self.updateGL()
00093 
00094     def reset_view(self):
00095         # scene pos and size
00096         glMatrixMode(GL_MODELVIEW)
00097         glLoadIdentity()
00098         self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
00099         self.view_all()
00100 
00101     def reset_rotation(self):
00102         self._modelview_matrix[0] = [1.0, 0.0, 0.0, 0.0]
00103         self._modelview_matrix[1] = [0.0, 1.0, 0.0, 0.0]
00104         self._modelview_matrix[2] = [0.0, 0.0, 1.0, 0.0]
00105         glMatrixMode(GL_MODELVIEW)
00106         glLoadMatrixd(self._modelview_matrix)
00107 
00108     def translate(self, trans):
00109         # translate the object
00110         self.makeCurrent()
00111         glMatrixMode(GL_MODELVIEW)
00112         glLoadIdentity()
00113         glTranslated(trans[0], trans[1], trans[2])
00114         glMultMatrixd(self._modelview_matrix)
00115         # update _modelview_matrix
00116         self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
00117 
00118     def rotate(self, axis, angle):
00119         # rotate the object
00120         self.makeCurrent()
00121         glMatrixMode(GL_MODELVIEW)
00122         glLoadIdentity()
00123         t = [self._modelview_matrix[3][0], self._modelview_matrix[3][1], self._modelview_matrix[3][2]]
00124         glTranslatef(t[0], t[1], t[2])
00125         glRotated(angle, axis[0], axis[1], axis[2])
00126         glTranslatef(-t[0], -t[1], -t[2])
00127         glMultMatrixd(self._modelview_matrix)
00128         # update _modelview_matrix
00129         self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
00130 
00131     def view_all(self):
00132         self.translate([-self._modelview_matrix[0][3], -self._modelview_matrix[1][3], -self._modelview_matrix[2][3] - self._radius / 2.0])
00133 
00134     def wheelEvent(self, event):
00135         # only zoom when no mouse buttons are pressed, to prevent interference with other user interactions
00136         if event.buttons() == Qt.NoButton:
00137             try:
00138                 delta = event.angleDelta().y()
00139             except AttributeError:
00140                 delta = event.delta()
00141             d = float(delta) / 200.0 * self._radius
00142             self.translate([0.0, 0.0, d])
00143             self.updateGL()
00144             event.accept()
00145 
00146     def mousePressEvent(self, event):
00147         self._last_point_2d = event.pos()
00148         self._last_point_3d_ok, self._last_point_3d = self._map_to_sphere(self._last_point_2d)
00149 
00150     def mouseMoveEvent(self, event):
00151         new_point_2d = event.pos()
00152 
00153         if not self.rect().contains(new_point_2d):
00154             return
00155 
00156         new_point_3d_ok, new_point_3d = self._map_to_sphere(new_point_2d)
00157 
00158         dy = float(new_point_2d.y() - self._last_point_2d.y())
00159         h = float(self.height())
00160 
00161         # left button: rotate around center
00162         if event.buttons() == Qt.LeftButton and event.modifiers() == Qt.NoModifier:
00163             if self._last_point_3d_ok and new_point_3d_ok:
00164                 cos_angle = numpy.dot(self._last_point_3d, new_point_3d)
00165                 if abs(cos_angle) < 1.0:
00166                     axis = numpy.cross(self._last_point_3d, new_point_3d)
00167                     angle = 2.0 * math.acos(cos_angle) * 180.0 / math.pi
00168                     self.rotate(axis, angle)
00169 
00170         # middle button (or left + shift): move in x-y-direction
00171         elif event.buttons() == Qt.MidButton or (event.buttons() == Qt.LeftButton and event.modifiers() == Qt.ShiftModifier):
00172             dx = float(new_point_2d.x() - self._last_point_2d.x())
00173             w = float(self.width())
00174             z = -self._modelview_matrix[3][2] / self._modelview_matrix[3][3]
00175             n = 0.01 * self._radius
00176             up = math.tan(self._fovy / 2.0 * math.pi / 180.0) * n
00177             right = up * w / h
00178             self.translate([2.0 * dx / w * right / n * z, -2.0 * dy / h * up / n * z, 0.0])
00179 
00180         # left and middle button (or left + ctrl): move in z-direction
00181         elif event.buttons() == (Qt.LeftButton | Qt.MidButton) or (event.buttons() == Qt.LeftButton and event.modifiers() == Qt.ControlModifier):
00182             delta_z = self._radius * dy * 2.0 / h
00183             self.translate([0.0, 0.0, delta_z])
00184 
00185         # remember the new points and flag
00186         self._last_point_2d = new_point_2d
00187         self._last_point_3d = new_point_3d
00188         self._last_point_3d_ok = new_point_3d_ok
00189 
00190         # trigger redraw
00191         self.updateGL()
00192 
00193     def mouseReleaseEvent(self, _event):
00194         self._last_point_3d_ok = False
00195 
00196     def _map_to_sphere(self, pos):
00197         v = [0.0, 0.0, 0.0]
00198         # check if inside widget
00199         if self.rect().contains(pos):
00200             # map widget coordinates to the centered unit square [-0.5..0.5] x [-0.5..0.5]
00201             v[0] = float(pos.x() - 0.5 * self.width()) / self.width()
00202             v[1] = float(0.5 * self.height() - pos.y()) / self.height()
00203             # use Pythagoras to compute z (the sphere has radius sqrt(2.0*0.5*0.5))
00204             v[2] = math.sqrt(max(0.5 - v[0] * v[0] - v[1] * v[1], 0.0))
00205             # normalize direction to unit sphere
00206             v = numpy.array(v) / numpy.linalg.norm(v)
00207             return True, v
00208         else:
00209             return False, v


rqt_pose_view
Author(s): Dorian Scholz
autogenerated on Thu Jun 6 2019 18:58:42