CMesh.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/poses/CPose3D.h>
#include <mrpt/opengl/CMesh.h>
#include <mrpt/opengl/CSetOfTriangles.h>
#include <mrpt/utils/color_maps.h>
#include <mrpt/utils/CStream.h>

#include "opengl_internals.h"

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

IMPLEMENTS_SERIALIZABLE(CMesh, CRenderizableDisplayList, mrpt::opengl)

CMesh::CMesh(
        bool enableTransparency, float xMin, float xMax, float yMin, float yMax)
        : m_textureImage(0, 0),
          m_enableTransparency(enableTransparency),
          m_colorFromZ(false),
          m_isWireFrame(false),
          m_isImage(false),
          Z(0, 0),
          mask(0, 0),
          U(0, 0),
          V(0, 0),
          C(0, 0),
          C_r(0, 0),
          C_g(0, 0),
          C_b(0, 0),
          m_colorMap(mrpt::utils::cmHOT),
          m_modified_Z(true),
          m_modified_Image(false),
          xMin(xMin),
          xMax(xMax),
          yMin(yMin),
          yMax(yMax),
          trianglesUpToDate(false)
{
        m_color.A = 255;
        m_color.R = 0;
        m_color.G = 0;
        m_color.B = 150;
}

CMesh::~CMesh() {}
void CMesh::updateTriangles() const
{
        CRenderizableDisplayList::notifyChange();

        // Remember:
        /** List of triangles in the mesh */
        // mutable
        // std::vector<std::pair<CSetOfTriangles::TTriangle,TTriangleVertexIndices>
        // > actualMesh;
        /** The accumulated normals & counts for each vertex, so normals can be
         * averaged. */
        // mutable std::vector<std::pair<mrpt::math::TPoint3D,size_t> >
        // vertex_normals;

        const size_t cols = Z.getColCount();
        const size_t rows = Z.getRowCount();

        actualMesh.clear();
        if (cols == 0 && rows == 0) return;  // empty mesh

        ASSERT_(cols > 0 && rows > 0)
        ASSERT_(xMax > xMin && yMax > yMin)

        // we have 1 more row & col of vertices than of triangles:
        vertex_normals.assign(
                (1 + cols) * (1 + rows),
                std::pair<TPoint3D, size_t>(TPoint3D(0, 0, 0), 0));

        float cR[3], cG[3], cB[3], cA[3];
        cA[0] = cA[1] = cA[2] = m_color.A / 255.f;

        if ((m_colorFromZ) || (m_isImage))
        {
                updateColorsMatrix();
        }
        else
        {
                cR[0] = cR[1] = cR[2] = m_color.R / 255.f;
                cG[0] = cG[1] = cG[2] = m_color.G / 255.f;
                cB[0] = cB[1] = cB[2] = m_color.B / 255.f;
        }

        bool useMask = false;
        if (mask.getColCount() != 0 && mask.getRowCount() != 0)
        {
                ASSERT_(mask.getColCount() == cols && mask.getRowCount() == rows);
                useMask = true;
        }
        const float sCellX = (xMax - xMin) / (rows - 1);
        const float sCellY = (yMax - yMin) / (cols - 1);

        CSetOfTriangles::TTriangle tri;
        for (size_t iX = 0; iX < rows - 1; iX++)
                for (size_t iY = 0; iY < cols - 1; iY++)
                {
                        if (useMask && (!mask(iX, iY) || !mask(iX + 1, iY + 1))) continue;
                        tri.x[0] = xMin + iX * sCellX;
                        tri.y[0] = yMin + iY * sCellY;
                        tri.z[0] = Z(iX, iY);
                        tri.x[2] = tri.x[0] + sCellX;
                        tri.y[2] = tri.y[0] + sCellY;
                        tri.z[2] = Z(iX + 1, iY + 1);

                        // Vertex indices:
                        TTriangleVertexIndices tvi;
                        tvi.vind[0] = iX + rows * iY;
                        tvi.vind[2] = (iX + 1) + rows * (iY + 1);

                        // Each quadrangle has up to 2 triangles:
                        //  [0]
                        //   |
                        //   |
                        //  [1]--[2]
                        // Order: 0,1,2
                        if (!useMask || mask(iX + 1, iY))
                        {
                                tri.x[1] = tri.x[2];
                                tri.y[1] = tri.y[0];
                                tri.z[1] = Z(iX + 1, iY);
                                for (int i = 0; i < 3; i++)
                                        tri.a[i] = cA[i];  // Assign alpha channel

                                if (m_colorFromZ)
                                {
                                        colormap(
                                                m_colorMap, C(iX, iY), tri.r[0], tri.g[0], tri.b[0]);
                                        colormap(
                                                m_colorMap, C(iX + 1, iY), tri.r[1], tri.g[1],
                                                tri.b[1]);
                                        colormap(
                                                m_colorMap, C(iX + 1, iY + 1), tri.r[2], tri.g[2],
                                                tri.b[2]);
                                }
                                else if (m_isImage)
                                {
                                        if (m_textureImage.isColor())
                                        {
                                                tri.r[0] = tri.r[1] = tri.r[2] = C_r(iX, iY);
                                                tri.g[0] = tri.g[1] = tri.g[2] = C_g(iX, iY);
                                                tri.b[0] = tri.b[1] = tri.b[2] = C_b(iX, iY);
                                        }
                                        else
                                        {
                                                tri.r[0] = tri.r[1] = tri.r[2] = C(iX, iY);
                                                tri.g[0] = tri.g[1] = tri.g[2] = C(iX, iY);
                                                tri.b[0] = tri.b[1] = tri.b[2] = C(iX, iY);
                                        }
                                }
                                else
                                {
                                        tri.r[0] = tri.r[1] = tri.r[2] = m_color.R / 255.f;
                                        tri.g[0] = tri.g[1] = tri.g[2] = m_color.G / 255.f;
                                        tri.b[0] = tri.b[1] = tri.b[2] = m_color.B / 255.f;
                                }

                                // Compute normal of this triangle, and add it up to the 3
                                // neighboring vertices:
                                // A = P1 - P0, B = P2 - P0
                                float ax = tri.x[1] - tri.x[0];
                                float bx = tri.x[2] - tri.x[0];
                                float ay = tri.y[1] - tri.y[0];
                                float by = tri.y[2] - tri.y[0];
                                float az = tri.z[1] - tri.z[0];
                                float bz = tri.z[2] - tri.z[0];
                                const TPoint3D this_normal(
                                        ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);

                                // Vertex indices:
                                tvi.vind[1] = iX + 1 + rows * iY;

                                // Add triangle:
                                actualMesh.push_back(
                                        std::pair<CSetOfTriangles::TTriangle,
                                                          TTriangleVertexIndices>(tri, tvi));

                                // For averaging normals:
                                for (int k = 0; k < 3; k++)
                                {
                                        vertex_normals[tvi.vind[k]].first += this_normal;
                                        vertex_normals[tvi.vind[k]].second++;
                                }
                        }
                        // 2:
                        //  [0]--[1->2]
                        //     \  |
                        //       \|
                        //       [2->1]
                        // Order: 0,2,1
                        if (!useMask || mask(iX, iY + 1))
                        {
                                tri.x[1] = tri.x[2];
                                tri.y[1] = tri.y[2];
                                tri.z[1] = tri.z[2];

                                tri.x[2] = tri.x[0];
                                // tri.y[2]=tri.y[1];
                                tri.z[2] = Z(iX, iY + 1);
                                if (m_colorFromZ)
                                {
                                        colormap(
                                                m_colorMap, C(iX, iY), tri.r[0], tri.g[0], tri.b[0]);
                                        colormap(
                                                m_colorMap, C(iX + 1, iY + 1), tri.r[1], tri.g[1],
                                                tri.b[1]);
                                        colormap(
                                                m_colorMap, C(iX, iY + 1), tri.r[2], tri.g[2],
                                                tri.b[2]);
                                }
                                else if (m_isImage)
                                {
                                        if (m_textureImage.isColor())
                                        {
                                                tri.r[0] = tri.r[1] = tri.r[2] = C_r(iX, iY);
                                                tri.g[0] = tri.g[1] = tri.g[2] = C_g(iX, iY);
                                                tri.b[0] = tri.b[1] = tri.b[2] = C_b(iX, iY);
                                        }
                                        else
                                        {
                                                tri.r[0] = tri.r[1] = tri.r[2] = C(iX, iY);
                                                tri.g[0] = tri.g[1] = tri.g[2] = C(iX, iY);
                                                tri.b[0] = tri.b[1] = tri.b[2] = C(iX, iY);
                                        }
                                }
                                else
                                {
                                        tri.r[0] = tri.r[1] = tri.r[2] = m_color.R / 255.f;
                                        tri.g[0] = tri.g[1] = tri.g[2] = m_color.G / 255.f;
                                        tri.b[0] = tri.b[1] = tri.b[2] = m_color.B / 255.f;
                                }

                                // Compute normal of this triangle, and add it up to the 3
                                // neighboring vertices:
                                // A = P1 - P0, B = P2 - P0
                                float ax = tri.x[1] - tri.x[0];
                                float bx = tri.x[2] - tri.x[0];
                                float ay = tri.y[1] - tri.y[0];
                                float by = tri.y[2] - tri.y[0];
                                float az = tri.z[1] - tri.z[0];
                                float bz = tri.z[2] - tri.z[0];
                                const TPoint3D this_normal(
                                        ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);

                                // Vertex indices:
                                tvi.vind[1] = tvi.vind[2];
                                tvi.vind[2] = iX + rows * (iY + 1);

                                // Add triangle:
                                actualMesh.push_back(
                                        std::pair<CSetOfTriangles::TTriangle,
                                                          TTriangleVertexIndices>(tri, tvi));

                                // For averaging normals:
                                for (int k = 0; k < 3; k++)
                                {
                                        vertex_normals[tvi.vind[k]].first += this_normal;
                                        vertex_normals[tvi.vind[k]].second++;
                                }
                        }
                }

        // Average normals:
        for (size_t i = 0; i < vertex_normals.size(); i++)
        {
                const size_t N = vertex_normals[i].second;
                if (N > 0) vertex_normals[i].first *= 1.0 / N;
        }

        trianglesUpToDate = true;
        polygonsUpToDate = false;
}

