TrajectoryDrawer.cpp
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License (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 along
00022  * with this program; if not, write to the Free Software Foundation, Inc.,
00023  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00024  */
00025 
00026 #include <octovis/TrajectoryDrawer.h>
00027 
00028 namespace octomap {
00029 
00030   TrajectoryDrawer::TrajectoryDrawer()
00031   : ScanGraphDrawer(), m_trajectoryVertexArray(NULL), m_trajectoryColorArray(NULL), m_trajectorySize(0)
00032   {
00033 
00034   }
00035 
00036   TrajectoryDrawer::TrajectoryDrawer(const octomap::ScanGraph& graph)
00037   : ScanGraphDrawer(), m_trajectoryVertexArray(NULL), m_trajectoryColorArray(NULL), m_trajectorySize(0)
00038   {
00039     this->setScanGraph(graph);
00040   }
00041 
00042   TrajectoryDrawer::~TrajectoryDrawer() {
00043     clear();
00044 
00045   }
00046 
00047   void TrajectoryDrawer::draw() const{
00048     if (m_trajectorySize == 0)
00049       return;
00050 
00051     // draw lines:
00052     glEnableClientState(GL_VERTEX_ARRAY);
00053     glEnableClientState(GL_COLOR_ARRAY);
00054     glLineWidth(3.0f);
00055     glVertexPointer(3, GL_FLOAT, 0, m_trajectoryVertexArray);
00056     glColorPointer(4, GL_FLOAT, 0, m_trajectoryColorArray);
00057     glDrawArrays(GL_LINE_STRIP, 0, m_trajectorySize);
00058     glDisableClientState(GL_COLOR_ARRAY);
00059     glDisableClientState(GL_VERTEX_ARRAY);
00060 
00061     // draw nodes:
00062     GLUquadricObj* quadric=gluNewQuadric();
00063     gluQuadricNormals(quadric, GLU_SMOOTH);
00064     for (uint i = 0; i < m_trajectorySize; ++i){
00065       glPushMatrix();
00066       glTranslatef(m_trajectoryVertexArray[3*i], m_trajectoryVertexArray[3*i +1], m_trajectoryVertexArray[3*i +2]);
00067       glColor4f(m_trajectoryColorArray[4*i],m_trajectoryColorArray[4*i+1],m_trajectoryColorArray[4*i+2],m_trajectoryColorArray[4*i+3]);
00068       gluSphere(quadric, 0.05, 32, 32);
00069       glPopMatrix();
00070     }
00071 
00072     gluDeleteQuadric(quadric);
00073   }
00074 
00075   void TrajectoryDrawer::clear(){
00076 
00077     if (m_trajectorySize != 0) {
00078       delete[] m_trajectoryVertexArray;
00079       delete[] m_trajectoryColorArray;
00080       m_trajectorySize = 0;
00081     }
00082   }
00083 
00084   void TrajectoryDrawer::setScanGraph(const octomap::ScanGraph& graph){
00085 
00086     clear();
00087 
00088     m_trajectorySize = graph.size();
00089     m_trajectoryVertexArray = new GLfloat[m_trajectorySize * 3];
00090     m_trajectoryColorArray = new GLfloat[m_trajectorySize * 4];
00091 
00092     uint i = 0;
00093     for (octomap::ScanGraph::const_iterator it = graph.begin(); it != graph.end(); it++) {
00094       m_trajectoryVertexArray[i] = (*it)->pose.trans().x();
00095       m_trajectoryVertexArray[i+1] = (*it)->pose.trans().y();
00096       m_trajectoryVertexArray[i+2] = (*it)->pose.trans().z();
00097       i+=3;
00098     }
00099 
00100     for (unsigned int j=0; j < m_trajectorySize*4; j+=4) {
00101       m_trajectoryColorArray[j]   = 0.; // r
00102       m_trajectoryColorArray[j+1] = 0.; // g
00103       m_trajectoryColorArray[j+2] = 1.; // b
00104       m_trajectoryColorArray[j+3] = 1.; // alpha
00105     }
00106   }
00107 
00108 }
00109 


octovis
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:26