CSetOfTriangles.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/CSetOfTriangles.h>
#include "opengl_internals.h"
#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/utils/CStream.h>

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

IMPLEMENTS_SERIALIZABLE(CSetOfTriangles, CRenderizableDisplayList, mrpt::opengl)

/*---------------------------------------------------------------
                                                        render
  ---------------------------------------------------------------*/
void CSetOfTriangles::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);
        }

        vector<TTriangle>::const_iterator it;

        glEnable(GL_NORMALIZE);  // Normalize normals
        glBegin(GL_TRIANGLES);

        for (it = m_triangles.begin(); it != m_triangles.end(); ++it)
        {
                // Compute the normal vector:
                // ---------------------------------
                float ax = it->x[1] - it->x[0];
                float ay = it->y[1] - it->y[0];
                float az = it->z[1] - it->z[0];

                float bx = it->x[2] - it->x[0];
                float by = it->y[2] - it->y[0];
                float bz = it->z[2] - it->z[0];

                glNormal3f(ay * bz - az * by, -ax * bz + az * bx, ax * by - ay * bx);

                glColor4f(it->r[0], it->g[0], it->b[0], it->a[0]);
                glVertex3f(it->x[0], it->y[0], it->z[0]);

                glColor4f(it->r[1], it->g[1], it->b[1], it->a[1]);
                glVertex3f(it->x[1], it->y[1], it->z[1]);

                glColor4f(it->r[2], it->g[2], it->b[2], it->a[2]);
                glVertex3f(it->x[2], it->y[2], it->z[2]);
        }

        glEnd();
        glDisable(GL_NORMALIZE);

        glDisable(GL_BLEND);
#endif
}

static void triangle_writeToStream(
        mrpt::utils::CStream& o, const CSetOfTriangles::TTriangle& t)
{
        o.WriteBufferFixEndianness(t.x, 3);
        o.WriteBufferFixEndianness(t.y, 3);
        o.WriteBufferFixEndianness(t.z, 3);

        o.WriteBufferFixEndianness(t.r, 3);
        o.WriteBufferFixEndianness(t.g, 3);
        o.WriteBufferFixEndianness(t.b, 3);
        o.WriteBufferFixEndianness(t.a, 3);
}
static void triangle_readFromStream(
        mrpt::utils::CStream& i, CSetOfTriangles::TTriangle& t)
{
        i.ReadBufferFixEndianness(t.x, 3);
        i.ReadBufferFixEndianness(t.y, 3);
        i.ReadBufferFixEndianness(t.z, 3);

        i.ReadBufferFixEndianness(t.r, 3);
        i.ReadBufferFixEndianness(t.g, 3);
        i.ReadBufferFixEndianness(t.b, 3);
        i.ReadBufferFixEndianness(t.a, 3);
}

/*---------------------------------------------------------------
   Implements the writing to a CStream capability of
         CSerializable objects
  ---------------------------------------------------------------*/
void CSetOfTriangles::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 1;
        else
        {
                writeToStreamRender(out);
                uint32_t n = (uint32_t)m_triangles.size();
                out << n;
                for (size_t i = 0; i < n; i++)
                        triangle_writeToStream(out, m_triangles[i]);

                // Version 1:
                out << m_enableTransparency;
        }
}

/*---------------------------------------------------------------
        Implements the reading from a CStream capability of
                CSerializable objects
  ---------------------------------------------------------------*/
void CSetOfTriangles::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                case 1:
                {
                        readFromStreamRender(in);
                        uint32_t n;
                        in >> n;
                        m_triangles.assign(n, TTriangle());
                        for (size_t i = 0; i < n; i++)
                                triangle_readFromStream(in, m_triangles[i]);

                        if (version >= 1)
                                in >> m_enableTransparency;
                        else
                                m_enableTransparency = true;
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
        polygonsUpToDate = false;
        CRenderizableDisplayList::notifyChange();
}

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

