COctoMapVoxels.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "opengl-precomp.h"  // Precompiled header

#include <mrpt/opengl/COctoMapVoxels.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/utils/stl_serialization.h>
#include "opengl_internals.h"


using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::utils;
using namespace mrpt::math;
using namespace std;

IMPLEMENTS_SERIALIZABLE( COctoMapVoxels, CRenderizableDisplayList, mrpt::opengl )

/** Ctor */
COctoMapVoxels::COctoMapVoxels() :
        m_enable_lighting(false),
        m_enable_cube_transparency(true),
        m_showVoxelsAsPoints(false),
        m_showVoxelsAsPointsSize(3.0f),
        m_show_grids  (false),
        m_grid_width  (1.0f),
        m_grid_color  (0xE0,0xE0,0xE0, 0x90),
        m_visual_mode (COctoMapVoxels::COLOR_FROM_OCCUPANCY)
{
}

/** Clears everything */
void COctoMapVoxels::clear()
{
        m_voxel_sets.clear();
        m_grid_cubes.clear();

        CRenderizableDisplayList::notifyChange();
}

void COctoMapVoxels::setBoundingBox(const mrpt::math::TPoint3D &bb_min, const mrpt::math::TPoint3D &bb_max)
{
        m_bb_min = bb_min;
        m_bb_max = bb_max;
}


#if MRPT_HAS_OPENGL_GLUT

// See: http://www.songho.ca/opengl/gl_vertexarray.html

// cube ///////////////////////////////////////////////////////////////////////
//        v6----- v5
// +Z    /|      /|
// A    v1------v0|
// |    | |     | |
// |    | |v7---|-|v4   / -X
// |    |/      |/     /
// |    v2------v3    L +X
// ----------------------> +Y
//

const GLubyte grid_line_indices[] = {
        0,1, 1,2, 2,3, 3,0,
        4,5, 5,6, 6,7, 7,4,
        0,5, 1,6, 2,7, 3,4
        };

const GLubyte cube_indices[36]  = {
        0,1,2, 2,3,0,
        0,3,4, 4,5,0,
        0,5,6, 6,1,0,
        1,6,7, 7,2,1,
        7,4,3, 3,2,7,
        4,7,6, 6,5,4};

// normal array
const GLfloat normals_cube[3*6*4]  = {
  1, 0, 0,   1, 0, 0,   1, 0, 0,   0, 0, 0,   // v0,v1,v2,v3 (front)
  0, 1, 0,   0, 1, 0,   0, 1, 0,   0, 1, 0,   // v0,v3,v4,v5 (right)
  0, 0, 1,   0, 0, 1,   0, 0, 1,   0, 0, 1,   // v0,v5,v6,v1 (top)
  0,-1, 0,   0,-1, 0,   0,-1, 0,   0,-1, 0,   // v1,v6,v7,v2 (left)
  0, 0,-1,   0, 0,-1,   0, 0,-1,   0, 0,-1,   // v7,v4,v3,v2 (bottom)
 -1, 0, 0,  -1, 0, 0,  -1, 0, 0,  -1, 0, 0 }; // v4,v7,v6,v5 (back)

#endif

/*---------------------------------------------------------------
                                                        render
  ---------------------------------------------------------------*/
void   COctoMapVoxels::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT

        glEnableClientState(GL_VERTEX_ARRAY);

        // Draw grids ====================================
        if (m_show_grids)
        {
                glLineWidth(m_grid_width);
                checkOpenGLError();

                glDisable(GL_LIGHTING);  // Disable lights when drawing lines

                // Antialiasing:
        glPushAttrib( GL_COLOR_BUFFER_BIT | GL_LINE_BIT );
        glEnable(GL_LINE_SMOOTH);
                if ( m_grid_color.A != 255 )
                {
                        glEnable(GL_BLEND);
                        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
                }

                glColor4ub(m_grid_color.R,m_grid_color.G,m_grid_color.B,m_grid_color.A);

                const size_t nGrids = m_grid_cubes.size();
                for (size_t i=0;i<nGrids;i++)
                {
                        const TGridCube &c = m_grid_cubes[i];

                        const GLdouble vertices[8*3] = {
                                c.max.x,c.max.y,c.max.z,
                                c.max.x,c.min.y,c.max.z,
                                c.max.x,c.min.y,c.min.z,
                                c.max.x,c.max.y,c.min.z,
                                c.min.x,c.max.y,c.min.z,
                                c.min.x,c.max.y,c.max.z,
                                c.min.x,c.min.y,c.max.z,
                                c.min.x,c.min.y,c.min.z
                        };
                        glVertexPointer(3, GL_DOUBLE, 0, vertices);
                        glDrawElements(GL_LINES, sizeof(grid_line_indices)/sizeof(grid_line_indices[0]), GL_UNSIGNED_BYTE, grid_line_indices);
                }

                glEnable(GL_LIGHTING);  // Disable lights when drawing lines
                // End of antialiasing:
        glPopAttrib();
        }

        // Draw cubes ====================================
        if (!m_enable_lighting) glDisable(GL_LIGHTING);

    glEnableClientState(GL_NORMAL_ARRAY);

        glNormalPointer(GL_FLOAT, 0, normals_cube);

        if (m_enable_cube_transparency)
        {
                glEnable(GL_BLEND);
                glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        }

        if (m_showVoxelsAsPoints)
        {
                glPointSize(m_showVoxelsAsPointsSize);
            glBegin( GL_POINTS );
        }

        for (size_t i=0;i<m_voxel_sets.size();i++)
        {
                if (!m_voxel_sets[i].visible) continue;

                const std::vector<TVoxel> & voxels = m_voxel_sets[i].voxels;
                const size_t N = voxels.size();
                for (size_t j=0;j<N;j++)
                {
                        const mrpt::utils::TColor &vx_j_col = voxels[j].color;
                        glColor4ub(vx_j_col.R, vx_j_col.G, vx_j_col.B, vx_j_col.A);

                        const mrpt::math::TPoint3D &c = voxels[j].coords;
                        const double                L = voxels[j].side_length * 0.5;

                        if (!m_showVoxelsAsPoints)
                        {
                                // Render as cubes:
                                const GLdouble vertices[8*3] = {
                                        c.x+L,c.y+L,c.z+L,
                                        c.x+L,c.y-L,c.z+L,
                                        c.x+L,c.y-L,c.z-L,
                                        c.x+L,c.y+L,c.z-L,
                                        c.x-L,c.y+L,c.z-L,
                                        c.x-L,c.y+L,c.z+L,
                                        c.x-L,c.y-L,c.z+L,
                                        c.x-L,c.y-L,c.z-L
                                };
                                glVertexPointer(3, GL_DOUBLE, 0, vertices);
                                glDrawElements(GL_TRIANGLES, sizeof(cube_indices)/sizeof(cube_indices[0]), GL_UNSIGNED_BYTE, cube_indices);
                        }
                        else
                        {
                                // Render as simple points:
                                glVertex3f(c.x,c.y,c.z);
                        }
                }
        }


        if (m_showVoxelsAsPoints)
        {
                glEnd(); // of  GL_POINTS
        }

        if (m_enable_cube_transparency)
                glDisable(GL_BLEND);

    glDisableClientState(GL_NORMAL_ARRAY);

        if (!m_enable_lighting) glEnable(GL_LIGHTING);

        glDisableClientState(GL_VERTEX_ARRAY);
        checkOpenGLError();