/*---------------------------------------------------------------
                                                        render
  ---------------------------------------------------------------*/
void CMesh::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
        if (m_enableTransparency)
        {
                glDisable(GL_DEPTH_TEST);
                glEnable(GL_BLEND);
                glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        }
        else
        {
                glEnable(GL_DEPTH_TEST);
                glDisable(GL_BLEND);
        }
        glEnable(GL_NORMALIZE);  // So the GPU normalizes the normals instead of
        // doing it in the CPU
        glEnable(GL_COLOR_MATERIAL);
        glShadeModel(GL_SMOOTH);
        if (!trianglesUpToDate) updateTriangles();
        if (!m_isWireFrame) glBegin(GL_TRIANGLES);
        for (size_t i = 0; i < actualMesh.size(); i++)
        {
                const CSetOfTriangles::TTriangle& t = actualMesh[i].first;
                const TTriangleVertexIndices& tvi = actualMesh[i].second;

                if (m_isWireFrame)
                {
                        glDisable(GL_LIGHTING);  // Disable lights when drawing lines
                        glBegin(GL_LINE_LOOP);
                }
                for (int i = 0; i < 3; i++)
                {
                        const mrpt::math::TPoint3D& n = vertex_normals[tvi.vind[i]].first;
                        glNormal3f(n.x, n.y, n.z);
                        glColor4f(t.r[i], t.g[i], t.b[i], t.a[i]);
                        glVertex3f(t.x[i], t.y[i], t.z[i]);
                }
                if (m_isWireFrame)
                {
                        glEnd();
                        glEnable(GL_LIGHTING);
                }
        }
        if (!m_isWireFrame) glEnd();
        glDisable(GL_BLEND);
        glDisable(GL_NORMALIZE);
        glEnable(GL_DEPTH_TEST);