// Helper function. Given two 2D points (y1,z1) and (y2,z2), returns three
// coefficients A, B and C so that both points
// verify Ay+Bz+C=0
// returns true if the coefficients have actually been calculated
/*
inline bool lineCoefs(const float &y1,const float &z1,const float &y2,const
float &z2,float coefs[3])       {
        if ((y1==y2)&(z1==z2)) return false;    //Both points are the same
        if (y1==y2)     {
                //Equation is y-y1=0
                coefs[0]=1;
                coefs[1]=0;
                coefs[2]=-y1;
                return true;
        }       else    {
                //Equation is:
                // z1 - z2        /z2 - z1       \ .
                // -------y + z + |-------y1 - z1| = 0
                // y2 - y1        \y2 - y1       /
                coefs[0]=(z1-z2)/(y2-y1);
                coefs[1]=1;
                coefs[2]=((z2-z1)/(y2-y1))*y1-z1;
                return true;
        }
}
*/
/*
bool CSetOfTriangles::traceRayTriangle(const mrpt::poses::CPose3D &transf,double
&dist,const float xb[3],const float yb[3],const float zb[3])    {
        //Computation of the actual coordinates in the beam's system.
        float x[3];
        float y[3];
        float z[3];
        for (int i=0;i<3;i++) transf.composePoint(xb[i],yb[i],zb[i],x[i],y[i],z[i]);

        //If the triangle is parallel to the beam, no collision is posible.
        //The triangle is parallel to the beam if the projection of the triangle to
the YZ plane results in a line.
        float lCoefs[3];
        if (!lineCoefs(y[0],z[0],y[1],z[1],lCoefs)) return false;
        else if (lCoefs[0]*y[2]+lCoefs[1]*z[2]+lCoefs[2]==0) return false;
        //Basic sign check
        if (x[0]<0&&x[1]<0&&x[2]<0) return false;
        if (y[0]<0&&y[1]<0&&y[2]<0) return false;
        if (z[0]<0&&z[1]<0&&z[2]<0) return false;
        if (y[0]>0&&y[1]>0&&y[2]>0) return false;
        if (z[0]>0&&z[1]>0&&z[2]>0) return false;
        //Let M be the following matrix:
        //  /p1\ /x1 y1 z1\ .
        //M=|p2|=|x2 y2 z2|
        //  \p3/ \x3 y3 z3/
        //If M has rank 3, then (p1,p2,p3) conform a plane which does not contain
the origin (0,0,0).
        //If M has rank 2, then (p1,p2,p3) may conform either a line or a plane
which contains the origin.
        //If M has a lesser rank, then (p1,p2,p3) do not conform a plane.
        //Let N be the following matrix:
        //N=/p2-p1\ = /x1 y1 z1\ .
        //  \p3-p1/   \x3 y3 z3/
        //Given that the rank of M is 2, if the rank of N is still 2 then (p1,p2,p3)
conform a plane; either, a line.
        float mat[9];
        for (int i=0;i<3;i++)   {
                mat[3*i]=x[i];
                mat[3*i+1]=y[i];
                mat[3*i+2]=z[i];
        }
        CMatrixTemplateNumeric<float> M=CMatrixTemplateNumeric<float>(3,3,mat);
        float d2=0;
        float mat2[6];
        switch (M.rank())       {
                case 3:
                        //M's rank is 3, so the triangle is inside a plane which doesn't
pass through (0,0,0).
                        //This plane's equation is Ax+By+Cz+1=0. Since the point we're
searching for verifies y=0 and z=0, we
                        //only need to compute A (x=-1/A). We do this using Cramer's method.
                        for (int i=0;i<9;i+=3) mat[i]=1;
                        d2=(CMatrixTemplateNumeric<float>(3,3,mat)).det();
                        if (d2==0) return false;
                        else dist=M.det()/d2;
                        break;
                case 2:
                        //if N's rank is 2, the triangle is inside a plane containing
(0,0,0).
                        //Otherwise, (p1,p2,p3) don't conform a plane.
                        for (int i=0;i<2;i++)   {
                                mat2[3*i]=x[i+1]-x[0];
                                mat2[3*i+1]=y[i+1]-y[0];
                                mat2[3*i+2]=z[i+1]-z[0];
                        }
                        if (CMatrixTemplateNumeric<float>(2,3,mat2).rank()==2) dist=0;
                        else return false;
                        break;
                default:
                        return false;
        }
        if (dist<0) return false;
        //We've already determined the collision point between the beam and the
plane, but we need to check if this
        //point is actually inside the triangle. We do this by projecting the scene
into a <x=constant> plane, so
        //that the triangle is defined by three 2D lines and the beam is the point
(y,z)=(0,0).

        //For each pair of points, we compute the line that they conform, and then
we check if the other point's
        //sign in that line's equation equals that of the origin (that is, both
points are on the same side of the line).
        //If this holds for each one of the three possible combinations, then the
point is inside the triangle.
        //Furthermore, if any of the three equations verify f(0,0)=0, then the point
is in the verge of the line, which
        //is considered as being inside. Note, whichever is the case, that
f(0,0)=lCoefs[2].

        //lineCoefs already contains the coefficients for the first line.
        if (lCoefs[2]==0) return true;
        else if (((lCoefs[0]*y[2]+lCoefs[1]*z[2]+lCoefs[2])>0)!=(lCoefs[2]>0))
return false;
        lineCoefs(y[0],z[0],y[2],z[2],lCoefs);
        if (lCoefs[2]==0) return true;
        else if (((lCoefs[0]*y[1]+lCoefs[1]*z[1]+lCoefs[2])>0)!=(lCoefs[2]>0))
return false;
        lineCoefs(y[1],z[1],y[2],z[2],lCoefs);
        if (lCoefs[2]==0) return true;
        else return ((lCoefs[0]*y[0]+lCoefs[1]*z[0]+lCoefs[2])>0)==(lCoefs[2]>0);
}
*/