#endif
}

DECLARE_CUSTOM_TTYPENAME(COctoMapVoxels::TInfoPerVoxelSet)
DECLARE_CUSTOM_TTYPENAME(COctoMapVoxels::TGridCube)
DECLARE_CUSTOM_TTYPENAME(COctoMapVoxels::TVoxel)

namespace mrpt{
        namespace utils
        {
                CStream & operator<<(mrpt::utils::CStream&out, const COctoMapVoxels::TInfoPerVoxelSet &a) {
                        out << a.visible << a.voxels;
                        return out;
                }
                CStream & operator>>(mrpt::utils::CStream&in, COctoMapVoxels::TInfoPerVoxelSet &a) {
                        in >> a.visible >> a.voxels;
                        return in;
                }

                CStream & operator<<(mrpt::utils::CStream&out, const COctoMapVoxels::TGridCube &a) {
                        out << a.min << a.max;
                        return out;
                }
                CStream & operator>>(mrpt::utils::CStream&in, COctoMapVoxels::TGridCube &a) {
                        in >> a.min >> a.max;
                        return in;
                }

                CStream & operator<<(mrpt::utils::CStream&out, const COctoMapVoxels::TVoxel &a) {
                        out << a.coords << a.side_length << a.color;
                        return out;
                }
                CStream & operator>>(mrpt::utils::CStream&in, COctoMapVoxels::TVoxel &a) {
                        in >> a.coords >> a.side_length >> a.color;
                        return in;
                }

}
}


/*---------------------------------------------------------------
   Implements the writing to a CStream capability of
     CSerializable objects
  ---------------------------------------------------------------*/
void  COctoMapVoxels::writeToStream(mrpt::utils::CStream &out,int *version) const
{
        if (version) *version=2;
        else
        {
                writeToStreamRender(out);

                out << m_voxel_sets
                        << m_grid_cubes
                        << m_bb_min << m_bb_max
                        << m_enable_lighting << m_showVoxelsAsPoints << m_showVoxelsAsPointsSize
                        << m_show_grids << m_grid_width << m_grid_color
                        << m_enable_cube_transparency // added in v1
                        << uint32_t(m_visual_mode); // added in v2
        }
}

/*---------------------------------------------------------------
        Implements the reading from a CStream capability of
                CSerializable objects
  ---------------------------------------------------------------*/
void  COctoMapVoxels::readFromStream(mrpt::utils::CStream &in,int version)
{
        switch(version)
        {
        case 0:
        case 1:
        case 2:
                {
                        readFromStreamRender(in);

                        in  >> m_voxel_sets
                                >> m_grid_cubes
                                >> m_bb_min >> m_bb_max
                                >> m_enable_lighting >> m_showVoxelsAsPoints >> m_showVoxelsAsPointsSize
                                >> m_show_grids >> m_grid_width >> m_grid_color;

                        if (version>=1)
                                in >> m_enable_cube_transparency;
                        else m_enable_cube_transparency = false;

                        if (version>=2)
                        {
                                uint32_t i;
                                in >> i;
                                m_visual_mode = static_cast<COctoMapVoxels::visualization_mode_t>(i);
                        }
                        else m_visual_mode = COctoMapVoxels::COLOR_FROM_OCCUPANCY;
                }
                break;
        default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)

        };

        CRenderizableDisplayList::notifyChange();
}

void COctoMapVoxels::getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
{
        bb_min = m_bb_min;
        bb_max = m_bb_max;

        // Convert to coordinates of my parent:
        m_pose.composePoint(bb_min, bb_min);
        m_pose.composePoint(bb_max, bb_max);
}

bool sort_voxels_z(const COctoMapVoxels::TVoxel &a, const COctoMapVoxels::TVoxel& b)
{
        return a.coords.z < b.coords.z;
}

void COctoMapVoxels::sort_voxels_by_z()
{
        for (size_t i=0;i<m_voxel_sets.size();i++)
        {
                std::sort( m_voxel_sets[i].voxels.begin(),m_voxel_sets[i].voxels.end(), &sort_voxels_z );
        }
}