OcTreeDrawer.h

Go to the documentation of this file.

/*
 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
 * http://octomap.github.com/
 *
 * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
 * All rights reserved.
 * License (octovis): GNU GPL v2
 * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
 *
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#ifndef OCTREEDRAWER_H_
#define OCTREEDRAWER_H_

#include "SceneObject.h"

namespace octomap {

  class OcTreeDrawer: public octomap::SceneObject {
  public:
    OcTreeDrawer();
    virtual ~OcTreeDrawer();
    void clear();

    void draw() const;

    // initialization of drawer  -------------------------

    void setOcTree(const AbstractOcTree& octree){
      octomap::pose6d o; // initialized to (0,0,0) , (0,0,0,1) by default
      setOcTree(octree, o, 0);
    }

    virtual void setOcTree(const AbstractOcTree& octree, const octomap::pose6d& origin, int map_id_);

    // modification of existing drawer  ------------------

    void setOcTreeSelection(const std::list<octomap::OcTreeVolume>& selectedPoints);

    void clearOcTreeSelection();

    void setAlphaOccupied(double alpha);

    void enableOcTree(bool enabled = true);
    void enableOcTreeCells(bool enabled = true) {m_drawOccupied = enabled; };
    void enableFreespace(bool enabled = true) {m_drawFree = enabled; };
    void enableSelection(bool enabled = true) {m_drawSelection = enabled; };
    void setMax_tree_depth(unsigned int max_tree_depth) {m_max_tree_depth = max_tree_depth;};

    // set new origin (move object)
    void setOrigin(octomap::pose6d t);
    void enableAxes(bool enabled = true) { m_displayAxes = enabled; };

  protected:
    //void clearOcTree();
    void clearOcTreeStructure();

    void drawOctreeGrid() const;
    void drawOccupiedVoxels() const;
    void drawFreeVoxels() const;
    void drawSelection() const;
    void drawCubes(GLfloat** cubeArray, unsigned int cubeArraySize,
        GLfloat* cubeColorArray = NULL) const;

    void drawAxes() const;

    void generateCubes(const std::list<octomap::OcTreeVolume>& voxels,
                       GLfloat*** glArray, unsigned int& glArraySize, 
                       octomath::Pose6D& origin,
                       GLfloat** glColorArray = NULL);
    
    void clearCubes(GLfloat*** glArray, unsigned int& glArraySize,
                    GLfloat** glColorArray = NULL);
    void initGLArrays(const unsigned int& num_cubes, unsigned int& glArraySize,
                       GLfloat*** glArray, GLfloat** glColorArray);
    void initCubeTemplate(const octomath::Pose6D& origin,
                          std::vector<octomath::Vector3>& cube_template);
    unsigned int generateCube(const octomap::OcTreeVolume& v,
                              const std::vector<octomath::Vector3>& cube_template,
                              const unsigned int& current_array_idx,
                              GLfloat*** glArray);
    unsigned int setCubeColorHeightmap(const octomap::OcTreeVolume& v,
                                       const unsigned int& current_array_idx,
                                       GLfloat** glColorArray);
    unsigned int setCubeColorRGBA(const unsigned char& r, const unsigned char& g, 
                                  const unsigned char& b, const unsigned char& a,
                                  const unsigned int& current_array_idx,
                                  GLfloat** glColorArray);
      

    void initOctreeGridVis();


    GLfloat** m_occupiedThresArray;
    unsigned int m_occupiedThresSize;
    GLfloat** m_freeThresArray;
    unsigned int m_freeThresSize;
    GLfloat** m_occupiedArray;
    unsigned int m_occupiedSize;
    GLfloat** m_freeArray;
    unsigned int m_freeSize;
    GLfloat** m_selectionArray;
    unsigned int m_selectionSize;

    GLfloat* m_occupiedThresColorArray;
    GLfloat* m_occupiedColorArray;

    // TODO: put in its own drawer object!
    GLfloat* octree_grid_vertex_array;
    unsigned int octree_grid_vertex_size;

    std::list<octomap::OcTreeVolume> m_grid_voxels;

    bool m_drawOccupied;
    bool m_drawOcTreeGrid;
    bool m_drawFree;
    bool m_drawSelection;
    bool m_octree_grid_vis_initialized;
    bool m_displayAxes;

    unsigned int m_max_tree_depth;
    double m_alphaOccupied;

    octomap::pose6d origin;
    octomap::pose6d initial_origin;

    int map_id;
  };
}

#endif /* OCTREEDRAWER_H_ */