#endif
}

/*---------------------------------------------------------------
                                                        assignImage
  ---------------------------------------------------------------*/
void CMesh::assignImage(const CImage& img)
{
        MRPT_START

        // Make a copy:
        m_textureImage = img;

        // Delete content in Z
        Z.setSize(img.getHeight(), img.getWidth());
        Z.assign(0);

        m_modified_Image = true;
        m_enableTransparency = false;
        m_colorFromZ = false;
        m_isImage = true;
        trianglesUpToDate = false;

        CRenderizableDisplayList::notifyChange();

        MRPT_END
}

/*---------------------------------------------------------------
                                                        assign Image and Z
  ---------------------------------------------------------------*/
void CMesh::assignImageAndZ(
        const CImage& img, const mrpt::math::CMatrixTemplateNumeric<float>& in_Z)
{
        MRPT_START

        ASSERT_(
                (img.getWidth() == static_cast<size_t>(in_Z.cols())) &&
                (img.getHeight() == static_cast<size_t>(in_Z.rows())))

        Z = in_Z;

        // Make a copy:
        m_textureImage = img;

        m_modified_Image = true;
        m_enableTransparency = false;
        m_colorFromZ = false;
        m_isImage = true;
        trianglesUpToDate = false;

        CRenderizableDisplayList::notifyChange();

        MRPT_END
}

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

                // Version 0:
                out << m_textureImage;
                out << xMin << xMax << yMin << yMax;
                out << Z << U << V
                        << mask;  // We don't need to serialize C, it's computed
                out << m_enableTransparency;
                out << m_colorFromZ;
                // new in v1
                out << m_isWireFrame;
                out << int16_t(m_colorMap);
        }
}

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

                        in >> m_textureImage;

                        in >> xMin;
                        in >> xMax;
                        in >> yMin;
                        in >> yMax;

                        in >> Z >> U >> V >> mask;
                        in >> m_enableTransparency;
                        in >> m_colorFromZ;

                        if (version >= 1)
                        {
                                in >> m_isWireFrame;
                                int16_t i;
                                in >> i;
                                m_colorMap = TColormap(i);
                        }
                        else
                                m_isWireFrame = false;

                        m_modified_Z = true;
                }
                        trianglesUpToDate = false;
                        break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
        trianglesUpToDate = false;
        CRenderizableDisplayList::notifyChange();
}

