CAxis.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/CAxis.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/system/os.h>
#include <mrpt/opengl/gl_utils.h>

#include "opengl_internals.h"

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

IMPLEMENTS_SERIALIZABLE(CAxis, CRenderizableDisplayList, mrpt::opengl)

CAxis::CAxis(
        float xmin, float ymin, float zmin, float xmax, float ymax, float zmax,
        float frecuency, float lineWidth, bool marks)
        : m_xmin(xmin),
          m_ymin(ymin),
          m_zmin(zmin),
          m_xmax(xmax),
          m_ymax(ymax),
          m_zmax(zmax),
          m_frequency(frecuency),
          m_lineWidth(lineWidth),
          m_textScale(0.25f)
{
        for (int i = 0; i < 3; i++) m_marks[i] = marks;

        // x:180, 0, 90
        m_textRot[0][0] = 180.f;
        m_textRot[0][1] = 0.f;
        m_textRot[0][2] = 90.f;
        // y:90, 0, 90
        m_textRot[1][0] = 90.f;
        m_textRot[1][1] = 0.f;
        m_textRot[1][2] = 90.f;
        // z:180, 0, 90
        m_textRot[2][0] = 180.f;
        m_textRot[2][1] = 0.f;
        m_textRot[2][2] = 90.f;
}

CAxis::Ptr CAxis::Create(
        float xmin, float ymin, float zmin, float xmax, float ymax, float zmax,
        float frecuency, float lineWidth, bool marks)
{
        return CAxis::Ptr(
                new CAxis(
                        xmin, ymin, zmin, xmax, ymax, zmax, frecuency, lineWidth, marks));
}

void CAxis::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
        MRPT_START
        glDisable(GL_LIGHTING);

        glEnable(GL_BLEND);
        checkOpenGLError();
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        checkOpenGLError();

        ASSERT_(m_frequency >= 0);

        glLineWidth(m_lineWidth);
        checkOpenGLError();
        glBegin(GL_LINES);
        glColor4ub(m_color.R, m_color.G, m_color.B, m_color.A);
        // X axis
        glVertex3f(m_xmin, 0.0f, 0.0f);
        glVertex3f(m_xmax, 0.0f, 0.0f);
        // Y axis
        glVertex3f(0.0f, m_ymin, 0.0f);
        glVertex3f(0.0f, m_ymax, 0.0f);
        // Z axis
        glVertex3f(0.0f, 0.0f, m_zmin);
        glVertex3f(0.0f, 0.0f, m_zmax);

        glEnd();
        checkOpenGLError();

        glLineWidth(1.0f);
        checkOpenGLError();

        glDisable(GL_BLEND);
        checkOpenGLError();

        // Draw the "tick marks":
        char n[50];
        if (m_marks[0])
        {
                // X axis
                glPushMatrix();
                glTranslatef(m_xmin, .0f, .05f);
                for (float i = m_xmin; i <= m_xmax; i = i + m_frequency)
                {
                        os::sprintf(n, 50, "%.02f", i);
                        glPushMatrix();
                        glRotatef(m_textRot[0][0], 0, 0, 1);
                        glRotatef(m_textRot[0][1], 0, 1, 0);
                        glRotatef(m_textRot[0][2], 1, 0, 0);
                        gl_utils::glDrawText(n, m_textScale, mrpt::opengl::FILL);
                        glPopMatrix();
                        glTranslatef(m_frequency, 0, 0);
                }

                glPopMatrix();
                glPushMatrix();
                glTranslatef(m_xmax + 1.0f * m_frequency, 0, 0);
                glRotatef(m_textRot[0][0], 0, 0, 1);
                glRotatef(m_textRot[0][1], 0, 1, 0);
                glRotatef(m_textRot[0][2], 1, 0, 0);
                gl_utils::glDrawText("+X", m_textScale * 1.2, mrpt::opengl::NICE);
                glPopMatrix();
        }
        if (m_marks[1])
        {
                // Y axis
                glPushMatrix();
                glTranslatef(.0f, m_ymin, .05f);
                for (float i = m_ymin; i <= m_ymax; i = i + m_frequency)
                {
                        if (std::abs(i) > 1e-4)
                        {  // Dont draw the "0" more than once
                                os::sprintf(n, 50, "%.02f", i);
                                glPushMatrix();
                                glRotatef(m_textRot[1][0], 0, 0, 1);
                                glRotatef(m_textRot[1][1], 0, 1, 0);
                                glRotatef(m_textRot[1][2], 1, 0, 0);
                                gl_utils::glDrawText(n, m_textScale, mrpt::opengl::FILL);
                                glPopMatrix();
                        }
                        glTranslatef(0, m_frequency, 0);
                }

                glPopMatrix();
                glPushMatrix();
                glTranslatef(0, m_ymax + .5f * m_frequency, 0);
                glRotatef(m_textRot[1][0], 0, 0, 1);
                glRotatef(m_textRot[1][1], 0, 1, 0);
                glRotatef(m_textRot[1][2], 1, 0, 0);
                gl_utils::glDrawText("+Y", m_textScale * 1.2, mrpt::opengl::NICE);
                glPopMatrix();
        }
        if (m_marks[2])
        {
                // Z axis
                glPushMatrix();
                glTranslatef(.0f, .0f, m_zmin);
                for (float i = m_zmin; i <= m_zmax; i = i + m_frequency)
                {
                        if (std::abs(i) > 1e-4)
                        {  // Dont draw the "0" more than once
                                glPushMatrix();
                                glRotatef(m_textRot[2][0], 0, 0, 1);
                                glRotatef(m_textRot[2][1], 0, 1, 0);
                                glRotatef(m_textRot[2][2], 1, 0, 0);
                                os::sprintf(n, 50, "%.02f", i);
                                gl_utils::glDrawText(n, m_textScale, mrpt::opengl::FILL);
                                glPopMatrix();
                        }
                        glTranslatef(0, 0, m_frequency);
                }

                glPopMatrix();
                glPushMatrix();
                glTranslatef(0, 0, m_zmax + 0.5f * m_frequency);
                glRotatef(m_textRot[2][0], 0, 0, 1);
                glRotatef(m_textRot[2][1], 0, 1, 0);
                glRotatef(m_textRot[2][2], 1, 0, 0);
                gl_utils::glDrawText("+Z", m_textScale * 1.2, mrpt::opengl::NICE);
                glPopMatrix();
        }

        glEnable(GL_LIGHTING);
        MRPT_END
