SelectionBox.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping
00003  * Framework Based on Octrees
00004  * http://octomap.github.io
00005  *
00006  * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg
00007  * All rights reserved. License for the viewer octovis: GNU GPL v2
00008  * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
00009  *
00010  *
00011  * This program is free software; you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation; either version 2 of the License, or
00014  * (at your option) any later version.
00015  *
00016  * This program is distributed in the hope that it will be useful, but
00017  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
00018  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
00019  * for more details.
00020  *
00021  * You should have received a copy of the GNU General Public License
00022  * along with this program. If not, see http://www.gnu.org/licenses/.
00023  */
00024 
00025 // workaround for Windows
00026 #define NOMINMAX
00027 #include <octovis/SelectionBox.h>
00028 #include <manipulatedFrame.h>
00029 
00030 namespace octomap{
00031 
00032 SelectionBox::SelectionBox()
00033 : m_visible(false),
00034   m_minPt(0,0,0), m_maxPt(1,1,1),
00035   m_arrowLength(0.2)
00036 {
00037 
00038 
00039 
00040   for (unsigned i=0; i< 3; ++i){
00041     m_frames.push_back(new qglviewer::ManipulatedFrame());
00042   }
00043 
00044   for (unsigned i=0; i< 3; ++i){
00045     m_frames.push_back(new qglviewer::ManipulatedFrame());
00046   }
00047 
00048   qglviewer::WorldConstraint* XAxis = new qglviewer::WorldConstraint();
00049   XAxis->setTranslationConstraint(qglviewer::AxisPlaneConstraint::AXIS, qglviewer::Vec(1.0,0.0,0.0));
00050   XAxis->setRotationConstraint   (qglviewer::AxisPlaneConstraint::FORBIDDEN, qglviewer::Vec(0.0,0.0,0.0));
00051 
00052   qglviewer::WorldConstraint* YAxis = new qglviewer::WorldConstraint();
00053   YAxis->setTranslationConstraint(qglviewer::AxisPlaneConstraint::AXIS, qglviewer::Vec(0.0,1.0,0.0));
00054   YAxis->setRotationConstraint   (qglviewer::AxisPlaneConstraint::FORBIDDEN, qglviewer::Vec(0.0,0.0,0.0));
00055 
00056   qglviewer::WorldConstraint* ZAxis = new qglviewer::WorldConstraint();
00057   ZAxis->setTranslationConstraint(qglviewer::AxisPlaneConstraint::AXIS, qglviewer::Vec(0.0,0.0,1.0));
00058   ZAxis->setRotationConstraint   (qglviewer::AxisPlaneConstraint::FORBIDDEN, qglviewer::Vec(0.0,0.0,0.0));
00059 
00060 
00061   frame(0)->setConstraint(XAxis);
00062   frame(1)->setConstraint(YAxis);
00063   frame(2)->setConstraint(ZAxis);
00064   frame(3)->setConstraint(XAxis);
00065   frame(4)->setConstraint(YAxis);
00066   frame(5)->setConstraint(ZAxis);
00067 
00068 
00069 
00070 }
00071 
00072 SelectionBox::~SelectionBox(){
00073   delete m_frames[0];
00074   delete m_frames[1];
00075 }
00076 
00077 void SelectionBox::draw(bool withNames){
00078 
00079 
00080   // set min/max new from grabbed frame:
00081   for (unsigned i = 0; i < m_frames.size(); ++i){
00082     if (frame(i)->grabsMouse()){
00083       qglviewer::Vec f = frame(i)->position();
00084 
00085       unsigned oi = i+3;
00086       float corr = m_arrowLength/2.0;
00087       if (i >= 3){
00088         oi = i-3;
00089         corr *= -1;
00090       }
00091 
00092       qglviewer::Vec fo = frame(oi)->position();
00093 
00094       unsigned ci = i%3;
00095       m_minPt[ci] = std::min(f[ci] - corr, fo[ci] + corr);
00096       m_maxPt[ci] = std::max(f[ci] - corr, fo[ci] + corr);
00097     }
00098   }
00099 
00100   // draw box:
00101 
00102   glEnable(GL_LINE_SMOOTH);
00103   glLineWidth(2.);
00104   glDisable(GL_LIGHTING);
00105   glColor3f(0.9,0.0, 0.0);
00106   glBegin(GL_LINE_LOOP); // Start drawing a line primitive
00107   glVertex3f(m_minPt.x, m_minPt.y, m_minPt.z);
00108   glVertex3f(m_maxPt.x, m_minPt.y, m_minPt.z);
00109   glVertex3f(m_maxPt.x, m_maxPt.y, m_minPt.z);
00110   glVertex3f(m_minPt.x, m_maxPt.y, m_minPt.z);
00111   glEnd();
00112 
00113   glBegin(GL_LINE_LOOP);
00114   glVertex3f(m_minPt.x, m_minPt.y, m_maxPt.z);
00115   glVertex3f(m_maxPt.x, m_minPt.y, m_maxPt.z);
00116   glVertex3f(m_maxPt.x, m_maxPt.y, m_maxPt.z);
00117   glVertex3f(m_minPt.x, m_maxPt.y, m_maxPt.z);
00118   //      glVertex3f(-1.0f, -1.0f, 0.0f); // The bottom left corner
00119   //      glVertex3f(-1.0f, 1.0f, 0.0f); // The top left corner
00120   //      glVertex3f(1.0f, 1.0f, 0.0f); // The top right corner
00121   //      glVertex3f(1.0f, -1.0f, 0.0f); // The bottom right corner
00122   glEnd();
00123 
00124   glBegin(GL_LINES);
00125   glVertex3f(m_minPt.x, m_minPt.y, m_minPt.z);
00126   glVertex3f(m_minPt.x, m_minPt.y, m_maxPt.z);
00127 
00128   glVertex3f(m_maxPt.x, m_minPt.y, m_minPt.z);
00129   glVertex3f(m_maxPt.x, m_minPt.y, m_maxPt.z);
00130 
00131   glVertex3f(m_maxPt.x, m_maxPt.y, m_minPt.z);
00132   glVertex3f(m_maxPt.x, m_maxPt.y, m_maxPt.z);
00133 
00134   glVertex3f(m_minPt.x, m_maxPt.y, m_minPt.z);
00135   glVertex3f(m_minPt.x, m_maxPt.y, m_maxPt.z);
00136   glEnd();
00137 
00138   glDisable(GL_LINE_SMOOTH);
00139   glEnable(GL_LIGHTING);
00140 
00141 
00142   // correct all arrow frames:
00143 
00144   for (unsigned i = 0; i < m_frames.size(); ++i){
00145     qglviewer::Vec pt = m_minPt;
00146     float corr = m_arrowLength/2;
00147     if (i/3 == 1){
00148       pt = m_maxPt;
00149       corr *= -1;
00150     }
00151 
00152     pt[i%3] += corr;
00153 
00154     frame(i)->setTranslation(pt);
00155 
00156   }
00157 
00158   // draw spheres in their frames:
00159   //      GLUquadricObj* quadric=gluNewQuadric();
00160   //      gluQuadricNormals(quadric, GLU_SMOOTH);
00161   //      glColor4f(1.0, 0.0, 0.0, 1.0);
00162 
00163   GLboolean lighting, colorMaterial;
00164   glGetBooleanv(GL_LIGHTING, &lighting);
00165   glGetBooleanv(GL_COLOR_MATERIAL, &colorMaterial);
00166 
00167   glDisable(GL_COLOR_MATERIAL);
00168   for (unsigned i = 0; i < m_frames.size(); ++i){
00169     glPushMatrix();
00170     glMultMatrixd(m_frames[i]->matrix());
00171     if (withNames)
00172       glPushName(i);
00173 
00174     float length = m_arrowLength;
00175     if (frame(i)->grabsMouse())
00176       length *= 2;
00177 
00178     const float radius = length/20;
00179 
00180     float color[4];
00181     if (i%3 == 0){ // x
00182       color[0] = 1.0f;  color[1] = 0.7f;  color[2] = 0.7f;  color[3] = 1.0f;
00183       glPushMatrix();
00184       glRotatef(90.0, 0.0, 1.0, 0.0);
00185 
00186     } else if (i%3 == 1){ // y
00187       color[0] = 0.7f;  color[1] = 1.0f;  color[2] = 0.7f;  color[3] = 1.0f;
00188       glPushMatrix();
00189       glRotatef(-90.0, 1.0, 0.0, 0.0);
00190 
00191     } else {  // z
00192       glPushMatrix();
00193       color[0] = 0.7f;  color[1] = 0.7f;  color[2] = 1.0f;  color[3] = 1.0f;
00194     }
00195     glTranslatef(0.0, 0.0, -length/2.0);
00196     glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
00197     QGLViewer::drawArrow(length, radius);
00198     glPopMatrix();
00199 
00200 
00201 
00202     if (withNames)
00203       glPopName();
00204 
00205     glPopMatrix();
00206   }
00207   if (colorMaterial)
00208     glEnable(GL_COLOR_MATERIAL);
00209   if (!lighting)
00210     glDisable(GL_LIGHTING);
00211 
00212 
00213   //gluDeleteQuadric(quadric);
00214 
00215 }
00216 
00217 void SelectionBox::getBBXMin(float& x, float& y, float& z) const {
00218   x = m_minPt.x;
00219   y = m_minPt.y;
00220   z = m_minPt.z;
00221 }
00222 
00223 void SelectionBox::getBBXMax(float& x, float& y, float& z) const {
00224   x = m_maxPt.x;
00225   y = m_maxPt.y;
00226   z = m_maxPt.z;
00227 }
00228 
00229 int SelectionBox::getGrabbedFrame() const {
00230   int frameid = -1;
00231   for (unsigned i = 0; i < m_frames.size(); ++i){
00232     if (frame(i)->grabsMouse()){
00233       frameid = i;
00234       break;
00235     }
00236   }
00237 
00238   return frameid;
00239 }
00240 
00241 
00242 void SelectionBox::drawAxis(float length) const
00243 {
00244   const float radius = length/20;
00245 
00246   GLboolean lighting, colorMaterial;
00247   glGetBooleanv(GL_LIGHTING, &lighting);
00248   glGetBooleanv(GL_COLOR_MATERIAL, &colorMaterial);
00249 
00250   glDisable(GL_COLOR_MATERIAL);
00251 
00252   float color[4];
00253   color[0] = 0.7f;  color[1] = 0.7f;  color[2] = 1.0f;  color[3] = 1.0f;
00254   glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
00255   QGLViewer::drawArrow(length, radius);
00256 
00257   color[0] = 1.0f;  color[1] = 0.7f;  color[2] = 0.7f;  color[3] = 1.0f;
00258   glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
00259   glPushMatrix();
00260   glRotatef(90.0, 0.0, 1.0, 0.0);
00261   QGLViewer::drawArrow(length, radius);
00262   glPopMatrix();
00263 
00264   color[0] = 0.7f;  color[1] = 1.0f;  color[2] = 0.7f;  color[3] = 1.0f;
00265   glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
00266   glPushMatrix();
00267   glRotatef(-90.0, 1.0, 0.0, 0.0);
00268   QGLViewer::drawArrow(length, radius);
00269   glPopMatrix();
00270 
00271   if (colorMaterial)
00272     glEnable(GL_COLOR_MATERIAL);
00273   if (!lighting)
00274     glDisable(GL_LIGHTING);
00275 }
00276 
00277 
00278 }


octovis
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:51:20