void CMesh::updateColorsMatrix() const
{
        if ((!m_modified_Z) && (!m_modified_Image)) return;

        CRenderizableDisplayList::notifyChange();

        if (m_isImage)
        {
                const size_t cols = m_textureImage.getWidth();
                const size_t rows = m_textureImage.getHeight();

                if ((cols != Z.getColCount()) || (rows != Z.getRowCount()))
                        printf("\nTexture Image and Z sizes have to be equal");

                else if (m_textureImage.isColor())
                {
                        C_r.setSize(rows, cols);
                        C_g.setSize(rows, cols);
                        C_b.setSize(rows, cols);
                        m_textureImage.getAsRGBMatrices(C_r, C_g, C_b);
                }
                else
                {
                        C.setSize(rows, cols);
                        m_textureImage.getAsMatrix(C);
                }
        }
        else
        {
                const size_t cols = Z.getColCount();
                const size_t rows = Z.getRowCount();
                C.setSize(rows, cols);

                // Color is proportional to height:
                C = Z;

                // If mask is empty -> Normalize the whole mesh
                if (mask.empty()) C.normalize(0.01f, 0.99f);

                // Else -> Normalize color ignoring masked-out cells:
                else
                {
                        float val_max = -std::numeric_limits<float>::max(),
                                  val_min = std::numeric_limits<float>::max();
                        bool any_valid = false;

                        for (size_t c = 0; c < cols; c++)
                                for (size_t r = 0; r < rows; r++)
                                {
                                        if (!mask(r, c)) continue;
                                        any_valid = true;
                                        const float val = C(r, c);
                                        mrpt::utils::keep_max(val_max, val);
                                        mrpt::utils::keep_min(val_min, val);
                                }

                        if (any_valid)
                        {
                                float minMaxDelta = val_max - val_min;
                                if (minMaxDelta == 0) minMaxDelta = 1;
                                const float minMaxDelta_ = 1.0f / minMaxDelta;
                                C.array() = (C.array() - val_min) * minMaxDelta_;
                        }
                }
        }

        m_modified_Image = false;
        m_modified_Z = false;
        trianglesUpToDate = false;
}

void CMesh::setZ(const mrpt::math::CMatrixTemplateNumeric<float>& in_Z)
{
        Z = in_Z;
        m_modified_Z = true;
        trianglesUpToDate = false;

        // Delete previously loaded images
        m_isImage = false;

        CRenderizableDisplayList::notifyChange();
}

void CMesh::setMask(const mrpt::math::CMatrixTemplateNumeric<float>& in_mask)
{
        mask = in_mask;
        trianglesUpToDate = false;
        CRenderizableDisplayList::notifyChange();
}

void CMesh::setUV(
        const mrpt::math::CMatrixTemplateNumeric<float>& in_U,
        const mrpt::math::CMatrixTemplateNumeric<float>& in_V)
{
        U = in_U;
        V = in_V;
        CRenderizableDisplayList::notifyChange();
}

bool CMesh::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
{
        if (!trianglesUpToDate || !polygonsUpToDate) updatePolygons();
        return mrpt::math::traceRay(tmpPolys, o - this->m_pose, dist);
}

static math::TPolygon3D tmpPoly(3);
mrpt::math::TPolygonWithPlane createPolygonFromTriangle(
        const std::pair<CSetOfTriangles::TTriangle, CMesh::TTriangleVertexIndices>&
                p)
{
        const CSetOfTriangles::TTriangle& t = p.first;
        for (size_t i = 0; i < 3; i++)
        {
                tmpPoly[i].x = t.x[i];
                tmpPoly[i].y = t.y[i];
                tmpPoly[i].z = t.z[i];
        }
        return mrpt::math::TPolygonWithPlane(tmpPoly);
}

void CMesh::updatePolygons() const
{
        if (!trianglesUpToDate) updateTriangles();
        size_t N = actualMesh.size();
        tmpPolys.resize(N);
        transform(
                actualMesh.begin(), actualMesh.end(), tmpPolys.begin(),
                createPolygonFromTriangle);
        polygonsUpToDate = true;
        CRenderizableDisplayList::notifyChange();
}

void CMesh::getBoundingBox(
        mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
{
        bb_min.x = xMin;
        bb_min.y = yMin;
        bb_min.z = Z.minCoeff();

        bb_max.x = xMax;
        bb_max.y = yMax;
        bb_max.z = Z.maxCoeff();

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