/*******************************************************/
#endif
}

/*---------------------------------------------------------------
   Implements the writing to a CStream capability of
         CSerializable objects
  ---------------------------------------------------------------*/
void CAxis::writeToStream(mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 1;
        else
        {
                writeToStreamRender(out);
                out << m_xmin << m_ymin << m_zmin;
                out << m_xmax << m_ymax << m_zmax;
                out << m_frequency << m_lineWidth;
                // v1:
                out << m_marks[0] << m_marks[1] << m_marks[2] << m_textScale;
                for (int i = 0; i < 3; i++)
                        for (int j = 0; j < 3; j++) out << m_textRot[i][j];
        }
}

/*---------------------------------------------------------------
        Implements the reading from a CStream capability of
                CSerializable objects
  ---------------------------------------------------------------*/
void CAxis::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                case 1:
                {
                        readFromStreamRender(in);
                        in >> m_xmin >> m_ymin >> m_zmin;
                        in >> m_xmax >> m_ymax >> m_zmax;
                        in >> m_frequency >> m_lineWidth;
                        if (version >= 1)
                        {
                                in >> m_marks[0] >> m_marks[1] >> m_marks[2] >> m_textScale;
                                for (int i = 0; i < 3; i++)
                                        for (int j = 0; j < 3; j++) in >> m_textRot[i][j];
                        }
                        else
                        {
                                bool v;
                                in >> v;
                                for (int i = 0; i < 3; i++) m_marks[i] = v;
                                m_textScale = 0.25f;
                        }
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
        CRenderizableDisplayList::notifyChange();
}

void CAxis::getBoundingBox(
        mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
{
        bb_min.x = m_xmin;
        bb_min.y = m_ymin;
        bb_min.z = m_zmin;

        bb_max.x = m_xmax;
        bb_max.y = m_ymax;
        bb_max.z = m_zmax;

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

void CAxis::setFrequency(float f)
{
        ASSERT_(f > 0);
        m_frequency = f;
        CRenderizableDisplayList::notifyChange();
}
float CAxis::getFrequency() const { return m_frequency; }
void CAxis::setLineWidth(float w)
{
        m_lineWidth = w;
        CRenderizableDisplayList::notifyChange();
}
float CAxis::getLineWidth() const { return m_lineWidth; }
void CAxis::enableTickMarks(bool v)
{
        for (int i = 0; i < 3; i++) m_marks[i] = v;
        CRenderizableDisplayList::notifyChange();
}
void CAxis::enableTickMarks(bool show_x, bool show_y, bool show_z)
{
        m_marks[0] = show_x;
        m_marks[1] = show_y;
        m_marks[2] = show_z;
        CRenderizableDisplayList::notifyChange();
}
void CAxis::setTextScale(float f)
{
        ASSERT_(f > 0);
        m_textScale = f;
        CRenderizableDisplayList::notifyChange();
}
float CAxis::getTextScale() const { return m_textScale; }
void CAxis::setAxisLimits(
        float xmin, float ymin, float zmin, float xmax, float ymax, float zmax)
{
        m_xmin = xmin;
        m_ymin = ymin;
        m_zmin = zmin;
        m_xmax = xmax;
        m_ymax = ymax;
        m_zmax = zmax;
        CRenderizableDisplayList::notifyChange();
}
void CAxis::setTextLabelOrientation(
        int axis, float yaw_deg, float pitch_deg, float roll_deg)
{
        ASSERT_(axis >= 0 && axis < 3);
        m_textRot[axis][0] = yaw_deg;
        m_textRot[axis][1] = pitch_deg;
        m_textRot[axis][2] = roll_deg;
}
void CAxis::getTextLabelOrientation(
        int axis, float& yaw_deg, float& pitch_deg, float& roll_deg) const
{
        ASSERT_(axis >= 0 && axis < 3);
        yaw_deg = m_textRot[axis][0];
        pitch_deg = m_textRot[axis][1];
        roll_deg = m_textRot[axis][2];
}