CRenderizable& CSetOfTriangles::setColor_u8(const mrpt::utils::TColor& c)
{
        CRenderizableDisplayList::notifyChange();
        m_color = c;
        mrpt::utils::TColorf col(c);
        for (std::vector<TTriangle>::iterator it = m_triangles.begin();
                 it != m_triangles.end(); ++it)
                for (size_t i = 0; i < 3; i++)
                {
                        it->r[i] = col.R;
                        it->g[i] = col.G;
                        it->b[i] = col.B;
                        it->a[i] = col.A;
                }
        return *this;
}

CRenderizable& CSetOfTriangles::setColorR_u8(const uint8_t r)
{
        CRenderizableDisplayList::notifyChange();
        m_color.R = r;
        const float col = r / 255.f;
        for (std::vector<TTriangle>::iterator it = m_triangles.begin();
                 it != m_triangles.end(); ++it)
                for (size_t i = 0; i < 3; i++) it->r[i] = col;
        return *this;
}

CRenderizable& CSetOfTriangles::setColorG_u8(const uint8_t g)
{
        CRenderizableDisplayList::notifyChange();
        m_color.G = g;
        const float col = g / 255.f;
        for (std::vector<TTriangle>::iterator it = m_triangles.begin();
                 it != m_triangles.end(); ++it)
                for (size_t i = 0; i < 3; i++) it->g[i] = col;
        return *this;
}

CRenderizable& CSetOfTriangles::setColorB_u8(const uint8_t b)
{
        CRenderizableDisplayList::notifyChange();
        m_color.B = b;
        const float col = b / 255.f;
        for (std::vector<TTriangle>::iterator it = m_triangles.begin();
                 it != m_triangles.end(); ++it)
                for (size_t i = 0; i < 3; i++) it->b[i] = col;
        return *this;
}

CRenderizable& CSetOfTriangles::setColorA_u8(const uint8_t a)
{
        CRenderizableDisplayList::notifyChange();
        m_color.A = a;
        const float col = a / 255.f;
        for (std::vector<TTriangle>::iterator it = m_triangles.begin();
                 it != m_triangles.end(); ++it)
                for (size_t i = 0; i < 3; i++) it->a[i] = col;
        return *this;
}

void CSetOfTriangles::getPolygons(
        std::vector<mrpt::math::TPolygon3D>& polys) const
{
        if (!polygonsUpToDate) updatePolygons();
        size_t N = tmpPolygons.size();
        for (size_t i = 0; i < N; i++) polys[i] = tmpPolygons[i].poly;
}

void CSetOfTriangles::updatePolygons() const
{
        TPolygon3D tmp(3);
        size_t N = m_triangles.size();
        tmpPolygons.resize(N);
        for (size_t i = 0; i < N; i++)
                for (size_t j = 0; j < 3; j++)
                {
                        const TTriangle& t = m_triangles[i];
                        tmp[j].x = t.x[j];
                        tmp[j].y = t.y[j];
                        tmp[j].z = t.z[j];
                        tmpPolygons[i] = tmp;
                }
        polygonsUpToDate = true;
        CRenderizableDisplayList::notifyChange();
}

void CSetOfTriangles::getBoundingBox(
        mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
{
        bb_min = mrpt::math::TPoint3D(
                std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
                std::numeric_limits<double>::max());
        bb_max = mrpt::math::TPoint3D(
                -std::numeric_limits<double>::max(),
                -std::numeric_limits<double>::max(),
                -std::numeric_limits<double>::max());

        for (size_t i = 0; i < m_triangles.size(); i++)
        {
                const TTriangle& t = m_triangles[i];

                keep_min(bb_min.x, t.x[0]);
                keep_max(bb_max.x, t.x[0]);
                keep_min(bb_min.y, t.y[0]);
                keep_max(bb_max.y, t.y[0]);
                keep_min(bb_min.z, t.z[0]);
                keep_max(bb_max.z, t.z[0]);

                keep_min(bb_min.x, t.x[1]);
                keep_max(bb_max.x, t.x[1]);
                keep_min(bb_min.y, t.y[1]);
                keep_max(bb_max.y, t.y[1]);
                keep_min(bb_min.z, t.z[1]);
                keep_max(bb_max.z, t.z[1]);

                keep_min(bb_min.x, t.x[2]);
                keep_max(bb_max.x, t.x[2]);
                keep_min(bb_min.y, t.y[2]);
                keep_max(bb_max.y, t.y[2]);
                keep_min(bb_min.z, t.z[2]);
                keep_max(bb_max.z, t.z[2]);
        }

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

void CSetOfTriangles::insertTriangles(const CSetOfTriangles::Ptr& p)
{
        reserve(m_triangles.size() + p->m_triangles.size());
        m_triangles.insert(
                m_triangles.end(), p->m_triangles.begin(), p->m_triangles.end());
        polygonsUpToDate = false;
        CRenderizableDisplayList::notifyChange();
}